根据对环境信息掌握的程度不同，可将移动机器人路径规划分为2种类型：基于环境先验信息的全局路径规划和基于实时传感器信息的局部路径规划。全局路径规划通常是基于数字地图进行的，根据周围的路网模型来选择路线[4-5]。若只进行全局路径规划，只需使移动机器人严格按照规划好的路径行驶即可实现移动机器人的平稳行驶。但移动机器人工作环境通常是动态的，并且难以获取准确的环境信息，因此只能将路径规划分为若干个子规划过程，即局部路径规划。局部路径规划依靠传感器实时探测的周围环境信息，规划出一条局部路径使移动机器人躲避障碍物[6]。
按照路径生成方式的不同可以将局部路径规划大致分为4类：基于图搜索的规划方法、基于曲线插值的规划方法、基于随机采样的规划方法以及基于群智能优化的规划方法。A*算法和Dijkstra算法是最常用的基于图搜索的路径规划方法[7]。Dijkstra算法通过搜索全局空间来求解最短路径，但难以满足快速性需求。A*算法在Dijkstra算法的基础上增加了启发式函数，在得到最短路径的同时减少了搜索空间，但A*算法和Dijkstra算法都是基于栅格八邻域搜索的，因此存在较多路径转折点，得到的路径平滑性较差，不利于机器人的实际应用[8-9]。基于曲线插值的规划方法都是通过在参考路径附近进行采样，然后利用多项式曲线或其他形式的曲线连接这些采样点，生成多条候选路径，设置评价函数对这些候选路径进行评价，从中选取一条代价最小的曲率连续、无碰撞的路径[10-12]。基于随机采样的快速扩展随机树(RRT)算法因为能快速得到一条无碰撞的路径得到了广泛的应用，但由于RRT算法是随机选择路径节点的，无法保证搜索到的路径是最优的。在基于群智能优化的规划方法中，具有代表性的有遗传算法、粒子群算法、蚁群算法等。遗传算法在解决组合优化问题时表现出很高的效率，能在合理的时间内找到最优或者次优解。文献[13]利用遗传算法同时对路径长度和路径的转折角度进行优化，优化后得到的路径长度短且平滑。但该算法只能实现全局路径平滑，无法保证局部路径连贯。
目前，大多数路径规划方法都只考虑单次规划路径的平滑性，没有考虑多次规划过程中路径的连贯性，无法保证本次规划的路径与上次规划的路径没有出现突变。虽然文献[3, 6]通过最小化当前候选路径节点与上次选择路径节点之间的距离积分来增加路径的连贯性约束，但真正对机器人平稳行驶产生影响的是离机器人最近的那一段路径的曲率，并非所有路径节点间的距离。
针对常规路径规划算法求解的路径长度非最短以及多次规划路径不连贯的问题，本文提出一种基于改进遗传算法的帧间关联平稳路径规划方法。首先通过预瞄确定局部目标点，然后在遗传算法的基础上引入删除算子来删除多余的路径节点，并在适应度函数中增加了路径连贯性约束，减小前后帧路径的突变，从而使移动机器人平稳地行驶。
1 路径规划模型 1.1 栅格环境模型 移动机器人进行路径规划前，一般将环境信息进行转换并分解为可处理的单元。栅格法是比较传统的环境建模方法，因为其容易创建和维护，得到了广泛的研究和应用。栅格图主要包括2种状态：自由栅格和障碍栅格。
本文将检测到的障碍物投影到栅格图中进行处理，如图 1所示。
 |
| 图 1 实际环境到栅格图的映射 Fig. 1 Mapping of actual environment to grid map |
| 图选项 |
设栅格图的行数为M，列数为N，选取栅格窗口的左下角栅格顶点作为坐标原点。则栅格坐标和栅格序号存在如下对应关系：
 | (1) |
