CN103901887B - 一种基于改进粒子群算法的多移动机器人编队控制方法 - Google Patents

Abstract

本发明公开了一种基于粒子群算法的多机器人编队控制方法，该方法采用领航跟随者的方法与人工势场方法相结合，引入粒子群算法，优化跟随机器人运行中的相关参数，使得跟随机器人的运动路径及避障、避碰效果有明显改善。通过调整势场函数，对危险距离以外的受力进行归一化，危险距离以内给予机器人较大的斥力，以优化避障、避碰效果。在粒子群算法中，通过调整权重系数和学习因子，引入竞争机制，以提高算法的优化效果，最终使得跟随机器人的避障、避碰效果有明显改善。

Description

一种基于改进粒子群算法的多移动机器人编队控制方法

技术领域

本发明属于多移动机器人的编队控制方法，尤其涉及一种基于改进粒子群优化算法的多移动机器人编队控制方法。

背景技术

编队控制是一个具有典型性和通用性的多机器人协调问题，是多机器人协调问题的基础。多机器人编队技术已广泛应用于国防、工农业、柔性制造业以及无人探险等领域。目前解决编队控制问题的方法很多，主要有基于行为的方法、领航跟随者方法、人工势场方法、虚拟结构方法等。基于行为的方法其行为难以明确定义，队形稳定性无法得到保证；领航跟随者的方法没有队形反馈，容易出现掉队现象；人工势场方法容易陷入局部极小值问题；虚拟结构方法要求队形作为一个虚拟结构体，无法考虑整体避障问题。

粒子群算法（PSO）需要调整的参数较少，易于实现，且可进行并行计算和无梯度计算，有较好的全局搜索能力。将粒子群算法应用于编队控制，用于调整行为权值，优化参数。在较大规模的多机器人系统中，要优化的行为控制参数很多，即优化目标维数高，目标函数极值多，采用标准的粒子群优化算法及后来改进的粒子群算法，很难快速收敛到全局最优值。

本发明提出了一种基于改进粒子群优化算法的编队控制方法。本发明对粒子群算法进行改进，核心是提高算法的收敛速度和执行效率，优化跟随机器人的相关参数，使得编队任务的路径更优。

公开号为CN102331711A的专利：一种移动自主机器人的编队控制方法，其采用的机器人队形控制算法，是基于此分布式方法所得到的优化方法，因此其能够解决广泛的目标队形控制问题，且局限于特殊的目标几何队形。但是实用性较差，没有涉及障碍物环境下队形收敛问题。

公开号为CN102096415A的专利：基于Ad-Hoc网络和leader-follower算法的多机器人编队方法，采用leader运动轨迹控制整个编队运动轨迹，建立信息反馈，确保follower对leader的跟踪过程无丢失，采用本发明可以使多机器人系统在完成任务的过程中，顺利避开障碍物，到达目标点，并能在整个过程中保持初始队形，能实现多机器人的实时的队形控制。但是该方法没有针对机器人避障过程中会出现振荡现象，以及人工势场方法中局部极小值问题提出具体解决方法。

发明内容

针对以上现有技术中的不足，本发明的目的在于提供一种对于编队运行过程中的路径长度，避障、避碰效果有明显优化的基于改进粒子群算法的多移动机器人编队控制方法。本发明的技术方案如下：一种基于改进粒子群算法的多移动机器人编队控制方法，其包括以下步骤：

101、系统初始化，将包含有障碍物的实际环境抽象成一张环境地图，且在环境地图中设置机器人的最终目标运动点S；

102、在步骤101的环境地图中初始化n个机器人的位置，其中每个机器人通过自身的探测器获取环境地图中的障碍物大小及位置坐标、环境地图中其余机器人的大小及位置坐标，并存储于机器人各自的信息库中；

103、根据步骤102中n个机器人的位置坐标计算出几何位置中心点作为队形集合点O，所述n个机器人根据人工人势场法向队形集合点O移动形成初始菱形队形；

