改进遗传算法与领航跟随法的机器人编队方法

江 涛，张志安，程 志，李金芝，陆 静

南京理工大学 机械工程学院，南京210094

1 引言

近年来，随着机器人技术、通信技术和自动控制技术的不断发展，多机器人协同控制研究引起了众多领域研究者的关注，并在军事、空间探索、交通控制与服务行业等领域展现了广阔的应用前景[1]。作为多机器人系统协同控制中最基础和最重要的研究问题之一，编队控制要求多个移动机器人同时运动到目标区域，并在运动过程中保持设定的队形，同时能安全地避开障碍物[2]。

移动机器人的路径规划是指：在障碍物环境中，根据评价指标（如移动时间、路径长度、能量消耗等）和周围环境信息自主地搜索出一条从起始点到目标点的安全无碰撞且较优的可行路径[3]。合理的路径规划算法是移动机器人研究的基础性问题，不仅是研究的核心，更是制约移动机器人发展的瓶颈[4]。目前移动机器人路径规划常用的优化算法主要有人工势场法、A*算法、粒子群算法和遗传算法等[5]。遗传算法是一种采用种群搜索机制的全局性寻优算法，由于遗传算法具有并行性、全局搜索能力等优点，使得求取最优解的可能性和效率大大提高，因此被国内外学者广泛应用到求解路径规划问题中[6]。

多机器人编队控制是指多个机器人向目标点运动的过程中，既能够收敛于特定的队形，又能适应环境约束的控制技术[7]。目前多机器人的编队控制方法主要有虚拟结构法、领航跟随方法、人工势场法、基于行为法等[8]。由于领航跟随法控制简单，易于工程实现，因此被广泛应用于编队控制中。

文献[9]利用启发式方法和随机方法相结合的策略加快了收敛速度；文献[10]对遗传算法进行自适应改进，计算出能够随时适应的遗传算子，以改进基本遗传算法陷入局部最优问题；文献[11]提出了一种基于障碍物的路径搜索方法，有效地缩小了算法的搜索空间；文献[12]提出了一种多次随机交叉法来维持种群的多样性，提高了算法寻找全局最优解的概率。

本文采用的是遗传算法与领航跟随法相结合的方法进行编队。传统遗传算法收敛速度较慢，且规划的路径没有充分考虑路径平滑度要素，因此规划出的路径转弯多，不利于机器人运动。本文对遗传算法进行了改进，在适应度函数中加入了路径平滑度因素，使转弯次数少的路径具有较高的适应度函数值，在进行选择操作时更容易被选中，然后通过交叉操作将优良基因传到下一代群体中保留下来，提高了路径的顺滑度；在种群选择时加入了精英保留策略，适应度函数值最大的个体可以不进行交叉变异操作，而是直接复制到下一代种群个体中，防止破坏父代个体的优良基因，提高算法收敛速度。通过MATLAB软件建立栅格地图进行仿真，仿真结果表明：与传统遗传算法相比，改进后的算法可以更快地收敛到最短路径，且路径平滑度较好。规划出最优路径后，使用领航跟随法控制跟随机器人的运动，首先根据领航机器人的位姿信息以及队形实时计算每台虚拟机器人的位置，然后控制每台跟随机器人运动到虚拟机器人的位置从而形成设定的队形。

2 遗传算法路径规划

2.1 遗传算法模型

遗传算法是模仿自然界生物进化“优胜劣汰，适者生存”法则构建起的一种随机全局搜索和优化方法，具有良好的并行性和可扩展性。图1为遗传算法路径规划流程图。

图1 遗传算法路径规划流程图

遗传算法在求解最优路径时，先将可行解通过某种方式形成染色体，随机选择一部分个体生成初始种群；然后通过一定方式将目标函数转化为适应度函数，并通过计算个体的适应度函数值的大小来对个体进行选择；最后再对个体进行交叉和变异操作，以产生适应度函数值更大的个体；通过不断的繁衍后代，使后代更加适应环境，直到满足期望的终止条件，从而形成种群的最优解。

2.1.1 路径编码

本文采用栅格法建立移动机器人的地图模型。用正方形表示机器人的行走空间，有障碍物的栅格称为障碍物栅格，用黑色填充；无障碍物的栅格称为自由栅格，用白色填充[13]。

常用的栅格标记法有坐标法和序号标记法[14]。坐标法标号时每一个栅格用(x,y)的坐标形式表示。序号标记法由栅格左下角为0开始，按照由左到右，由下到上的顺序依次加一进行标记。序号N和坐标(x,y)的转换公式为：

其中，N为使用序号标记法标记的栅格序号，Gsize为每一行所包含的栅格数量，int为取整操作，%为取余操作。

图2栅格地图中所示的折线表示为一条能使机器人从点（1，1）运行到点（6，6）的可行路径，路径编码为（0，6，12，18，25，32，33，34，35）。

图2 可行路径

2.1.2适应度函数计算

在遗传算法中，适应度函数决定了一个个体被遗传到下一代的概率。路径规划的目的是使机器人从起始点运动到目标点的距离最短，因此在进行适应度函数的设计时要满足机器人运行轨迹越短适应度值越大。在一个路径个体中，路径总长度Length的计算公式如下：

