基于距离-角度多移动机器人编队避障策略

呙生富，张鹏超，李海婷，徐鹏飞，刘亚恒

(1.陕西理工大学机械工程学院,陕西汉中 723000；2.陕西省工业自动化重点实验室,陕西汉中 723000；3.陕西理工大学电气工程学院,陕西汉中 723000)

0 前言

多移动机器人系统已广泛应用于军事及日常生活等场景，如人员搜救、合作搬运、无人机表演及机器人足球等。编队控制与避障作为多移动机器人系统的关键问题，众多学者对其进行了大量的研究。在多机器人合作完成任务过程中会遇到静态或动态障碍物，所以有效避障是必要的，否则会造成任务失败。

目前常用的避障方法分为两种：一种是编队拆分避障，如栅格法、可视图法、模糊逻辑法、RTT算法、遗传算法等；另一种为编队整体避障，如人工势场法、模型预测控制法、固定队形变换法和基于行为法等。DAI等采用具有速度约束的多机器人规避和穿越障碍物的切换编队策略，主要利用几何避障控制方法来规划安全路径，但需要精确找到航路点才可以稳定队形进行切换。陈刚等人公开一种基于模糊控制的以领航机器人广播的形式告知所有跟随机器人避障，跟随者根据存储的不同队形信息以及领航机器人广播的信息切换队形，但模糊规则繁杂且属人为规定，控制精度难以保证，且没有考虑避障后队形恢复和机器人之间在避障过程中碰撞问题。张佳龙等公开一种基于斥力场函数的编队避障方法以及避障后路径重规划策略，保证在多机编队执行任务中，实现机内实时避障，但斥力场越大，无人机离开触发区的响应速度和运动速度越快，队形变换超调量较大，控制系统稳定性降低。编队整体避障需保持编队内部队形，通过动态调节队形进行在线避障，相比于拆分避障，更具有灵活性、整体性和可靠性，可以有效避免拆分避障出现的跟随者掉队、机器人之间的碰撞问题。但多移动机机器人之间若没有进行有效的协调和沟通，将不能发挥出多机协同避障的优势，造成严重的经济损失。因此，设计一种合理有效的编队避障控制算法对多移动机器人系统的应用具有重要意义。

本文作者综合考虑上述整体编队避障过程中存在的系统响应慢、队形变换超调量大、控制精度低和在避障过程中机器人相互间会发生碰撞的问题，提出一种距离-角度优先级的避障策略，通过建立队形数据库，根据障碍物类型的不同，选择合理的避障队形进行避障，顺利通过障碍物后，快速恢复原队形继续完成任务。

1 领航-跟随队形结构模型及队形数据库的建立

1.1 领航-跟随队形结构模型

为减小跟随者对领航者的过度依赖，避免链式结构造成的跟踪误差累积问题，采用虚拟领航-跟随运动结构，如图1所示。

图1 跟随者与领航者相对运动模型

设领航者的位姿为=[],=[]，跟随机器人的位姿为=[],=[]，由图1可得跟随机器人与领航者的相对位姿方程：

(1)

其中:Δ=-,Δ=-，和为实际运动轨迹，和为理想运动轨迹。

由图1得跟随机器人的运动方程：

(2)

1.2 建立队形数据库

机器人的基本队形主要有一字形、楔形、圆形和三角形等，如图2所示。

图2 队形数据库中的基本队形形状

在每个队形中都至少有一个领航机器人，其余跟随机器人接收领航者传递的消息，形成目标队形。多机器人在执行任务过程中遇到障碍，通常是将队形变化成一字形通过障碍物，不考虑机器人的运动特性、具体环境情况和障碍物的类型，这并非是最优的避障方法。图3给出了两种不同的队形进行避障，其中图3(a)中由原有队形变成一字队形，尽管该队形容易通过当前障碍物，但在一定程度上增加了机器人避障路径长度，收敛队形的时间长，且易发生内部碰撞，队形失真度比较大。而图3(b)中以队形内部角度缩小形成的队形通过障碍物，编队没有改变整体队形结构，只是在不会发生内部碰撞的条件下，对机器人编队队形进行收缩，收缩后的队形顺利通过障碍物，在当前场景下，图3(b)中的避障方法明显要优于图3(a)。

图3 不同队形避障方式

以上是从具体的场景比较两种不同队形的优劣程度，在实际避障过程中，移动机器人整体如何自主地选择适应当前环境约束条件的拓扑结构需提出相对应的避障策略。

目前缺少统一有效的方式来定义多移动机器人编队的队形形状。为了方便描述编队控制中具体领航者-跟随者关系及队形形状信息。下面举例说明：

设有3个机器人参与编队，虚拟领航者表示为L，其位置坐标表示为[],跟随者表示为F(=1,2,3)，常用队形有一字形、三角形，如图4、图5所示。根据机器人数量设定相应的队形数据库。

图4 一字形编队位置示意