104、根据步骤103中形成的初始菱形队形，选择出与步骤101中设置的最终目标运动点S直线距离最近的机器人作为领航机器人，其余机器人作为跟随机器人以速度v运动，所述跟随机器人与领航机器人的距离为l，所述领航机器人以速度V向目标点运动，跟随机器人每运行一步，记录下D_rr、D_ro、D_rotatenum、D_rotateamo、D_{deathdirectionnum}，其中D_rr表示跟随机器人与跟随机器人间的碰撞次数；D_ro表示跟随机器人与障碍物间的碰撞次数；D_rotatenum表示跟随机器人避障及避碰过程中的转向次数；D_rotateamo表示跟随机器人避障及避碰过程中转动角度的大小；D_{deathdirectionnum}表示跟随机器人停止运行的步数；

105、根据步骤104中得到的D_rr、D_ro、D_rotatenum、D_rotateamo、D_{deathdirectionnum}，采用粒子群优化算法设计出适应度函数fitness，适应度函数表达式如下：fitness=D_rr+D_ro+D_rotatenum+D_rotateamo+D_{deathdirectionnum}；计算出最优位置点，作为跟随机器人下一步运动的最优目标点，跟随机器人根据计算出的最优位置点调整自己的位置、方向及速度值移动到最优位置点；

106、判断领航机器人是否已经到达步骤101中设定的最终目标运动点S，若没有到达，则返回步骤104；若到达则编队结束。

本发明的优点及有益效果如下：

本发明专利的技术方案就是针对现有技术采用领航跟随作者方法与人工势场方法相结合，同时引入粒子群优化算法实现多机器人编队控制。通过调整势场函数，对危险距离以外的受力进行归一化，危险距离以内给予机器人较大的斥力。同时引入粒子群优化算法，通过引入竞争机制，调整权重系数和学习因子，优化编队队形中跟随机器人的避障及避碰效果。经过大量实验，本发明的方法中跟随机器人的运动路径更短，避障及避碰过程中机器人偏转次数明显减小。

附图说明

图1是本发明优选实施例改进粒子群算法的多机器人编队控制方法流程图;

图2是队形实现模型图。

具体实施方式

下面结合附图给出一个非限定性的实施例对本发明作进一步的阐述。

如图1所示，本发明运用领航跟随者方法与人工势场的方法相结合，实现静态环境中多机器人的编队控制任务，具体实现步骤如下：

步骤1：将实际运行环境抽象成一张平面图，根据障碍物位置和大小建立环境地图。用ID号标记障碍物，以便机器人探测障碍物信息，同时设置最终目标点坐标。

步骤2：初始化机器人在地图中的位置信息。在地图中任意设置4个机器人的位置点，用半径为1个单位的圆点表示机器人，机器人通过自身的探测器获取全局地图信息，将信息记录于机器人的信息库中。包括各机器人的身份、大小及位置坐标，障碍物的大小及位置坐标，最终目标点的位置坐标。

步骤3：形成初始队形。编队的队形为一菱形，根据步骤2中获取的各机器人的信息，以各机器人的几何平均位置点作为参考点，以机器人到达队形点运动的距离最短为条件，将目标队形点位置分配给相应机器人。各机器人根据得到的目标队形点信息，运用人工势场的方法，使得机器人在队形目标点的吸引力作用下向队形目标点运动，运动过程中如遇到障碍物，则考虑障碍物排斥力的作用，采取避障措施。

在设置队形时，根据如下公式调整队形，使得队形沿队形中心点与最终目标点连线方向偏转一定角度，得到最终队形点坐标：

其中x和y为通过计算得到的队形位置点；θ为队形几何中心点到编队任务最终目标点的方向角；△x和△y分别为计算得到的队形位置点与队形几何中心点的距离差值。

队形实现模型图2所示，图中R1、R2、R3、R4分别代表四个机器人的位置点。