式中：x为直角坐标系下的栅格的横坐标；y为直角坐标系下的栅格的纵坐标；n为栅格序号。
1.2 预瞄跟踪模型 预瞄跟踪机制是一种基于预先确定路径离散点的跟踪方法，经常应用于汽车自动驾驶领域[14-15]。本文将预瞄跟踪机制(见图 2)引入到路径规划中来确定局部目标点，也即图 1中的栅格终点。通过预瞄全局路径得到预瞄点与预瞄方向，沿着预瞄方向取得预瞄线与栅格图的交点，从而使机器人在避障的同时跟随给定路径。全局路径通过设定GPS坐标来获得。图 2中XOY表示全局坐标系，xloyl表示局部坐标系，Pf为预瞄点，Psub为局部目标点，Ps和Pg为GPS设定的全局起点和终点，Pcur为XOY坐标系下的当前点位置，v为机器人行驶速度，R为机器人的预瞄半径，β为预瞄方向和偏航方向的偏差角。
设全局路径方程为y′=f(x′)，由式(2)可以求得预瞄点坐标(x, y)：
 |
| 图 2 预瞄跟踪机制 Fig. 2 Preview tracking mechanism |
| 图选项 |
 | (2) |
式中：x′和y′为全局坐标系下的当前点坐标。由β即可求局部目标点Psub的坐标。
1.3前后帧路径规划差异模型
路径的长度和平滑性仅根据某一帧收集到的环境信息进行计算，只考虑路径的长度和平滑性因素无法保证移动机器人平稳地行驶，若当前帧规划的路径与上一帧规划的路径之间相差太远，可能会导致机器人行驶稳定性的下降甚至与障碍物发生碰撞，如图 3所示。
 |
| 图 3 当前帧和上一帧的路径差异 Fig. 3 Path difference between current frame and previous frame |
| 图选项 |
从图 3可以看出，移动机器人上一帧规划的路径在障碍物的右侧，而当前帧规划的路径在障碍物的左侧，前后帧规划路径的不连贯使机器人继续向前行驶，最后导致机器人无法避开障碍物，图 3中虚线箭头代表机器人的实际行驶方向。为了避免当前帧规划的路径和前一帧规划的路径出现显著差异，需要在当前帧进行路径规划时考虑上一帧的路径信息。
2 帧间关联平稳路径规划方法 遗传算法是一种基于生物进化机制的优化算法，通常分为生成初始种群、采用遗传算子进行遗传操作、计算个体的适应度值、迭代求得最优个体等步骤[16]。为了减少移动机器人行驶的路径长度，提高移动机器人行驶过程的平稳性，本文在常规遗传算法的基础上引入删除、插入等遗传操作算子，并且与上一帧路径规划结果进行关联。即在当前帧进行规划时考虑上一帧的路径规划信息，将上一帧路径规划结果作为参考来减少前后两帧规划路径间的差异。主要思想为：比较当前候选路径和上一帧规划路径的夹角差值，差值越大则该条候选路径的适应度值越低，被选择的概率越小。改进的遗传算法流程如图 4所示。
 |
| 图 4 改进遗传算法流程图 Fig. 4 Flowchart of improved genetic algorithm |
| 图选项 |
2.1 染色体编码 本文将每条路径表示成为一条染色体，且种群中的每个个体只有一条染色体，染色体上的基因代表了路径中的路径节点。栅格序号与栅格坐标相比，形式更加简单，更易于遗传算子操作，所以本文采用栅格序号对染色体进行编码。以P=[p1, p2, …, pn]表示为一条路径，则pi(i=1, 2, …, n)表示路径上的第i个路径节点, 如图 5所示。图中:S为路径起点，G为路径终点。
 |
| 图 5 路径编码 Fig. 5 Path coding |
| 图选项 |
2.2 种群初始化 遗传算法初始种群质量的高低对遗传算法的求解非常重要，为了满足初始种群多样性和随机性的要求，本文以相同概率采用定向和随机两种搜索方式来生成候选路径，即分别采用式(3)和式(4)来选择下一路径节点，当路径节点距离终点较近时，停止生成路径节点，保存当前路径。若生成的路径节点导致路径穿过障碍物，则重新选择路径节点。
 | (3) |
 | (4) |
