本文中的方法来自文章:
许松清,吴海彬,林宜,高洪张,陈天炎. 基于Voronoi图法的移动机器人路径规划[J]. 中国工程机械学报,2005,(03):336-340.
在Voronoi路径规划代码下载本文代码。
维诺图
用X表示一个距离函数为d的空间。令K为一个指示集合,(P_k ),k∈K为空间X的一个非空子集的有序元组。对应于P_k 的R_k,称为沃洛诺伊元胞,或沃洛诺伊区域,是空间X中所有到P_k 的距离不大于其到其他位置P_j (j≠k)的点集。如果定义d(x,A)=inf{d(x,a)|a∈A}为点x和子集A的距离,则
R_k={x∈X|d(x,P_k )≤d(x,P_j ) for all j≠k}
算法流程
确定Voronoi图和Voronoi子图,根据地图确定Voronoi图和地图边界内的Voronoi子图,确定起点/目标点到Voronoi子图的最近点。寻找起点到目标点的路径,实际上是在Voronoi子图内寻找两最近点之间的路径。
利用维诺图进行路径规划一般不能得到两点最短路径,仅能得到两点间“较安全”路径。
本算法中的运动体为圆形。首先的到每个障碍物的外接圆,并对外接圆进行径向扩张,扩展尺寸为运动体的半径,即可将运动体作为单点处理,只要该单点的路径不经过扩张后的圆,运动体即可无碰撞的沿路径运动。如果两个或多个扩张后的圆相交,表明运动体无法从这些障碍物之间通过,则将其相应的障碍物作为一个障碍物处理。
此时,即可将处理后的圆的圆心并以此作为Voronoi图的生成元。
生成Voronoi图后,对其进行处理,得到Voronoi图的子图,即地图边界内的部分Voronoi图。按照某种策略确定起点/目标点到Voronoi子图的最近点。
最后,使用Dijkstra算法在Voronoi子图中寻找两最近点之间的路径。
=========
首先,初始化地图数据,其中红色色块为障碍物,绿色圆圈表示圆形运动体,它在起点的位置上,红色*表示目标点。
之后,得到障碍物的外接圆,并“增长”外接圆,此时与运动体可作为单点处理。
可以看到,右下角两个障碍物“增长”后的外接圆有重叠部分,将其视为一个障碍物。
绘制维诺图,可以看到此算法的一个问题,虽然通过增长障碍物外接圆半径使运动体“可以被”视为一个质点,并且在此基础上合并了运动体无法通过的障碍物,但是voronoi图是通过外接圆圆心生成的,与外接圆半径无关,因此voronoi图的边仍可能与障碍物圆相交,仍有碰撞的可能。如下图所示。
得到维诺子图:
确定起点/目标点到维诺子图的最近点,并将其连接,通过Dijkstra算法寻找路径:
此时,可以看到此算法的另一个问题,无论起点/目标点到voronoi子图的最近点如何选择,此文中都没有起点/目标点到最近点的路径做碰撞检测,起点/目标点到voronoi图子图的路径很可能与障碍物产生碰撞,如下图所示。