图5 三角形编队位置示意

领航者与跟随者坐标如表1和表2所示。

表1 一字形队形各跟随者位置坐标

表2 三角形队形各跟随者位置坐标

通过建立队形数据库，便于领航者迅速做出队形选择，降低计算复杂度，提高决策效率。

2 距离-角度优先级避障策略

充分考虑在队形变换过程中的环境约束，合理选择队形避障方式，分别针对单侧障碍物和双侧障碍物提出基于距离-角度优先级的队形变化避障策略。

图6所示为多移动机器人避障算法流程。机器人编队在向目标点运动的过程中，领航机器人通过自身携带的传感器感知周围环境信息，得到领航机器人与障碍物之间的相对位置，并考虑各个机器人的自身安全距离和通信范围的关系判断当前以何种队形避障最优；跟随者根据领航者的指令进行避障。

图6 多移动机器人避障流程

2.1 单双侧障碍物避障策略

设机器人自身安全距离为半径为，则两个机器人并排运动的最小安全距离为4，3个机器人跟随领航者以三角形队形沿水平方向运动。

(1)单侧障碍物避障策略

单侧障碍物避障示意如图7—图8所示。

图7 躲避上侧障碍物示意

图8 躲避下侧障碍物示意

编队遇到单侧障碍物时，如图7所示，通过比较障碍物与编队的距离和sin+的大小，判断编队是否需要避障；若>sin+，则不需要避障。若≤sin+，则需避障，同时机器人通过传感器判断障碍物，位于前进方向的上侧或下侧，若为上侧，则机器人系统保持初始队形右拐进行避障，若为下侧，则机器人系统保持初始队形左拐前进进行避障；当编队整体通过障碍物后队形收敛到原轨迹水平线继续前行。

(2)双侧障碍物避障策略

当领航机器人检测到双侧障碍物时，如图9所示，如果>，编队中的成员机器人保持现有队形形状即可通过障碍物。若≤，编队无法直接通过障碍物区域，需要将队形切换到另一种状态，保障编队顺利通过。其中=2sin+2。

双侧障碍物属于复杂区域，整体编队需要具体判断以哪种队形避障，若<2，则按单侧避障方案绕行避障；若2<<4，需变为一字队形通过障碍物；若>4时，编队通过减小角度减小队形的宽度，依然按照原队形即可通过障碍物。其中表示障碍物之间的最小间距。

图9 树杈状三角形队形避障示意

通过分析对一般环境中可能出现的单双侧障碍物，考虑机器人自身安全距离及队形整体与障碍物之间距离，提出距离-角度优先级避障策略，即先判断是否需要避障，其次优先考虑距离判断是否需要改变队形，最后决定是否需要改变角度选择合适队形通过障碍物。当编队中最后一个机器人成功通过障碍物后，由领航者机器人发布队形恢复指令，队形中的跟随机器人以各自领航者为参考恢复队形。

2.2 队形快速恢复算法

为提高多机器人的工作效率，当机器人编队整体通过障碍物后需及时恢复队形，沿原轨迹继续前行，设避障后领航者位姿为()=[]，设队形快速恢复函数为

()=(-)e-+

(3)

其中：>0；为初始值即避障后虚拟领航者位置状态量；为()的稳态值即恢复队形后的位置状态量。()按指数快速递减到，可实现队形以指数形式快速恢复。

3 仿真实验及结果分析

为了充分验证文中避障策略的可行性和优越性，在两种不同的障碍物环境中进行仿真。

将3个机器人的初始编队设置为三角形，初始位置分别为=[2 1 0 0]、=[0 1 1 0]、=[0 9 0],理想线速度、角速度分别为=1 m/s、=0.5 rad/s，机器人自身安全距离为0.3 m，最大通信距离为5 m，最大通信角度为120°，仿真结果如图10—图13所示。

图10 躲避单侧障碍物

图11 躲避双侧障碍物

图12 躲避双侧障碍物时实际与理想位置跟踪误差

图13 躲避双侧障碍物时实际与理想速度跟踪误差

图10、图11分别为在单、双侧障碍物环境中机器人群的避障及队形恢复示意，可得：机器人以三角形队形运动遇到单侧障碍物时，能够准确判别出障碍物的方向并控制编队转向安全通过障碍物；遇到双侧障碍物时，充分考虑障碍物与队形的间距和角度，开始缩小队形通过第一个障碍物，然后变为一字形队形通过第二个障碍物，并在完全通过障碍物时迅速在3 s内恢复队形继续前进。