式中:(xi+1，yi+1)为待选路径节点pi+1的直角坐标；(xi，yi)为当前路径节点pi的直角坐标；(xG, yG)为终点坐标；rand为0~1之间的随机数。
2.3 遗传算子 遗传操作主要采用一系列的遗传算子进行运算。由于路径是随机生成的，可能会出现路径中相邻路径节点存在断点的情况，不利于遗传操作。并且随机生成的路径存在较多路径冗余点，路径长度并非最短。因此本文新增插入算子来连接间断路径，新增删除算子来删除冗余的路径节点。
1) 选择算子。在遗传算法中，通常采用轮盘赌算法进行选择操作，但由于其随机性会引入较大的选择误差，导致后代中适应度大的个体出现次数过多，使算法陷入局部最优。因此本文采用确定式采样选择法，具体步骤如下：
步骤1 ??计算个体在下一代中的期望生存数目。
 | (5) |
式中：Mp为种群中个体数目；Ni为第i个个体的期望生存数目；fi为第i个个体的适应度值。
步骤2 ??由Ni的整数部分确定各个体在下一代中的生存数目。一共可确定下一代种群中的个体数为
步骤3 ??按照Ni的小数部分对个体进行降序排序，顺序取前
2) 交叉算子。交叉操作选用单点交叉的方式，本文选择在2条路径的相同节点处进行交叉操作，避免交叉后出现路径不连续的情况。若有多个相同路径节点，随机选择一处进行交叉，若没有相同路径节点，则不进行交叉操作。
3) 变异算子。在常规的遗传算法中通常采用随机变异的方式，随机选择一个路径节点替换原路径节点。采用随机变异的方式可能造成路径质量差，甚至路径不可行的情况。因此本文采用文献[17]所提出的变异方法，在变异点的八邻域中随机选择非障碍物点替换原有路径节点，如图 6所示。
 |
| 图 6 变异点的八邻域范围 Fig. 6 Eight neighborhoods of mutated points |
| 图选项 |
若根据设置的变异概率对每条路径进行变异操作。当变异概率较大时，计算量较大且容易破坏最优解；当变异概率较小时，不易产生更优的路径。本文在文献[17]采用的变异方法上进行改进，在变异点的八邻域中，选择替换后路径适应度值最高的路径节点。若得到的路径比原路径更优，则用变异后的路径替换原来路径，否则保持原来路径不变。由于变异操作不会产生更差的路径，因此本文仅针对每一代中保留的最优路径进行变异操作，既降低了计算量，同时也能得到更优的路径。
4) 插入算子。插入算子在路径间断处利用周围的自由栅格对其进行填补，使之成为可行的连续路径[18]。根据式(6)来判断两相邻路径点是否间断。
 | (6) |
式中:如果Δ=1，则pi和pi+1是连续的，否则视为不连续的。若不连续，采用平均值法来填补间断路径，具体计算式为
 | (7) |
式中：(x′i+1，y′i+1)为候补栅格坐标；若ni为障碍物栅格，则以ni最近的栅格进行填补。
5) 删除算子。
为了缩短染色体的长度和算法计算时间，本文新增删除算子。其主要思想是：从终点开始，依次遍历路径节点，若当前路径节点可以与起点无障碍连接，那么它们之间的中间节点就是冗余的，删除这些冗余节点并重新计算新路径的适应度。如图 7所示。
 |
| 图 7 删除前后路径对比 Fig. 7 Comparison of path before and after deletion |
| 图选项 |
6) 精英保留策略
为确保每一代中的最优路径不被破坏，本文采用了精英保留策略来保留每一代中适应度值最高的路径。在迭代过程中，找出当前候选路径中适应度最高的路径a，将此路径的适应度值和与迄今为止最优路径(精英)A的适应度值进行比较，若a优于A，则将a记为A，完成最优路径的保存。
2.4 适应度函数 适应度函数是评判个体优劣的一种性能指标，将直接影响遗传算法能否找到最优解。本文目的是使移动机器人在运动过程中平稳地避开障碍物，到达目的地，并且行驶路径最短。因此本文同时对路径长度、路径的连贯程度进行优化。适应度函数设计如下：
 | (8) |
式中：f1(p)为路径长度函数；f2(p)为路径连贯性函数；w1、w2分别为f1(p)、f2(p)的权值；Inf为一个足够大的实数。路径长度函数为
 | (9) |
式中：L为路径节点的数量。距离机器人越近的路径越趋近于机器人下一时刻行驶的位置，因此本文取栅格图中的第一段路径当作机器人即将行驶的路径。路径连贯性函数为
 | (10) |
