基于Dijkstra算法的封闭环境全局路径规划.pdf

10.16638/ki.1671-7988.2023.016.002 10.16638/ki.1671-7988.2023.016.002 基于 Dijkstra 算法的封闭环境全局路径规划 郑 好，冯虢靓雯*，蒲文杰，徐马长啸（陕西法士特汽车传动集团有限责任公司 智能传动研究院，陕西 西安 710119）摘要：随着自动驾驶技术的快速发展，其关键技术被划分为感知、决策、规划及控制四大模块，路径规划在其中承担着重要角色。良好的路径规划算法能够在行车前规划出一条最优路径，并在行车过程中能够结合感知、决策传送回来的环境信息，实时规划出微调后的局部路径，在完成避障、换道等功能的基础上，兼顾着行驶轨迹的安全性、舒适性。文章从全局路径规划出发，基于 Dijkstra 最短路径规划算法，构建封闭环境下拓扑节点地图，改进算法中不适用自动驾驶实车测试中的痛点情况，以实现任意起始点至终点的全局路径规划任务。最终在实车试验中进行验证，试验结果表明，在封闭且闭环的环境下，自动驾驶汽车可规划出理想的最短路径进行行驶。关键词：自动驾驶；全局路径规划；迪杰斯特拉算法；封闭环境建模 中图分类号：U463.6 文献标识码：A 文章编号：1671-7988(2023)16-07-05 Global Path Planning in Closed Environment Based on Dijkstra Algorithm ZHENG Hao,FENG Guoliangwen*,PU Wenjie,XU Machangxiao(Intelligent Transmission Research Institute,Shaanxi Fast Auto Drive Refco Group Company Limited,Xian 710119,China)Abstract:With the rapid development of autonomous driving technology,its key technologies can be divided into four modules:perception,decision,planning and control.Path planning plays an important role in them.A good path planning algorithm can plan an optimal path before automatic driving.In the process of driving,it can combine the environmental information sent back by perception and decision,and then plan a fine-tuned local path in real time.On the basis of completing functions such as obstacle avoidance and lane changing,the planning takes into account the safety and comfort of the driving trajectory.Based on Dijsktra shortest path planning algorithm,this paper starts with the discussion of global path planning and constructs a topological node map in a closed environment.Then improve the algorithm,which not applicable to the pain points in the real environment test of autonomous driving,so as to realize the global path planning task from any starting point to the end point.Finally,it is verified in the real vehicle test.The test results show that 作者简介：郑好（1996），男，硕士，助理工程师，研究方向为智能驾驶车辆，E-mail:。通信作者：冯虢靓雯（1996），女，硕士，助理工程师，研究方向为智能驾驶决策算法，E-mail:。8 汽 车 实 用 技 术 2023 年 in a close and closed-loop environment,the self-driving vehicle can plan the ideal shortest path for driving.Keywords:Autonomous driving;Global path planning;Dijkstra algorithm;Closed environment construction 在自动驾驶领域中，路径规划处于控制环节之前，可计算横向与纵向控制量，同时为感知环境提供辅助先验信息，使用地图信息提前得到前方道路信息，为决策提供支持。因而良好、稳定的路径规划算法在自动驾驶车辆是不可或缺的一环。路径规划实时读取组合惯导信息，依据相应规则与算法，为车辆提供通往目的地的最优或最短路径。国内外已有部分研究针对机器人或自动驾驶领域的全局路径规划算法研究，如对图搜索中的 Dijkstra 算法与 A*算法进行比较，并应用于汽车导航路径中1，或是将二者进行结合，在多个车辆同时运行的动态环境中完成机器人避障任务2。本文主要针对路径规划中的全局规划算法，对所用 Dijkstra 算法进行简要介绍，对 Dijkstra 基础算法进行改进，使其适用于自动驾驶车辆，并结合法士特试车场相应结构，设计适用于该算法的拓扑结构图，完成车辆从当前点至任一点的路径规划任务。1 路径规划及 Dijkstra 算法简介 在无人驾驶领域中根据任务要求可分为全局路径规划与局部路径规划两大模块。其中全局规划需要已知地图信息或障碍物信息，在车辆行驶前生成起点至任意终点的且能避开障碍物的最优路径规划，全局路径规划属于静态规划，主要方法可分为基于图论、基于采样等的传统算法，以及采用神经网路或是基于生物仿真等的人工智能算法3；局部规划考虑的是在车辆行驶过程中，根据车身安装的环境感知传感器实时返回的数据，从当前所在位置开始，规划出前方一段最优行驶路径，完成避障、超车等功能，属于动态规划，局部规划算法通常包含动态窗口算法（Dynamic Window Approach,DWA）、人工势场算法（Artificial Potential Field,APF）等4。通常全局规划算法运行在局部规划算法前，首先生成一条完整的行车路径完成全局规划任务，随后以一定的时间频率不断运行局部规划算法，实时根据检测到的障碍物信息并调整运行轨迹。本文改进的 Dijkstra 算法隶属于全局规划中的传统算法之一，又可称为迪杰斯特拉算法，是一种典型的最短路径算法。适用于循迹控制中，它以起始点开始向外层层扩展，逐渐遍历完所有的节点，得到起始点至每一点的最短距离，并记录到达终点的最短路径5。其算法流程可简述为 1）初始化时，使集合 S 只包含起点 s，集合U 则包含剩余其他节点，若集合 U 中某些节点与s 相邻，则该节点相应的值为距离 s 的距离，若与s 不相邻，则距离为无穷大；2）从集合 U 选择距离 s 最近的节点，作为下一次计算的点，并将其加入集合 S 中，同时从集合 U 中移除；3）当集合 U 中剩余节点通过此节点到达起点s 的距离小于存储的距离时，更新距离；4）重复步骤 2）和步骤 3），直至遍历所有节点，此时集合 U 应为空集。Dijkstra 算法适用于非负权值的有向或无向，有环亦或是无环图中6，如图 1 所示。构建一幅有向有环地图，该图包含 7 个节点，从 1001 至 1007，单向箭头表明只能从某一节点到达另一节点，而无法原路返回，箭头上方数字则为路径权重，在自动驾驶全局路径规划中，权重代表了路线长度，而节点可以为路口，延伸至不同的地点。假设起点为 1001，所设置的目标地为 1004，则搜索过程如表 1 所示。图 1 构建 Dijkstra 节点地图 表 1 中集合 S 开始只有起点 1001，1002 与其相邻且距离为 200，其余节点 1001 无法直接到达，默认距离为无穷大，此时将 1002 作为下一轮遍历的途经点，加入集合 S 中；在第二步中，以 1002作为中间节点，计算其余节点至 1001 的距离，此时 1003 可直接到达 1002 继而达到 1001，更新其第 16 期 郑 好，等：基于 Dijkstra 算法的封闭环境全局路径规划 9 距离为 500。同理 1005 距离 1001 应为 220，此时由于 1005 距离 1001 比 1003 更小，则将 1005 加入集合 S 中；在第三步中以 1005 节点出发，可更新 1006 距离为 420；第四步以 1006 节点出发，此刻与之前逻辑略有不同，由于 1001、1002、1005已在集合 S 中，故不计算到这些节点的距离，且到 1004，1007 距离仍未无穷，只能取先前计算1003 时的距离 500，并以 1003 作为中间节点进行下次循环；在第五步时可计算得到至 1004 距离520 和至 1007 的距离 550；在第六、第七步计算时，通过节点 1004 未有距离减少的情况，故最终距离不变，则可得到 1001 至其余六个节点的最短距离，同时保存至终点的路径应为 10011002 10031004。表 1 Dijkstra 路径规划过程 集合 S 节点编号 1001 1002 1003 1004 1005 1006 1007 S=1001 0 200 S=1001，1002 0 200 500 220 S=1001，1002，1005 0 200 500 220 420 S=1001，1002，1005，1006 0 200 500 220 420 S=1001，1002，1003，1005，1006 0 200 500 520 220 420 550 S=1001，1002，1003，1004，1005，1006 0 200 500 520 220 420 550 S=1001，1002，1003，1004，1005，1006，1007 0 200 500 520 220 420 550 2 法士特试车场路径规划 本文以法士特试车场为 Dijkstra 路径规划试验场地，对各路口及特殊路段路口进行编码，如图 2 所示。其中通过 1002 路口既可达到 1005 也可达到 1003，通过 1003 路口即可达到 1004 也可 达到 1007。同时为了使自动驾驶车辆能够安装预定轨迹在路段内安全行驶，采用卫星定位与惯性测量相结合的组合惯导预先采集被编码路口分割的各路段，解码惯导原始输出数据，仅当其处于载波相位差分工作状态时以一定时间间距或固定距离记录车辆行驶过程中的经纬度数据及偏航角，以保证后续循迹时的路径精度。图 2 法士特试车场地图及路口节点编码 为了方便在计算机设备上进行存储各路口节点的连接顺序，以及存储轨迹经纬度坐标等信息，设计一种新的路口信息存储格式，包括路口名称、路口编号、可抵达路口、途径路段及各途径路段长度五个信息。其中对于交叉路口，其可抵达路口可有多个，此时途径路段及路段长度也应与可抵达路口顺序相对应。同时设计一种与路口信息存储方式相对应的路段信息存储格式，包括路段名称、路段编号、邻近入口、邻近出口、路段坐标点集五种信息，其中路段坐标点集合分别包含三个数组：经度、纬度及航向角，三个数组长度应一致7。由于 Dijkstra 路径规划算法是根据路口、路口与路口间的距离生成起始点至终点的最短路径，在实际自动驾驶车辆应用中，存在以下三点不足：1）只能到达选定终点路口，不能到达路口前后的路段中任一点；2）不支持指定一个或多个必须通过的中间节点；3）若起点与终点在同一路段内，则算法将出现规划失败的情况。针对上述问题，本文基于 Dijkstra 针对路口编码所进行的路径规划，对所规划完成的路径进行修改，方法如下：1）先确定自动驾驶车辆当前所处路段的入口编码，以及目的地所在路段相对应的入口编码，调用 Dijkstra 算法函数，以起点、终点路口编码作为参数，规划出行驶路径，该路径多包含了起点路口至当前点的已行驶轨迹，应对该段路程予以删除，该路径还未包含终点路口至终点的未行使轨迹，应对于该段轨迹予以补充，以形成完整的行驶路径。10 汽 车 实 用 技 术 2023 年 2）若驾驶人本身意愿想强制通过某些节点，例如该节点可能为加油站、休息区等途经点，而Dijkstra 算法仅考虑了抵达终点的最短路径，因此当输入除起点终点外，还存在其他节点时，应以中间节点作为终点，调用一次 Dijkstra 路径规划算法，再以该中间节点为起点，下一中间节点为终点再次调用 Dijkstra 路径规划算法，遍历所有强制通过的途径点，直至循环结束，将每次规划的路径拼接起来，生成完整的行驶路径。3）当起点与终点在同一路段下时，将导致起点路口编码与终点路口编码相同，无法调用Dijkstra 算法生成路径，此时可分为以下两类情况进行解决，一类是终点位置位于起点位置后方，此时无需调用 Dijkstra 算法，直接在路段文件内根据索引生成起点至终点的路径即可；另一种时终点位于起点之前，由于车辆不可逆行的原因，配置有全局规划路径算法的自动驾驶车辆必须规划出最小回环路径，回到此路段内，此时可使用方法 2），根据路段、路口间的排列关系，找到终点路口的前一路口作为必要途经点，两次调用Dijkstra 算法完成路径规划与拼接。3 全局路径规划实车试验 为了验证 Dijkstra 算法在实际使用中的性能，设计实车试验进行验证。试验基于配备有法士特自动挡变速箱的大运商用车，如图 3 所示。采用python 语言编写 Dijkstra 算法，并针对上述问题进行程序上的补充，使用 NVIDIA Orin 平台存储程序与地图文件。图 3 验证算法所使用的大运自动挡车型 首先使用组合惯导系统完成对各分割路段的经纬度、航向角数据采集、显示。然后将车辆行驶于不同路段下启动全局路径规划程序，鼠标选择不同终点或所需要通过的中间节点，如图 4 所示。图中三角形即为鼠标点选的目标终点，加粗路线为改进 Dijkstra 算法规划出的行驶路径，检验 (a)第一目的地 (b)第二目的地 (c)第三目的地 图 4 不同目的地所规划的最短路径 第 16 期 郑 好，等：基于 Dijkstra 算法的封闭环境全局路径规划 11 所规划的路径是否与实际预期相符，例如图 4(a)中目的终点为当前起始位置的对向车道，则最短路径应通过离其最近的掉头弯道从而抵达目标路段。4 总结 本文基于 Dijkstra 算法在法士特试车场环境下，首先完成对试车场的地图采集与构建，解决了实际应用中路径不连续，不能指定途经点等使用中的问题，可实现自动驾驶车辆当前点至任一点的最短路径规划。也应当看到，本文主要针对试车场等封闭环境，节点数量有限，且全局规划运行于车辆行驶前，未考虑程序搜索效率与实时性要求，应在扩大环境的情况下继续研究改进Dijkstra 算法，不断提高其适用范围与性能。参考文献 1 王运.汽车导航路径规划算法研究J.汽车实用技 术,2017,42(21):202-204.2 ZHANG Z Y,ZHAO Z P.A Multiple Mobile Robots Path Planning Algorithm Based on A-star and Dijkstra AlgorithmJ.International Journal of Smart Home,2014,8(3):75-86.3 张广林,胡小梅,柴剑飞,等.路径规划算法及其应用综述J.现代机械,2011(5):85-90.4 朱大奇,颜明重.移动机器人路径规划技术综述J.控制与策,2010,25(7):961-967.5 邓鹏,张杭,申有吉.基于改进 Dijkstra 算法在智能导航中的应用J.新型工业化,2019,9(12):91-95.6 王树西,吴政学.改进的 Dijkstra 最短路径算法及其应用研究J.计算机科学,2012,39(5):223-228.7 董俊,黄传河.改进Dijkstra算法在GIS导航应用中最短路径搜索研究J.计算机科学,2012,39(10):245-247,257.