混合A* 中基于 Voronoi 势场的路径代价和 Voronoi 势场的实现测试

参考

Practical Search Techniques in Path Planning for Autonomous Driving 混合 A* 论文

Sensor-Based Exploration: The Hierarchical Generalized Voronoi Graph Voronoi 图论文

认识 Voronoi ，泰森多边形 voronoi 介绍和应用

Voronoi Field 和 Voronoi Diagram

沃罗诺伊图 Voronoi Diagram 又称作狄利克雷镶嵌或者泰森多边形。是一种离散区域分割方法。

离散性：将空间划分为离散的区域，每个区域与一个生成点相关联。
连续性：Voronoi边界是由相邻的生成点之间的距离确定的连续线段。
局部性：每个点的Voronoi区域仅由其最近的邻近点决定，与其他点的位置无关。

沃罗诺伊场 Voronoi Field 是一个连续的函数场，用于描述空间中的距离信息。

连续性：Voronoi field 是一个连续的函数场，而不是离散的区域划分。
定义：它基于一组特定点，为每个点定义一个场值，表示该点到最近的邻近点的距离。
插值：Voronoi field 可以通过插值方法（如线性插值或高斯过程）计算出在整个空间中的值。

混合 A* 中的 Voronoi 势场代价

用 voronoi 势场中路径点的势场值来描述路径与障碍物接近程度之间。

voronoi 势场定义如下：

$\begin{cases} \rho_V(x,y)=(\frac{\alpha}{\alpha+d_{O}(x,y)})(\frac{d_{V}(x,y)}{d_{O}(x,y)+d_V(x,y)}) \frac{(d_O-d_O^{max})^2}{(d_O^{max})^2}, \quad &d_O\le d^{max}_O \\ \rho_V(x,y)=0, &otherwise \end{cases}$

$d_O(x,y)$ 是 (x,y) 到最近障碍物的距离， $d_V(x,y)$ 是 (x,y) 到所在 Generalized Voronoi Diagram (GVD) 区域的边的最近距离。

$\alpha>0$ 是控制势场衰减率常数， $d_O>0$ 是控制势场最大有效范围常数， $d_O^{max}>0$ 是地图中距离障碍物最远的距离。

Voronoi 场性质

$\rho_V(x,y)=0,\quad d_O\ge d^{max}_O$ ；
$\rho_V(x,y)\in[0,1]$ 在 (x,y) 上连续，因为 $d_O(x,y)$ 和 $d_V(x,y)$ 不会同时为 0；
$\rho_V(x,y)$ 只有在障碍物内才会达到最大值；
$\rho_V(x,y)$ 只有在 GVD 边上才为 0；

Voronoi 场优势

Voronoi 场的主要优势在于，场值与可导航空间（自由空间）成比例关系，即使是狭窄的开口，由于比例缩放，依然存在低势场区域（GVD 的边）。

图 fig.3 中，(a) 显示 Voronoi 势场，红色是障碍物，灰色部分是表示势场强度，颜色越黑，说明此处势场越强，距离障碍物越近。(b) 显示对应的 Voronoi 图，其中色块的边界就是势场为 0 的空间，在 (b) 中可以看到永远存在一条势场值为 0 的通道！(c) 表示在标准势场中，即使是狭窄通道仍具有高势能区域。

！！！注意！！！

图 3(c) 在原文中称为 standard potential field 标准势场，什么是标准势场？没有找到好的解释。在标准势场中，狭窄通道中仍有高势能区域。这说明在“标准势场”中，自由区间是高势能区域，障碍物是低势能区域。而在 costmap 中，自由空间是低数值区域，障碍物是高数值区域。

图 3(c) 中高势能区域是浅色，低势能区域是深色，这与 rgd 图一致，说明原文的标准势场图是用 rgb 图画的。

Voronoi 势场反映地图障碍物的相对距离，即使在狭窄区域也不会完全封闭。也就是说 Voronoi 势场图中，在非完全封闭区域，始终存在一条高势能通道。Voronoi 势场图可以用来找地图中离障碍物最远的通道。

Voronoi 图可以导出自由空间骨架，但对于运动学非完整性车辆或机器而言，这样的路径是不可直接使用。