式中：θk为当前时刻移动机器人朝向与规划路径的夹角；θk－1为前一时刻移动机器人朝向与规划路径的夹角。本文将移动机器人抽象成为一个质点，θ也可以表示为移动机器人的期望转角，即
 | (11) |
式中：(x1, y1)为执行删除算子后的第1个路径节点；(xS, yS)为起点坐标。
3 仿真验证 本文将整个路径规划过程分成若干个子过程，以滚动窗口的形式进行在线规划。要想最终得到长度最短且最平滑的路径，就必须使每个滚动窗口规划出的路径长度最短且最平滑。因此本文首先分别对比在3种不同场景下传统A*算法、常规遗传算法(GA)和本文改进遗传算法(IGA)搜索出的路径长度和路径平滑程度。场景1为障碍物形状规则且环境简单的场景，场景2为障碍物形状不规则且环境复杂的场景，场景3为带“U型”障碍物的场景。综合考虑算法运行速度和栅格环境建模的准确性，将栅格图的尺寸大小定为25×25格，每一栅格的边长等效于0.2 m的实际场景距离。3种场景下的路径规划对比结果如图 8所示。
 |
| 图 8 三种场景下的路径规划结果对比 Fig. 8 Comparison of path planning results in three scenarios |
| 图选项 |
仿真和实验中, 改进遗传算法的参数{Mp, T, Pc}设置为{100, 100, 0.8}。T为算法迭代的次数，Pc为交叉概率。可以看出，在3种不同的复杂场景下，常规遗传算法和A*算法规划出的路径存在较多转折点和冗余点，不利于移动机器人的运动控制，而改进遗传算法规划出的路径最为平滑且路径长度最短，且改进遗传算法在带有“U型”障碍物的场景下，仍然能够规划出较好的路径，证明本文所提出的算法能够达到全局最优。
为了验证本文将改进的遗传算法应用到局部路径规划中能够改善移动机器人行驶过程的平稳性，并减少移动机器人行驶的路径长度，分别对比在相同场景下移动机器人采用A*算法、常规遗传算法和改进遗传算法的局部路径规划结果。设置移动机器人的单步步长为0.4m，障碍物感知距离为3m。由于只有距离期望路径较近的障碍物才会影响到移动机器人，因此本文在期望路径附近设置多个障碍物(其他没有影响到移动机器人行驶的障碍物未画)。
若移动机器人在行驶过程中偏航角变化的越小，则认为行驶过程越平稳。本文以机器人行驶过程中最大偏航角变化量及转角绝对值之和来衡量机器人行驶的平稳程度。3种情况下的移动机器人的转角变化曲线如图 9所示。可以看出，使用本文改进的遗传算法进行路径规划时，移动机器人的最大偏航角变化量及转角绝对值之和均小于常规遗传算法和A*算法。移动机器人的行驶轨迹对比结果如图 10和图 11所示。仿真结果如表 1所示。
 |
| 图 9 移动机器人转角变化曲线 Fig. 9 Curves of mobile robot turning angle change |
| 图选项 |
 |
| 图 10 改进遗传算法和A*算法路径规划结果对比 Fig. 10 Path planning results comparison between improved genetic algorithm and A* algorithm |
| 图选项 |
 |
| 图 11 改进遗传算法和常规遗传算法路径规划结果对比 Fig. 11 Path planning results comparison between improved genetic algorithm and conventional genetic algorithm |
| 图选项 |
表 1 路径规划仿真结果比较 Table 1 Comparison of path planning simulation results
| 算法 | 路径 长度/m | 最大偏航角 变化量/(°) | 转角绝对值 之和/(°) |
| A* | 15.4 | 28.8 | 385.0 |
| GA | 15.2 | 25.0 | 250.8 |
| IGA | 15.2 | 10.9 | 135.8 |
表选项
从仿真结果来看：机器人使用传统A*算法和常规遗传算法进行路径规划时，行驶轨迹均出现不连续的情况，而使用本文所改进的遗传算法进行路径规划时，机器人行驶过程比较平稳，相对于常规遗传算法和A*算法有较大的改善。
4 实验验证 该实验的目标是为了验证本文所提出的路径规划方法能够使移动机器人平稳行驶并且避免行驶多余的路径，本文在Jaugar4×4轮式移动机器人平台上进行实验，如图 12所示。
 |