当得到的最终队形点与障碍物位置发生冲突时，将队形点作适当的偏移，以适应障碍物。形成初始队形过程中涉及到机器人躲避障碍物及其它机器人的问题，采用人工势场的方法进行处理。设定机器人受到来自障碍物的排斥力、其它机器人的排斥力以及队形点的吸引力的合力作用向各自队形点运动。对于吸引力，采用两点间的距离来表示，并进行归一化。

排斥力大小采用如下公式得到：

其中d表示机器人表面到障碍物表面或其它机器人表面的距离；dl表示机器人传感器的探测距离；ds表示机器人避免发生碰撞而与障碍物或其它机器人的限制距离；M为一数值较大的常数。将机器人受力进行归一化而给限制距离内的排斥力一个较大值，能够更有效地避免碰撞的发生。

步骤4：设置队形下一步目标点。待步骤3中所有机器人都到达期望队形点，则机器人开始执行奔向目标任务。根据机器人在队形中的位置，选择最接近最终目标点的机器人作为领航机器人，领航机器人保持一定速度向最终目标点移动。根据初始队形，以领航机器人的位置为参考点，计算出各跟随机器人位置点到领航机器人位置点的距离，将这几个距离量作为队形偏移量，分别记为l₁、l₂、l₃，用以保持队形。领航机器人每运动一步，计算一次各跟随机器人与领航机器人之间的实际队形偏移量，分别记为l'₁、l'₂、l'₃。计录下跟随机器人相应的位置点并更新机器人信息库。

领航机器人每运行一步，根据设计的队形为跟随机器人分配队形点。跟随机器人根据得到的队形点及队形偏差值，调整速度。当1号跟随机器人的实际队形偏移量l'₁大于期望队形偏移量l₁时，跟随机器人会获得一个加速度a₁，以追踪期望队形点，达到保持队形的目的。l'₁与l₁之间的差值越大，则加速度a₁也就越大。如此调整各个跟随机器人的运动行为，以便更好地保持队形。

各机器人在保持队形向目标点运动的过程中，同时记录下机器人之间的碰撞次数D_rr，机器人与障碍物间的碰撞次数D_rr，机器人避障（躲避障碍物）及避碰（躲避其他机器人）过程中的转向次数D_rotatenum，机器人避障及避碰过程中转动角度的大小D_rotateamo，机器人停止运行的步数机器人停止运行的步数D_{deathdirectionnum}。若机器人停止运动，则表示机器人陷入死锁状态。设定D_{deathdirectionnum}不小于3时，激活机器人的随机扰动行为，使机器人随机转动一个15度以内的角，以跳出方向死锁。

步骤5：优化当前位置点。由于随机扰动行为的设计，结合人工势场的方法，使得机器人的运动波动性较大。引入粒子群算法，根据步骤4中记录下的跟随机器人获取的队形目标点的信息及运行得到的相关参数设计适应度函数，运用粒子群优化算法通过优化下面五个参数，从而达到优化跟随机器人的位置点得到更优的运动路线的目的。其适应度函数如下：

fitness=D_rr+D_ro+D_rotatenum+D_rotateamo+D_{deathdirectionnum}

对粒子群算法作了一些改进，引入竞争机制，以增加算法的多样性，同时调整相关系数。设定算法中每个粒子维度为2维，用粒子的这两个维度来分别表示机器人位置点的横纵坐标。初始化算法速度参数大小在0-1之间，初始化算法的位置参数为某一跟随机器人由领航机器人分配得到的位置点。运用粒子群算法更新公式进行迭代计算，每个粒子选取与之相邻的两个粒子作为邻域粒子，分别计算出该粒子与其邻域粒子的个体适应度，将计算得到的个体适应度值进行比较，选取个体适应度更优的粒子代替原来的粒子，作为粒子群中新的粒子。开始给予算法一个较大的自学习因子，目的在于增加算法的局部搜索能力，随着迭代次数的增加，社会学习因子逐渐增大，目的在于增加算法向全局最优的收敛速度。设定算法的最大迭代次数为4000次，如果前后两次迭代中所得到的全局最优粒子的适应度值之差的绝对值不大于0.001，算法终止。所得到的全局最优粒子的位置信息赋给相应的跟随机器人，作为跟随机器人下一步位置的目标点。

