一种基于改进EM算法的局部路径规划方法、介质及设备.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113654556A(43)申请公布日2021.11.16(21)申请号202110756851.9(22)申请日2021.07.05(71)申请人的卢技术有限公司地址210001江苏省南京市栖霞区汇智科技园A区A1栋11楼(72)发明人王璐(74)专利代理机构南京苏高专利商标事务所(普通合伙)32204代理人柏尚春(51)Int.Cl.G01C21/20(2006.01)G01C21/34(2006.01)权利要求书2页说明书6页附图2页(54)发明名称一种基于改进EM算法的局部路径规划方法、介质及设备(57)摘要本发明公开了一种基于改进EM算法的局部路径规划方法、介质及设备，所述方法包括：输入参考线、障碍物及其预测轨迹、车辆状态；根据障碍物生成Voronoi图并进行点采样；根据采样点生成路径并计算代价；选取代价最小的路径作为最优路径；根据参考线和车辆运动学约束获取车辆速度上限；根据车辆和障碍物生成速度并计算代价；选取代价最小的速度作为最优速度；根据约束条件和优化目标进行速度优化；将路径和速度进行合成，生成轨迹。本发明是根据障碍物之间的空间分布情况进行路径搜索，进行点采样时减少无效采样点，可以提高效率；本发明用尽可能远离障碍物的路径表示弧，如果控制过程产生位置误差，车辆也不会碰到障碍物，可以保证无人驾驶车辆安全行驶。CN113654556ACN113654556A权利要求书1/2页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)中所述约束条件具体为：2CN113654556A权利要求书2/2页起始点函数值约束f(0)＝0；等式约束；起始点速度约束f'(0)＝v0＝初始速度；等式约束；单调性约束，即随时间t增大，s＝f(t)一定是不变或增大(单调递增)，因为车会停下或前进，不后退；不等式约束；分界点处各阶导数连续平滑性约束，这里用了0～3阶导数；等式约束；采样时刻t对应的自车s坐标范围约束；不等式约束；采样时刻t对