| 图 12 Jaugar4×4轮式移动机器人 Fig. 12 Jaugar4×4 wheeled mobile robot |
| 图选项 |
Jaugar4×4轮式移动机器人装载了双目摄像头、GPS和IMU等传感器。通过双目摄像头获取周围环境信息，其最大探测距离为3m，根据VSLAM/GPS/IMU组合定位得到当前机器人的位置和姿态信息来进行路径规划，其定位误差小于0.2m。上位机根据路径规划结果发送指令控制机器人进行避障以及跟踪期望路径。
本文在完全相同的场景下进行路径规划对比实验，分别测试移动机器人在使用A*算法、常规遗传算法和改进遗传算法进行路径规划时的行驶路径长度和平稳程度。移动机器人采用改进遗传算法、A*算法和常规遗传算法进行路径规划的转角变化对比结果如图 13所示，行驶轨迹对比结果如图 14和图 15所示。实验数据如表 2所示。
 |
| 图 13 移动机器人实际行驶过程中转角变化曲线 Fig. 13 Curves of turning angle change in actual driving process of mobile robot |
| 图选项 |
 |
| 图 14 改进遗传算法和A*算法移动机器人行驶轨迹对比 Fig. 14 Comparison of mobile robot trajectory between improved genetic algorithm and A* algorithm |
| 图选项 |
 |
