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

---

前言

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

---

一、基于感知的避障

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的计算
避障项的计算不变，但触发条件改变，之前通过动捕计算机器人中心位置与障碍物中心位置的距离(是