基于改进OpenPlanner 算法的移动机器人局部路径规划

张志伟， 马小平， 白亚腾， 雷震亚， 李佳明

（1. 中国矿业大学 信息与控制工程学院，江苏 徐州 221000；2. 中国中煤能源集团有限公司，北京 100083）

0 引言

煤矿作业环境复杂多变，存在各种危险因素，传统人工作业效率低且安全难以保障。移动机器人能够自动规划路径和执行[1]，可有效代替部分人工作业。

路径规划是移动机器人自主导航的核心，也是技术难点[2]。路径规划主要指机器人基于环境信息和全局位姿确定从一个地点到另一个地点的最佳路径或轨迹，以实现特定的任务或目标。具体可分为全局路径规划和局部路径规划[3-4]。全局路径规划按照相关评价指标规划出始末点间的最优路径[5]，该过程通常不考虑障碍物的具体位置和动态变化，而是在地图或环境模型的基础上计划一个整体路径，它描述了移动机器人应该遵循的大致方向和路线。局部路径规划依赖搭载的传感器实时获取的环境信息规划出可供执行的实时路径，它关注如何绕开障碍物、避免碰撞、调整速度和方向，以保证移动机器人在复杂环境中安全、高效地移动。现有的全局路径规划算法已较为成熟，在大部分场景下求解速度快且鲁棒性较好；而针对煤矿复杂环境，要保证移动机器人避开障碍物，局部路径规划算法就显得更加重要。常用的局部路径规划算法有人工势场法、动态窗口法（Dynamic Window Approach，DWA）和人工神经网络法等。人工势场法将场景中的障碍物和目标点分别看作对移动机器人有斥力和引力的物体，移动机器人沿着斥力和引力的合力避开障碍物，到达终点[6-8]。如S. M. H. Rostami[9]等通过改进成本函数使得机器人避开障碍物，而不会停在局部最优点；J. Batista[10]等通过遗传算法对人工势场参数进行动态优化调整，实现碰撞避免和更高的目标精度。DWA 在速度空间采集多组线速度与角速度的交叉组合数据，模拟并评估未来的运动路径，对比生成路径与参考路径贴合程度，剔除有障碍物路径，最终采用最优路径作为规划结果[11-14]。如王永雄等[15]提出了稠密场景内的自适应DWA，可根据障碍物稠密程度自动调整目标函数权值；魏立新等[16]对DWA 进行改进并与蚁群算法融合，提高了DWA 响应时间。人工神经网络是一种自适应非线性动力学系统[17]，由大量神经元互相连接组成。如M. Duguleana 等[18]使用Q-learning 算法和人工神经网络解决不确定工作空间内的路径规划问题；Xue Yang[19]使用非主导排序遗传算法解决静态环境下的多智能体路径规划问题；S. Josef 等[20]在强化学习网络中加入自注意模块，以增强对局部环境中重要信息的感知，实现了在离散障碍物和连续地形的路径规划；F. Leiva 等[21]使用深度确定性策略梯度算法，依据传感器获取一个可变的2D 点云，实现在无图情况下的路径规划。

然而，上述局部路径规划算法只实现了移动机器人在场景内自由移动，但局部路径生成并未考虑场景内道路限制，对于一些规则化的结构道路并不适用。OpenPlanner 算法很好地解决了该问题，该算法以全局路径（即行车道中心线）为参考线，在道路内横纵向均匀采样生成局部路径簇，使得移动机器人可沿结构道路移动。但传统OpenPlanner 算法未考虑移动机器人最大转向曲率约束，生成的局部路径无法被移动机器人所跟踪。因此，本文基于传统OpenPlanner 算法，提出了改进OpenPlanner 算法，并用于移动机器人局部路径规划。采用双层局部路径簇，同时在评价函数中引入曲率代价，不仅可扩大解空间，还使得优化过程筛选出的最优局部路径能够满足移动机器人最大转向曲率约束，从而使移动机器人能平顺、安全地跟踪局部路径。

1 传统OpenPlanner 算法

OpenPlanner 算法包含用于全局路径规划、任务规划和局部路径规划的开源自动驾驶导航框架[22]。其中局部路径规划的基本思想：首先分别根据横向采样距离和纵向采样距离进行状态采样，生成初始局部路径簇，然后基于梯度下降方法完成路径簇的平滑处理，最后综合考虑参考线代价、过渡代价和障碍物代价筛选出最优的局部路径，重复迭代以上过程，直到移动机器人行驶至目标位置。

1.1 状态采样

