自动驾驶路径规划-Voronoi Planner

图片来源: https://www.youtube.com/watch?v=7eCrHAv6sYY

假设种子节点(Seed Points)序列为:\{x_1, x_2,..., x_n | x_i \in R^d, i = 1, 2,...,n\} ，R^d 表示所有种子节点(Seed Points)为d维空间的坐标点，这n个种子节点将d维空间切分为n个Cell，每个Cell的数学定义如下：V_i=\{x \in R_d | \forall j \neq i, d(x, x_i) \leq d(x, x_j) \} 每个Cell中包含的都是距离当前Cell距离最近的所有点，因此Cell的边界就是距离种子点(Seed Points)最远的点的集合。利用Voronoi Diagram的这个特性，将障碍物的边界当做种子点(Seed Points)，那么Cell的边界就是远离所有障碍物的可行驶路径。

Voronoi Planner最大化的利用了障碍物之间的空隙，确保生成的路径是最大程度远离所有障碍物的安全行驶路径。

图片来源：https://natanaso.github.io/ece276b2018/ref/ECE276B_5_ConfigurationSpace.pdf

1、使用Voronoi Diagram进行路径规划

下图是一所大学校园的地图，地图中包含各种多变形的障碍物，我们可以使用Voronoi Planner实现在地图中查找一条安全路径，最大程度的避开障碍物。

the northern half of Columbia's Morningside Campus.图片来源：https://www.cs.columbia.edu/~pblaer/projects/path_planner/

为了实现Voronoi路径规划，首先用一系列的离散点集序列组成的小线段模拟逼近多边形障碍物的每个边。

The points that approximate the polygonal obstacles.图片来源：https://www.cs.columbia.edu/~pblaer/projects/path_planner/

然后使用这些近似的离散点作为输入，使用Voronoi构造算法构造Voronoi Diagram。

The points that approximate the polygonal obstacles.图片来源：https://www.cs.columbia.edu/~pblaer/projects/path_planner/

Voronoi diagram构造完成之后，消除顶点包含在障碍物或者与障碍物相交的Voronoi Edge，剩下的Voronoi Edge就构成了避开所有障碍物的可行驶路径集合。

The points that approximate the polygonal obstacles.图片来源：https://www.cs.columbia.edu/~pblaer/projects/path_planner/

最后，将Voronoi Edge转化为Grahp结构，将机器人的起点位置和终点位置关联到最近的Voronoi Edge，然后通过图搜索算法(Dijkstra等)就可以生成一条从起点到终点的安全行驶路线。

2、Voronio Planner VS Sample Planner

从下图的对比可以看出，Voronoi Planner规划的路径的特点是尽可能的远离障碍物。

图片来源:Local and Global Motion Planning for Unmanned Surface Vehicle

图片来源:Local and Global Motion Planning for Unmanned Surface Vehicle

3、梯度下降的路径平滑算法

同基于采样的运动规划生成的曲线一样，Voronio Planner生成的曲线都是不平滑的折线，所以需要对路径进行平滑操作，平滑的方法也比较多，今天先介绍其中一种。

3.1 问题定义

如下图所示，s表示运动规划的起点，e表示运动规划终点，斜线填充的网格表示障碍物位置，蓝色的线为运动规划算法(RRT、Voronoi etc.)规划出的路线,曲折不平；红色为平滑后的运动曲线，对车辆的实际行驶比较友好。

假设运动规划的结果点序列为:[x_1,x_2,…,x_n]

平滑后的运动规划的点序列:[y_1,y_2,…, y_n]

我们可以定义如下的平滑Cost函数:Cost = c_1 ||x_i - y_i|| + c_2 ||y_i - y_{i+1}|| 其中第一项用于衡量平滑后的点偏离原始点的程度；第二项用于衡量平滑点之间的距离；这两个Cost项相互制衡，平滑的过程就是最小化Cost的过程。其中c_1 与c_2 是对目标路线平滑程度的参数，c_1 相对于 c_2 越大，平滑后的点就越接近于原始点，反之，路线就越平滑。对Cost求解最优解的方法采用梯度下降法(gradient descent)，即通过多次迭代，调整y_i 使得Cost Function取得最小值。

1. 起始值: y_i = x_i ,其中i=[1,…n]
2. 迭代：遍历除起点和终点外的所有点，更新y_i

循环执行迭代过程直到达到迭代次数上限或者Cost Function梯度下降至指定阈值。

3.2 算法实现

上图代码一个5x5的网格地图，红色圆圈代表一条从(0,0)到(4,4)的规划路线，下Python面代码演示了如何由这条路线生成一条平滑路线。

from math import *

path = [[0, 0],
        [0, 1],
        [0, 2],
        [1, 2],
        [2, 2],
        [3, 2],
        [4, 2],
        [4, 3],
        [4, 4]]

def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001):
    # Make a deep copy of path into newpath
    newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))]
    for i in range(len(path)):
        for j in range(len(path[0])):
            newpath[i][j] = path[i][j]
            
    change = tolerance
    while change >= tolerance:
        change = 0
        for i in range(1,len(path) - 1):
            for j in range(len(path[0])):
                d1 = weight_data * (path[i][j] - newpath[i][j])
                d2 = weight_smooth * (newpath[i-1][j] + newpath[i+1][j] - 2 * newpath[i][j])
                change += abs(d1 + d2)
                newpath[i][j] += d1 + d2
    return newpath

newpath = smooth(path)

for i in range(len(path)):
    print('['+ ', '.join('%.3f'%x for x in path[i]) +']> ['+', '.join('%.3f'%x for x in newpath[i]) +']')

平滑后的路径输出结果如下:

[0.000, 0.000]> [0.000, 0.000]
[0.000, 1.000]> [0.021, 0.979]
[0.000, 2.000]> [0.149, 1.851]
[1.000, 2.000]> [1.021, 1.979]
[2.000, 2.000]> [2.000, 2.000]
[3.000, 2.000]> [2.979, 2.021]
[4.000, 2.000]> [3.851, 2.149]
[4.000, 3.000]> [3.979, 3.021]
[4.000, 4.000]> [4.000, 4.000]

平滑算法的实际应用效果如下：

图片来源:Local and Global Motion Planning for Unmanned Surface Vehicle

相关代码

1、Boost Voronio Diagram。(https://www.boost.org/doc/libs/1_60_0/libs/polygon/doc/voronoi_diagram.htm)

2、Scipy Spatial Voronoi(https://docs.scipy.org/doc/scipy-0.18.1/reference/generated/scipy.spatial.Voronoi.html)

3、Voronoi Planner的代码实现可以参考:

https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/VoronoiRoadMap/voronoi_road_map.py

参考链接

1、Boost Voronio Diagram。(https://www.boost.org/doc/libs/1_60_0/libs/polygon/doc/voronoi_diagram.htm)

2、Robot Path Planning Using Generalized Voronoi Diagrams(https://www.cs.columbia.edu/~pblaer/projects/path_planner/)

3、Local and Global Motion Planning for Unmanned Surface Vehicle，Roman Fedorenko, Boris Gurenko