| 图 15 改进遗传算法和常规遗传算法移动机器人行驶轨迹对比 Fig. 15 Comparison of mobile robot trajectory between improved genetic algorithm and conventional genetic algorithm |
| 图选项 |
表 2 路径规划实验结果比较 Table 2 Comparison of path planning experiment results
| 算法 | 路径 长度/m | 最大偏航角 变化量/(°) | 转角绝对值 之和/(°) |
| A* | 16.4 | 12.1 | 206.5 |
| GA | 16.2 | 11.1 | 196.1 |
| IGA | 15.9 | 7.5 | 157.0 |
表选项
从实验结果来看，在用传统A*算法进行规划时，机器人的实际行驶轨迹长度为16.4m，行驶过程中最大偏航角的变化量为12.1°，转角绝对值之和为206.5°；使用常规遗传算法进行规划时，机器人的实际行驶轨迹为16.2m，行驶过程中最大偏航角变化量为11.1°，转角绝对值之和为196.1°；而使用本文所改进的遗传算法进行规划时，机器人的实际行驶轨迹为15.9m，行驶过程中最大偏航角变化量为7.5°，转角绝对值之和为157.0°。实验结果表明，在使用本文改进的遗传算法进行路径规划时，机器人行驶轨迹长度小于A*算法和常规遗传算法；行驶过程中的最大偏航角变化量和转角绝对值之和相对于常规遗传算法和A*算法均有所改善。
5 结论 1) 本文在遗传算法的基础上做出改进，新增删除算子剔除掉栅格图中冗余的路径点，解决了常规路径规划算法在栅格图中求解的路径长度非最短及转折点较多的问题，并且在适应度函数中考虑了前后两次规划过程路径的连贯性，解决了机器人行驶过程中多次规划出现路径不连贯的问题。
2) 使用本文改进遗传算法进行路径规划，移动机器人的实际行驶轨迹长度、行驶过程中的最大偏航角变化量及转角绝对值之和均小于常规遗传算法，显著提高了移动机器人行驶的效率和平稳性。
参考文献
| [1] | MAC T T, COPOT C, TRAN D T, et al. Heuristic approaches in robot path planning:A survey[J]. Robotics and Autonomous Systems, 2016, 86: 13-28. |
| [2] | 赵晓, 王铮, 黄程侃, 等. 基于改进A*算法的移动机器人路径规划[J]. 机器人, 2018, 40(6): 137-144. ZHAO X, WANG Z, HUANG C K, et al. Mobile robot path planning based on improved A* algorithm[J]. Robot, 2018, 40(6): 137-144. (in Chinese) |
| [3] | 周慧子, 胡学敏, 陈龙, 等. 面向自动驾驶的动态路径规划避障算法[J]. 计算机应用, 2017, 37(3): 883-888. ZHOU H Z, HU X M, CEHN L, et al. Dynamic path planning obstacle avoidance algorithm for autonomous driving[J]. Computer Application, 2017, 37(3): 883-888. (in Chinese) |
| [4] | PADEN B, CAP M, YONG S Z, et al. A survey of motion planning and control techniques for self-driving urban vehicles[J]. IEEE Transactions on Intelligent Vehicles, 2016, 1(1): 33-55. |
| [5] | ALIA C, GILLES T, REINE T, et al.Local trajectory planning and tracking of autonomous vehicles, using clothoid tentacles method[C]//IEEE Symposium on Intelligent Vehicles.Piscataway, NJ: IEEE Press, 2015: 674-679. |
| [6] | CHU K, LEE M, SUNWOO M. Local path planning for off-road autonomous driving with avoidance of static obstacles[J]. IEEE Transactions on Intelligent Transportation Systems, 2012, 13(4): 1599-1616. |
| [7] | 王景存, 张晓彤, 陈彬, 等. 一种基于Dijkstra算法的启发式最优路径搜索算法[J]. 北京科技大学学报, 2007, 29(3): 346-350. WANG J C, ZHANG X T, CHEN B, et al. Heuristic optimization path-finding algorithm based on Dijkstra algorithm[J]. Journal of University of Science and Technology Beijing, 2007, 29(3): 346-350. (in Chinese) |
| [8] | WANG Q, HUANG W H, LIU B, et al.An improved A* algorithm for path-planning of two-wheeled self-balancing vehicle[C]//IEEE Conference on Industrial Electronics and Applications(ICIEA).Piscataway, NJ: IEEE Press, 2018: 841-846. |
| [9] | 辛煜, 梁华为, 杜明博, 等. 一种可搜索无限个邻域的改A*算法[J]. 机器人, 2014, 35(5): 627-633. XIN Y, LIANG H W, DU M B, et al. An improved A* algorithm for searching infinite neighborhood[J]. Robo, 2014, 35(5): 627-633. (in Chinese) |
| [10] | XU W D, WEI J Q, DOLAN J M, et al.A real-time motion planner with trajectory optimization for autonomous vehicles[C]//IEEE International Conference on Robotics and Automation.Piscataway, NJ: IEEE Press, 2012: 2061-2067. |
| [11] | GU T Y, SNIDER J, DOLAN J M, et al.Focused trajectory planning for autonomous on-road driving[C]//IEEE Symposium on Intelligent Vehicle.Piscataway, NJ: IEEE Press, 2013: 547-552. |
| [12] | LI X H, SUN Z P, LIU D X, et al.Combining local trajectory planning and tracking control for autonomous ground vehicles navigating along a reference path[C]//IEEE International Conference on Intelligent Transportation Systems.Piscataway, NJ: IEEE Press, 2014: 725-731. |
| [13] | WU M H, CHEN E K, SHI Q Q, et al.Path planning of mobile robot based on improved genetic algorithm[C]//2017 Chinese Automation Congress(CAC).Piscataway, NJ: IEEE Press, 2017: 6696-6700. |
| [14] | GONG Z H, SHAN Y X, DENG Y Q, et al.Balance mechanism design for the fusion of pure pursuit and PI tracking controller[C]//2018 Chinese Automation Congress (CAC).Piscataway, NJ: IEEE Press, 2018: 3149-3152. |
| [15] | LIU R, DUAN J.A path tracking algorithm of intelligent vehicle by preview strategy[C]//Proceedings of the 32nd Chinese Control Conference.Piscataway, NJ: IEEE Press, 2013: 5630-5635. |
| [16] | LAMINI C, BENHLIMA S, ELBEKRI A. Genetic algorithm based approach for autonomous mobile robot path planning[J]. Procedia Computer Science, 2018, 127: 180-189. |
| [17] | LI Q, ZHANG W, YIN Y X, et al.An improved genetic algorithm of optimum path planning for mobile robots[C]//International Conference on Intelligent Systems Design & Applications.Piscataway, NJ: IEEE Press, 2006: 637-642. |
| [18] | SU J, LI J.Path planning for mobile robots based on genetic algorithms[C]//Proceedings of 9th International Conference on Natural Computation.Piscataway, NJ: IEEE Press, 2014: 723-727. |