OpenPlanner 算法以全局路径为参考线，接收移动机器人当前位姿及局部障碍物信息，实时生成局部路径簇并筛选出最优的局部路径。局部路径簇是OpenPlanner 算法的关键，其分段结构如图1 所示，主要由平滑段、入段和出段构成。平滑段为移动机器人后轴中心至平滑采样点的线段，其长度决定了局部路径转向的平滑性，各条局部路径的平滑段重合。入段为连接平滑段末端和各状态采样点的线段，状态采样点位置由入段横向采样距离和入段纵向采样距离决定。出段为各状态采样点与最远局部规划点的连线，可通过全局参考路径的平移得到。移动机器人沿参考线的状态采样过程如图2 所示，其中点M 为移动机器人后轴中心，点N 为路径簇平滑段和入段的衔接点，点A—E 分别为局部路径簇入段的末状态采样点，点A'—E'分别为路径簇出段的末状态采样点，这些点共同决定了路径簇的具体构型。

图1 局部路径簇分段结构Fig. 1 Sections of local path cluster

图2 沿参考线的状态采样过程Fig. 2 State sampling process along reference line

1.2 局部路径簇生成

由于状态采样后的初始局部路径簇在分段连接点处存在较大转折，所以需进行路径平滑处理。OpenPlanner 算法将路径平滑转换为无约束的优化问题，采用梯度下降方法求解平滑后的局部路径簇。该问题的优化变量为第k（k=0，1，···，n-1，n为路径点数量）个路径点坐标（xk，yk），代价函数为式中：csmooth为平滑度代价，促使局部路径趋向于直线，路径越平滑，csmooth越小；clength为长度代价，保证各路径点间距分布均匀，各路径点间距分布越均匀，clength越小；cdeviation为偏移代价，确保优化后路径基本维持原路径的几何形状，对应点之间距离越小，与原路径偏移越小；Pk为第k个路径点的笛卡尔坐标；Pk_ref为平滑后第k个路径点的笛卡尔坐标。

1.3 最优局部路径筛选

算法运行时，通过状态采样和梯度下降生成光滑的局部路径簇，并基于包含参考线代价、过渡代价和障碍物代价的评价函数完成最优路径筛选。局部路径的评价函数为

式中：i为局部路径索引；σ为评价系数；ω1，ω2，ω3分别为参考线代价ccenter（i）、过渡代价ctransition（i）和障碍物代价cobstacle（i）的权重系数。

评价函数的各加权项含义如下。

1） 参考线代价ccenter（i）：度量单个模拟周期内各条局部路径出段偏离行车道参考线距离的代价。

式中：z为单个模拟周期内局部路径条数；ldis_resolution为相邻局部路径出段间距。

参考线代价计算原理如图3 所示。

图3 参考线代价计算原理Fig. 3 Calculation principle of reference line cost

2） 过渡代价ctransition（i）：度量单个模拟周期内移动机器人当前位姿与各条局部路径出段距离的代价。该项用于限制最优路径的频繁更换，确保移动机器人平稳地转向。

式中q为与移动机器人距离最近的局部路径索引。过渡代价计算原理如图4 所示。

图4 过渡代价计算原理Fig. 4 Calculation principle of transition cost

3） 障碍物代价cobstacle（i）：度量单个模拟周期内各条局部路径与障碍物接近程度的代价。障碍物代价的计算过程分为2 个阶段：第1 阶段计算各条局部路径平滑段和入段的碰撞情况，如图5（a）所示，如果在以O为圆心的圆形碰撞检测区域内存在障碍物轮廓点，则该条路径被剔除；第2 阶段计算在第1 阶段中未发生碰撞的局部路径出段的障碍物代价，该代价包括障碍物横向代价cobstacle_longitude（i）和纵向代价cobstacle_lateral（i），含有m个障碍物轮廓点的代价计算公式为

图5 障碍物代价计算原理Fig. 5 Calculation principle of obstacle cost

式中：lj为移动机器人位置和第j（j=0，1，···，m-1）个障碍物轮廓点位置在路径上的投影点距离；dj为第j个障碍物轮廓点到参考路径的垂直距离。

2 改进OpenPlanner 算法

传统OpenPlanner 算法的状态采样阶段忽略了移动机器人实际运行状态的影响，同时未考虑移动机器人最大转向曲率约束，可能产生不满足最大转向曲率的局部路径。因此，本文从状态采样和评价函数2 个方面对传统OpenPlanner 算法进行改进。

2.1 状态采样改进

为扩大最优局部路径的解空间， 改进OpenPlanner 算法采用双层局部路径簇设计：次层局部路径簇的平滑段与首层局部路径簇的平滑段重合；次层局部路径簇的入段纵向采样距离为首层局部路径簇的1.5 倍，入段横向采样距离保持不变；根据最远局部路径规划距离平移参考线得到次层局部路径簇的出段。改进OpenPlanner 算法的局部路径簇分段结构如图6 所示。

图6 改进OpenPlanner 算法局部路径簇分段结构Fig. 6 Sections of local path cluster of improved OpenPlanner algorithm

改进OpenPlanner 算法在实际运行时，首层局部路径簇入段纵向采样距离llon与行驶速度v呈分段线性关系：

式中：llon_min为最小纵向采样距离；vmin为最小速度；a为速度影响因子；vmax为最大速度；llon_max为最大纵向采样距离。