其中，end为机器人行走的步数。

路径适应度函数为：

由上式可以看出，路径越短，适应度值越大，则该路径被保留下来的概率就越大。

2.1.3 选择操作

选择的目的是将满足要求的个体保留，不满足要求的个体剔除。选择方法基于上一步中计算出的适应度函数值，采用轮盘赌法。

轮盘赌的方式保证了部分非最优的个体，可以有效地防止算法陷入局部最优解[15]。

2.2 遗传算法改进

2.2.1 适应度函数加入路径平滑度

由于运动学和动力学的约束，机器人行进时拐弯不宜过大，相对平滑的路径更加有利于机器人的行驶[16]。在使用传统遗传算法进行路径规划时，机器人只是把路径最长度作为适应度函数的决定因素，造成生成的路径拐弯次数过多，机器人运动时间和耗能大大增加，同时还有撞击障碍物的危险。因此本文在计算路径适应度函数时考虑了路径平滑度因素。因此对适应度函数做出了修改，新的适应度函数如下：

其中，适应度函数的第一部分为传统遗传算法的适应度函数：

由几何关系可知，路径越平滑，相邻三点形成的角度越大，角度越大相邻三点之间的距离越大，因此需要计算路径中所有相邻三点的距离，计算公式如下：

d3值大小的不同可表示路径中连续三点的夹角为180°、钝角、直角和锐角这四种情况，其中夹角为180°时三点连成一条直线，平滑度最好，钝角和直角次之。由于机器人动力学的约束，本文算法中设定不允许出现锐角的情况，所以分别对钝角、直角和锐角给予5、30和1 000的惩罚值Punish。整条路径的惩罚值总和为：

适应度函数的第二部分fit2=1 Punish总，根据路径长度和平滑度之间的权重关系，适应度函数的两部分需要各取一个权重参数a和b。

2.2.2 选择使加入精英保留策略

精英保留策略的基本思想是对种群中适应度函数值最高的个体进行保留，不对其进行遗传操作而直接将其遗传到下一代之中。在遗传算法中，遗传操作在随机进行的过程中可能对优良个体进行遗传操作进而破坏其优秀的染色体结构，进而降低种群的平均适应度值，对算法的运算效率和收敛性都将产生不好的影响，因此希望拥有最高适应度值的个体尽可能地遗传到下一代。为了实现这一目标，采用精英选择方法来执行选择操作。通过使用这种选择方法，遗传过程中某一代的最优解可以不参加遗传操作而直接遗传到下一代，保证其不被交叉和变异等遗传操作破坏。

3 领航跟随法编队控制

相比于单台机器人而言，多机器人协同编队在灵活性、可拓展性、鲁棒性等方面具有更好的优越性。目前机器人编队中最常用的算法主要有领航跟随法、基于行为法、虚拟结构法等方法[17]。领航跟随法采用链式的拓扑结构，整个队形由领航机器人和跟随机器人组成，在实施过程中只需要使跟随者获得领航者的运动状态信息并对领航者的运动轨迹进行跟踪便可实现编队控制[18]。领航跟随法由于控制器的设计相对简单，被广泛应用于编队算法中。

本文中编队控制采用的算法是领航跟随法。首先需要选定一台机器人作为领航者，主要负责整个编队的路径规划任务，根据地图信息规划出一条从起点到终点的无碰最优路径。其余机器人则被视为跟随者，跟随机器人负责跟踪领航者进行运动，并尽可能与领航机器人之间保持队形所需要的距离和角度。多台机器人形成编队时，编队中的每台机器人不仅要避开障碍物安全抵达目的地，而且要避免与队形中其他成员发生碰撞[19]。

领航跟随法主要有l-l控制和l-φ控制两种控制模式。l-l控制即每个跟随机器人以固定的距离l跟随两个领航机器人，从而保持期望的队形；l-φ控制即每个跟随机器人以一定的距离和角度跟随领航机器人，实现期望队形[20]。本文采用l-φ控制方法。算法实现步骤如下：

步骤1领航机器人根据地图信息使用改进遗传算法规划出自身的最优路径。

步骤2领航机器人沿着最优路径向目标点运行，并利用设计好的算法进行避障，同时，领航机器人实时检测自身的位姿信息，根据设定的队形使用l-φ控制法生成虚拟机器人的轨迹，并发送给跟随机器人。

步骤3跟随机器人实时接收领航机器人发送的轨迹，调整自己的速度和运动方向，使沿着虚拟机器人的轨迹运行。同时，跟随机器人实时检测自身周围的障碍物信息，当跟随机器人遇到障碍物时，会在原地进行一定时间的等待，当领航机器人通过后，跟随机器人会运动到领航机器人上一时刻的位置，通过障碍物后，跟随机器人再回到虚拟机器人的轨迹上，恢复设定队形。

4 仿真

4.1 路径规划仿真

