多机器人编队避障算法（1）

---

前言

今天闲的没事，正好是10/24，CSDN有活动，就把之前做的一些多机器人编队避障算法（主要是纯控制避障）梳理小结下( •̀ ω •́ )✧

---

一、基于感知的避障

1.基于感知的Epuck2避障思路(理论)

Ri为人工势场的斥力项，假设在k时刻机器人可得到一定范围内的障碍物坐标Xobs ，障碍物会对机器人 的速度产生一个斥力的影响R：
R的计算公式为：

其中M为障碍物个数，dm为探测距离，在此时刻避障响应R（k）会对机器人的速度控制产生一个影响。

2.基于感知的Epuck2避障实现(现实)

在Epuck2实机避障时，最终的控制输出Ui是线速度和角速度。


其中position_difference和angular_difference是机器人与期望位置/角度的差值。

avoid_delta按照上述公式进行计算。

然后当Epuck2探测到障碍物距离小于0.1m时，计算产生避障项使linear_velocity为负值机器人远离障碍物，并且产生正向或反向的角速度使机器人转向。当与障碍物距离大于0.1m时避障项为0，机器人向期望位置前进。

3.距离传感器结合红外传感器修复避障Bug

前面所说的Epuck2避障思路，但最终呈现出的效果却是机器人在障碍物前徘徊，无法绕过障碍物。
原因是机器人接近障碍物后，距离小于0.1m后产生反方向的速度，使距离拉大。当距离大于0.1m后避障项为0，机器人又受到引力作用向前，进而又使距离缩小，虽然会产生角速度使机器人转向，但只要靠近障碍物距离小于0.1m即产生反方向速度使机器人无法前进，原地徘徊。

解决：没有对人工势场项进行更改，而是通过距离和红外传感器反馈的距离来细化何时添加避障项，进行避障。

如图所示为Epuck2距离传感器(ToF)和8个红外传感器(Prox)的位置，通过代码可以分别获得他们检测到的与障碍物的距离信息(距离传感器的探测距离为2m，红外传感器的探测距离为0.05m)。

距离传感器探测到的距离记为r，8个红外传感器探测到的距离依次记为p0,p1,p2…p7(依图中标号)。

(1)避障项avoid_delta的计算
避障项的计算不变，但触发条件改变，之前通过动捕计算机器人中心位置与障碍物中心位置的距离(是否小于0.1)进行触发。改为由Epuck2前方的1个距离传感器和4个红外传感器触发，如图所示。

程序设置当r小于0.08m或p0，p1，p7，p6返回的探测距离中有一个小于0.04m时，表明机器人的前侧方遇到障碍物，返回测得的最小距离代入公式计算 ，否则将其赋值为0

(2) 机器人线速度的计算

当机器人前侧方遇到障碍物后，计算产生了一个负的避障项avoid_delta，在乘以设定的系数k_avoid，使其与期望位置产生的正向速度叠加，最终产生一个负的线速度，使机器人向远离障碍物方向移动。
当机器人传感器没有检测到障碍物时，避障项avoid_delta为0，机器人根据与期望位置的距离产生正的引力(线速度)，使其朝着期望位置移动。

(3) 机器人角速度的计算
为了模拟出机器人受到(目标点吸引力和障碍物斥力叠加产生的合力)的效果，对机器人的角速度进行以下设计。

3.1)当障碍物位于机器人前进方向的右侧

如图所示当机器人前进方向的右侧出现障碍物时，受到目标位置引力和障碍物位置斥力的叠加，应产生向前进方向左侧的力。

我是这样模拟给这个力的：如图所示当机器人前进方向右侧遇到障碍物时，机器人右侧的两个红外传感器能够检测到与障碍物的距离p0,p1，当他们中有一个的距离小于0.04m时就产生这样一个角速度：

由于检测到障碍物时计算产生的 项为负，前面加负号产生一个正的角速度，使机器人逆时针旋转，并且为了使机器人不因 项产生负的线速度使机器人后退，设置当机器人角速度绝对值大于0时(即机器人旋转时)，机器人线速度赋值为0。

