[发明专利]一种基于改进EM算法的局部路径规划方法、介质及设备

权利要求书

1.一种基于改进EM算法的局部路径规划方法，其特征在于，包括以下步骤：

(1)输入参考线、障碍物检测结果、障碍物轨迹预测结果及车辆状态；

(2)根据障碍物生成Voronoi图并进行点采样；

(3)根据所有采样点生成路径并进行代价计算；

(4)选取代价最小的路径作为最优路径；

(5)根据参考线及车辆运动学约束获取速度上限；

(6)根据车辆速度上限及障碍物生成速度并进行代价计算；

(7)选取代价最小的速度作为最优速度；

(8)根据约束条件和优化目标进行速度优化；

(9)将路径和速度进行合成，生成轨迹。

2.根据权利要求1所述的方法，其特征在于，所述步骤(2)具体为：

(2.1)根据障碍物生成生成Voronoi图；

(2.2)对步骤(2.1)中的Voronoi图进行点采样。

3.根据权利要求2所述的方法，其特征在于，所述步骤(2.1)具体为：

(2.1.1)通过将障碍物边界形状分割成许多小线段，可将障碍物边界近似为大量点，这样就获取了表示多边形障碍物的一组离散点；

(2.1.2)对得到的离散点进行Delaunay三角剖分，采用增量法，逐个添加顶点到图中，并采用Quad-Edge表示三角形的边；

(2.1.3)对得到的Delaunay三角剖分中的每个三角形的每条边计算通道宽度；

(2.1.4)生成Voronoi图。

4.根据权利要求2所述的方法，其特征在于，所述步骤(2.2)具体为：

(2.2.1)纵向点采样根据车辆速度和最小采样距离取点；

(2.2.2)横向点采样根据Voronoi边的通道宽度，取通道宽度的中心点作为横向采样点。

5.根据权利要求3所述的方法，其特征在于，所述步骤(2.1.3)具体为：

(2.1.3.1)获取与该边相关联的左边三角形的所有的顶点，计算该三角形的外接圆的圆心，作为Voronoi边的起始顶点；

(2.1.3.2)获取与该边相关联的右边三角形的所有顶点，计算该三角形的外接圆的圆心，作为Voronoi边的终止顶；

(2.1.3.3)假设该Voronoi边的起始顶点和终止顶点位于障碍物内部，就删除该Voronoi边，并继续搜索Delaunay三角剖分中的其他边；否则转入步骤④，判断Voronoi边表示的通道宽度；

(2.1.3.4)选取右边的三角形，在右三角形中，如果三个点位于不同的障碍物的边上，则取三角形的第一个点和第二个点的连线长度Wr作为通道宽度，否则转入步骤(2.1.3.5)；

(2.1.3.5)在右三角形中，如果有两个点位于同一个障碍物的边上，则计算三角形中不在同一个障碍物的点到对边的垂直距离，取该距离为通道宽度Wr；

(2.1.3.6)左边的三角形按照右边的三角形执行相同的操作，得到通道宽度Wl，取W＝min(Wl,Wr)作为该Voronoi边表示的通道的宽度。

6.根据权利要求1所述的方法，其特征在于，步骤(8)中所述约束条件具体为：

起始点函数值约束f(0)＝0；等式约束；

起始点速度约束f'(0)＝v0＝初始速度；等式约束；

单调性约束，即随时间t增大，s＝f(t)一定是不变或增大(单调递增)，因为车会停下或前进，不后退；不等式约束；

分界点处各阶导数连续平滑性约束，这里用了0～3阶导数；等式约束；

采样时刻t对应的自车s坐标范围约束；不等式约束；

采样时刻t对应的自车速度范围约束；不等式约束；

采样时刻t对应的自车加速度范围约束；不等式约束。

7.根据权利要求1所述的方法，其特征在于，步骤(8)中所述优化目标包括加速度、加加速度、参考线参考速度、决策速度、路径参考速度、正则项。

8.根据权利要求1所述的方法，其特征在于，步骤(8)中采用的优化方法为qpOASES库优化。