为了验证本文改进的遗传算法地有效性，建立20×20的栅格地图环境模型对该算法进行仿真，并与传统遗传算法进行比较。设定机器人起始点坐标为（0，0），目标点坐标为（20，20），种群规模为100，最大迭代次数为50代，交叉概率为0.9，变异概率为0.002。实验结果如图3、图4所示。

从图3（a）和图4（a）可以看出，改进后遗传算法规划出的路径转弯次数明显减少，路径平滑度比改进前有很大改善。从图3（b）和图4（b）可以看出，传统遗传算法和改进遗传算法都收敛到了最短路径，路径长度都为29.799，但传统遗传算法用了15次迭代，改进后的遗传算法6次迭代就收敛到了最短路径。

为了避免偶然性因素，如图5（a）所示，在栅格地图左下角4×4区域内随机选取自由栅格作为起始点，在地图右上角4×4区域内随机选取自由栅格作为目标点，将传统遗传算法和改进遗传算法分别运行100次进行仿真对比，观察各个算法收敛至最短路径所需的迭代次数。

图3 传统遗传算法

图4 改进遗传算法

图5 算法改进前后收敛至最短路径迭代次数

由图5（b）可知，传统遗传算法需要经过13～24次迭代收敛到最短路径，平均迭代次数为17.85次；但使用改进后的遗传算法，只需5～15次便能收敛到最短路径，平均迭代次数为9.93次。

本文又选取了文献[9]、文献[10]中所述的遗传算法与传统遗传算法和改进后的遗传算法进行对比，观察四种遗传算法的收敛速度。

将四种算法运行100次统计平均迭代次数与最终路径的平均长度，得到的数据如表1所示。

表1 各算法迭代次数与路径长度对比

从图6和表1中可以看出，文献[9]中所用改进的遗传算法收敛速度非常快，能够迅速收敛到接近最优解的值，但最终并没有收敛到最优解；本文改进的遗传算法比传统遗传算法和文献[10]中改进的遗传算法都收敛到了最优解，但本文平均只用了9.93次便收敛到了最优解，速度更快。

图6 四种遗传算法性能对比

实验证明，与传统遗传算法、文献[9]中遗传算法和文献[10]遗传算法相比，改进的遗传算法具有较快的收敛速度，且规划出的路径平滑度较好，能够缩短规划路径所需的时间和机器人运动所需的时间，且运行更加安全，综合性能较优。

4.2 多机器人编队仿真

本文以三个机器人组成三角形队形为例进行验证。设定领航机器人的起始点坐标为（2，2），目标点坐标为（20，20），两个跟随机器人的起始点坐标分别为（1，2）和（2，1），目标点坐标分别为（19，20）和（20，19）。

仿真1领航者和跟随者都没有遇到障碍物，机器人沿直线直接从起点运动到了终点，所有机器人全程保持了三角形队形，轨迹图如图7所示。

仿真2领航机器人遇到障碍物，跟随者没有遇到障碍物。在起点与终点的连线上增加了一个障碍物，当领航机器人运动到障碍物面前会绕过障碍物寻找一条新的路径到达目标点，且跟随机器人会跟随着机器人绕开障碍物，且运动过程中始终保持着三角形队形运动到目标点，轨迹图如图8所示。

图7 无障碍环境下路径规划轨迹图

图8 领航者遇到障碍环境下路径规划轨迹图

仿真3领航者没有遇到障碍物，跟随者遇到障碍物。当跟随机器人的路径上出现了障碍物，编队会进行短时间的变换队形，跟随机器人会在障碍物面前原地等待一段时间，待到领航机器人通过后，跟随机器人会移动到领航机器人上一时刻的位置，当跟随者绕过障碍物以后，跟随机器人重新回到虚拟机器人的轨迹上，恢复设定队形，轨迹图如图9所示。

图9 跟随者遇到障碍环境下路径规划轨迹图

仿真4只能单个机器人通过时，两个跟随机器人会在障碍物前等待，当领航机器人通过狭窄通道后，跟随机器人会依次运动到领航机器人的位置，呈“一”字形挨个通过狭窄通道，当所有机器人通过狭窄区域后，再恢复至设定队形，轨迹图如图10所示。

实验表明，本文设计的领航跟随法能使所有机器人保持队形安全无碰撞地运行到目标位置。当机器人遇到障碍物时，能够及时地变换队形来避开障碍物，完成避障任务后再重新恢复队形。

图10 狭窄环境下路径规划轨迹图

5 结束语

本文将遗传算法和领航跟随法结合，并对遗传算法进行一定的改进，将平滑度要素加入到适应度函数中，并在选择操作时加入了精英保留策略，在存在障碍物的栅格地图中使用改进后的遗传算法规划出了最优路径，实验证明，通过多次迭代，最优路径长度迅速收敛并最终趋于稳定，而且相比于传统遗传算法，改进后的遗传算法收敛更快，规划出的路径也更加平滑。

从队形保持、队形变换来看，当没有遇到障碍物时，多机器人可以始终保持队形从起始点运动到目标位置，当机器人碰到狭窄通道或者遇到障碍物时，会进行适当的队形变换以躲避障碍物，通过狭窄通道或者绕开障碍物后，多机器人又恢复成设定的队形向目标点前进，最终到达指定位置。