如图所示，当转到机器人右侧的两个红外传感器检测到与障碍物的距离都大于0.04m时， avoid_delta项变为0，机器人的角速度为0，机器人停止旋转。但并没有结束，为了使机器人朝目标位置移动，此时会判断当前机器人角度与期望航向角的差值，若差值大于设定的阈值，即产生角速度使差值减小。

这样又会使机器人产生负的角速度，转向障碍物，以至于机器人在原地来回偏转，我通过位于机器人轮子旁的红外传感器p3来解决这个问题，当机器人位于图3.4所示的位置时，判断红外传感器p3返回的距离信息，如果小于0.045m，把角速度设置为0，这样当机器人的角速度为0，当机器人角速度为0时允许产生线速度，并且机器人前侧的距离和红外传感器测得的障碍物距离大于设定值， avoid_delta项为0，产生正的线速度使机器人前进。当机器人前进到如图所示位置时。

红外传感器测得的距离p3大于设定值0.045m，此时再根据当前角度与期望航向角的差值计算产生负的角速度，使机器人顺时针偏转绕开障碍物朝着目标位置前进。

3.2)当障碍物位于机器人前进方向的左侧
基本思路相同，在机器人左侧红外传感器检测到p7,p6小于0.04m时，产生一个负的角速度使机器人顺时针旋转，绕过障碍物。

3.3)其他情况
除了这两种情况，还有可能是恰好障碍物位于机器人前进方向的正中间(只有距离传感器测得的距离r小于设定值)，或者是遇到墙壁障碍物(前侧所有障碍物检测的距离都小于设定值)。此时不论机器人朝那个方向旋转都可以。

4.问题

虽然解决了机器人的避障问题，但理论方面并没有改进，只是在实机实验的过程中，遇到问题解决问题，相当于人为的把所有可预见的情况，设置了相应的机器人应对办法，好处是直接，坏处是泛化性差，可能有一些没有预见到的复杂情况，只用不断测试完善才能确保机器人的避障效果。并且根据机器人与障碍物距离计算产生的避障项作用不大不能很好体现出其优势。

5.代码逻辑图

距离传感器(ToF)测得的距离为r，红外传感器顺时针旋转依次测得距离依次为p1,p2,p3….p7，代码逻辑如下：

基于感知的避障

二、基于人工势场力的避障

1.基于人工势场的Epuck2避障思路(理论)

前一个方法虽然通过人工势场公式给机器人加了引力斥力，但力是根据机器人与目标点/障碍物间的距离作用于机器人的线速度和角速度。由于通过距离远近来改变力的大小而影响速度，故没办法控制力的方向。为能够实现当机器人陷入局部最优时给机器人速度方向垂直的力，引入方向向量设计如下：

2.基于人工势场力的Epuck2避障实现(现实)

以机器人前方方向左侧遇到障碍物进行受力分析，并说明代码实现思路：

基于人工势场力的避障

三、两种避障方法的对比分析

首先是两种避障方法的路径结果对比，设置相同的起始位置(0.229,0.367)，目标位置设为(2,0.367)，在前进方向设置障碍物如图所示。

分别采用两种不同的避障策略，记录各策略三次避障的路径结果如下：首先采用基于传感器感知的避障方法，路径结果记录如下。

(1) 基于传感器感知的避障



(2) 基于人工势场加速度方向垂直力的避障方法

---

总结

这两种方法都存在较大的问题，也简单的尝试了将这两种方法应用于多机器人编队中，但结果远远低于预期，两种方法存在相同的缺陷，在编队行进过程中一旦遇到障碍物，编队就离散后面也很难继续保持编队前进。

目前也看了一些其他的避障方法，发现做出很好实机效果的多机编队，绝大多数采用基于轨迹引导的避障方法(代表EGO-Swarm多无人机编队)，这种方法具有很大的优势，本文采用的两种避障方法遇到障碍物不断试探着去通过，而基于轨迹引导的避障具有预见性，在遇到障碍物前就已经开始规划进而调整整个编队中每个个体的控制策略，确保在避障的同时维持较好的编队效果。

写好多字了，就这样吧，还有一个基于二次规划的多机编队避障，下次再写o(￣▽￣)ブ