图12为机器人躲避双侧障碍物过程中的位置跟踪误差曲线，当机器人保持队形运动时，在轴方向上的平均绝对误差为0.001 1 m，在轴方向上的平均绝对误差为0.001 5 m，角度跟踪平均绝对误差为0.001 2 rad。当机器人变换队形时，在=30 s时队形超调量最大，在轴上最大超调量为3 m，轴上的为2 m，即队形最大超调距离约为3.606 m<5 m,角度最大超调量为1.2 rad<2π/3，机器人群始终处于通信范围内，没有出现掉队和碰撞现象。图13为机器人躲避双侧障碍物过程中的速度跟踪误差：当机器人保持队形运动时，线速度跟踪平均绝对误差为0.001 2 m/s，角速度跟踪平均绝对误差为0.001 1 rad/s；当机器人变换队形时，考虑到实际情况，转弯时需要减小速度，所以线速度与角速度跟踪均出现了超调，但避障后，能够在5 s内恢复原有速度继续前进。

为了进一步说明该避障策略的高效性，与文献[12]中提出的几何避障控制算法作对比，设置的障碍物和初始队形与图11中保持一致，仿真结果见图14—图16。

图14是文献[12]避障策略仿真轨迹，编队在进行队形变换时，轨迹不平滑，导致队形超调量较大。图15为文献[12]中避障策略下机器人躲避双侧障碍物过程中的位置跟踪误差曲线，当机器人变换队形时，在=30 s时队形超调量最大，轴方向最大超调量为3.4 m，轴上为2.2 m，即队形超调最大距离为4.04 m。图16为文献[12]中避障策略下机器人躲避双侧障碍物过程中的速度跟踪误差，当机器人变换队形时，完成避障后编队需要7 s才能恢复初始队形。

图14 文献[12]中避障策略双侧避障

图15 文献[12]中避障策略与理想位置跟踪误差

图16 文献[12]中避障策略与理想速度跟踪误差

为方便表述，将文中所提避障策略、文献[12]中设计的避障方法分别表述为A算法和B算法。由图11和图14可得：与B算法相比，A算法下编队轨迹相对比较平滑；由图12、13和图15、16可知：当机器人保持队形运动时，与B算法相比，A算法下位置跟踪误差在轴方向上的平均绝对误差、在轴方向上的平均绝对误差以及角度跟踪平均绝对误差分别提高了0.003 m和0.004 m；在线速度和角速度跟踪方面，A算法比B算法分别提高了0.002 m/s和0.004 rad/s；此外，队形恢复时间缩短了28.57%。

以上是对编队避障过程中的跟踪误差进行具体对比分析，证明了A算法的有效性。为了进一步说明A算法完成避障的高效性，现对编队整体避障过程进行综合评价，评价指标函数设计如下：

(1)避障路径函数

当机器人速度一定时，编队是否能够高效完成任务取决于路径的长短。将走过路径的长短作为评价避障性能函数之一，来描述机器人群避障走过路径的长度。避障路径函数为

(4)

其中：表示编队开始避障的初始时刻；表示编队完全通过障碍物的时刻。函数值与避障效率成反比。

(2)队形结构失真函数

切换队形时会引起编队控制系统不稳定，需要编队队形变形量尽可能地小。将队形结构失真作为评价避障性能的函数之一，来描述队形变换与原始队形结构畸变程度。队形结构失真函数为

(5)

其中：表示编队中机器人数量；()表示机器人在性切换队形中位置；(0)表示机器人在初始队形中的位置；，lF为各跟随者与领航者的相对距离。函数值表明队形失真度，值越大系统越不稳定，队形难以恢复。

(3)队形恢复时间

当机器人通过障碍物时，需要恢复队形继续前行完成任务，因此，队形恢复效率也是影响编队整体执行任务效率的影响因素之一。将队形恢复时间作为评价避障的附加技术指标，一方面来描述避障时队形的分散程度，另一方面，编队过程中需要多次变换队形时，就需要多次恢复队形，队形恢复时间从侧面反映出避障的效率。队形恢复时间函数为

(6)

其中：表示编队切换队形的次数；s表示避障后开始恢复队形的时刻；d表示队形恢复结束的时刻。函数值与避障效率成反比。

由表3可得：比B算法相比，路径的长度方面，A算法下的避障路径更短，减少了14.29%；在队形结构畸变程度方面，A算法减小了12.5%；在队形恢复时间上，A算法缩短了28.57%，极大地提高了避障效率。综上，文中编队避障策略效果更佳。

表3 文中方法与文献[12]中方法的指标评价对比

4 结论

针对复杂环境下多移动机器人编队整体避障效率低的问题，提出一种基于距离-角度优先级避障策略。该策略通过领航者机器人携带的传感器获取未知环境中障碍物的具体信息，引导整体编队通过障碍区。相对于传统的队形避障方法，此方法充分考虑编队避障中环境、自身安全和通信范围的约束，先通过整体编队队形的宽度与障碍物可通过区域的比较，选择合适队形，然后各跟随者调整自己与领航者的角度，形成目标队形通过障碍物。该策略充分考虑了当前环境对队形的影响，具有使编队自主切换到当前避障合理队形的良好性能，对于多移动机器人编队避障研究具有一定的理论意义和参考价值。