步骤6：判断领航机器人是否到达最终目标点，如果没有到达，则返回步骤4，由领航机器人分配队形点，跟随机器人获取队形点，运用粒子群算法优化跟随机器人的避障及避碰过程，跟随机器人调整速度，跟随领航机器人向目标点运动。如果领航机器人已经到达最终目标点，则编队任务结束。

本方法是一个具有典型性和通用性的多机器人协调控制方法，广泛应用于国防、工农业、柔性制造业以及无人探险等领域，有着广泛的应用前景。经过编队的多机器人系统能够协调一致的向目标点运动，可以更可靠地执行任务、提高系统效率，可以减少系统消耗，并且提供冗余度、重构能力和系统结构韧性。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明方法权利要求所限定的范围。

Claims (1)

1.一种基于改进粒子群算法的多移动机器人编队控制方法，其特征在于，包括以下步骤：

101、系统初始化，将包含有障碍物的实际环境抽象成一张环境地图，且在环境地图中设置机器人的最终目标运动点S；

102、在步骤101的环境地图中初始化n个机器人的位置，其中每个机器人通过自身的探测器获取环境地图中的障碍物大小及位置坐标、环境地图中其余机器人的大小及位置坐标，并存储于机器人各自的信息库中；

103、根据步骤102中n个机器人的位置坐标计算出几何位置中心点作为队形集合点O，所述n个机器人根据人工势场法向队形集合点O移动形成初始菱形队形；

104、根据步骤103中形成的初始菱形队形，选择出与步骤101中设置的最终目标运动点S直线距离最近的机器人作为领航机器人，其余机器人作为跟随机器人以速度v运动，所述跟随机器人与领航机器人的距离为l，所述领航机器人以速度V向目标点运动，跟随机器人每运行一步，记录下D_rr、D_ro、D_rotatenum、D_rotateamo、D_{deathdirectionnum}，其中D_rr表示跟随机器人与跟随机器人间的碰撞次数；D_ro表示跟随机器人与障碍物间的碰撞次数；D_rotatenum表示跟随机器人避障及避碰过程中的转向次数；D_rotateamo表示跟随机器人避障及避碰过程中转动角度的大小；D_{deathdirectionnum}表示跟随机器人停止运行的步数，设定此参数大于等于3时，激活机器人的随机扰动行为，使机器人随机转动一个15度以内的角，以跳出方向死锁；领航机器人每运行一步，根据设计的队形为跟随机器人分配队形点，跟随机器人根据得到的队形点及队形偏差值，调整速度，当1号跟随机器人的实际队形偏移量l'₁大于期望队形偏移量l₁时，跟随机器人会获得一个加速度a₁，以追踪期望队形点，达到保持队形的目的，l'₁与l₁之间的差值越大，则加速度a₁也就越大，如此调整各个跟随机器人的运动行为；

105、根据步骤104中得到的D_rr、D_ro、D_rotatenum、D_rotateamo、D_{deathdirectionnum}， 采用粒子群优化算法设计出适应度函数fitness，适应度函数表达式如下：fitness＝D_rr+D_ro+D_rotatenum+D_rotateamo+D_{deathdirectionnum}；计算出最优位置点，作为跟随机器人下一步运动的最优目标点，跟随机器人根据计算出的最优位置点调整自己的位置、方向及速度值移动到最优位置点；

106、判断领航机器人是否已经到达步骤101中设定的最终目标运动点S，若没有到达，则返回步骤104；若到达则编队结束。