2.2 评价函数改进

曲率代价能确保最优的局部路径满足移动机器人最大转向曲率约束，同时曲率代价越小，移动机器人行驶越平顺、安全，因此将曲率代价引入评价函数，改进后局部路径的评价函数为

式中：ccurve（i）为局部路径的曲率代价，由该条局部路径上各采样点曲率之和得到；ω4为曲率代价ccurve（i）的权重系数。

式中：κλ为局部路径上采样点曲率；λ为采样点索引（λ=1，2，···，t-2，t为局部路径上采样点数量）。

局部路径上采样点曲率κλ可由该点坐标（xb，yb）与左右相邻2 个点坐标（xa，ya），（xc，yc）通过外接圆求曲率法得到：

3 实验与分析

为验证改进OpenPlanner 算法的可行性和有效性，在Gazebo 仿真环境下搭建了有障碍物和无障碍物2 种场景， 对传统OpenPlanner 算法和改进OpenPlanner 算法进行对比实验。在2 种场景下设置相同起点，与障碍物距离相同，局部规划路径簇长度为15 m。传统OpenPlanner 算法的评价函数权重系数ω1=2.5，ω2=1.0，ω3=1.0，改进OpenPlanner 算法的评价函数权重系数ω1=2.5，ω2=0.5，ω3=1.0，ω4=0.5。算法运行环境为Ubuntu18.04 操作系统，处理器为Intel（R） Core（TM）i5-10400F，主频为2.9 GHz，内存为8 GiB。

传统OpenPlanner 算法和改进OpenPlanner 算法在有障碍物、无障碍物场景下的局部路径规划仿真结果分别如图7、图8 所示。可看出2 种算法均可满足避障需求，但改进OpenPlanner 算法筛选的局部路径转向更加平缓。

图7 有障碍物场景下局部路径规划仿真结果Fig. 7 Simulation results of local path planning in scenario with obstacle

图8 无障碍物场景下局部路径规划仿真结果Fig. 8 Simulation results of local path planning in scenario without obstacle

传统OpenPlanner 算法和改进OpenPlanner 算法在不同场景下多次仿真实验的平均数据见表1。可看出改进OpenPlanner 算法虽然在无障碍物和有障碍物场景下规划耗时较传统OpenPlanner 算法分别增加了30.9%和36.2%，但算法单次规划耗时仍小于1 ms，实时性能够满足移动机器人的要求；在无障碍物场景下，传统OpenPlanner 算法规划出的局部路径最大曲率为0.280 8 m-1，超过了移动机器人最大转向曲率0.219 3 m-1，导致局部路径无法被移动机器人所跟踪； 与传统OpenPlanner 算法相比， 改进OpenPlanner 算法生成的局部路径平均曲率更小，无障碍物场景下局部路径的平均曲率下降了31.3%，有障碍物场景下局部路径的平均曲率下降了6.2%，更有利于移动机器人平顺、安全地行驶。

表1 OpenPlanner 算法改进前后实验结果Table 1 Experimental results before and after improvement of OpenPlanner algorithm

在仿真环境中采用纯追踪控制算法，对传统OpenPlanner 算法和改进OpenPlanner 算法分别进行局部路径跟踪实验，对比结果如图9 所示。可看出传统OpenPlanner 算法规划的局部路径和移动机器人实际行驶路径无法重合，这是由于规划的局部路径存在部分路径曲率超过最大转向曲率限制，使得大曲率路径段无法被跟踪，并在后续跟踪中造成累计误差；而改进OpenPlanner 算法考虑了局部路径曲率代价，规划的局部路径曲率并未超过最大转向曲率约束，使得移动机器人能完全跟踪改进OpenPlanner算法规划的局部路径。

图9 局部路径跟踪对比Fig. 9 Comparison of local path tracking

为验证改进OpenPlanner 算法的实际效果，搭建了搭载激光雷达的双轮履带实验车进行实物实验。以行人为动态障碍物，在距离行人3 m 处开始避障规划，如图10 所示。实验结果表明，移动机器人能够准确发现行人且计算出左侧不满足通行要求，然后快速执行右转的避让操作，整个跟踪过程移动机器人动作执行平顺。

图10 改进OpenPlanner 算法实物实验Fig. 10 Physical experiment of improved OpenPlanner algorithm

4 结论

1） 为扩大最优局部路径解空间，设计了双层局部路径簇，其中首层局部路径簇入段纵向采样距离与行驶速度呈分段线性变化，次层局部路径簇入段纵向采样距离为首层局部路径簇的1.5 倍。

2） 在评价函数中引入曲率代价，既确保了最优路径满足移动机器人的最大转向曲率约束，又降低了局部路径的平均曲率。

3） 在无障碍物和有障碍物场景下，改进OpenPlanner 算法筛选的最优局部路径平均曲率较传统OpenPlanner 算法分别下降了31.3%和6.2%，局部路径能够被移动机器人较好地跟踪。