多机器人编队人工势场法协同避障算法原理及实现

多机器人协同编队需要将理论和实践紧密地结合起来，其应用包括编队队形生成、保持、变换和路径规划与避障等等都是基于图论的理论基础完成的。

详细请参考多机器人编队（一）多机器人协同编队算法原理及实现

自主避障功能是机器人编队在各种环境中保持自身安全的重要功能，在编队的基础上加入避障的功能，机器人可扫描到一定范围内的障碍物（包括其他机器人），在即将与之发生冲突时提前规避冲突，以保证自身的安全性，同时需要在避障的同时尽可能地保持队形，选择最优的避障路线以使障碍物对编队稳定性的影响降至最低。

避障算法原理

机器人在实际运动过程中，会借助自身传感器（如激光雷达）对周围环境进行扫描检测，如下图所示

假设在k时刻机器人可扫描到一定范围内的障碍物坐标$x_{obs}$（包括其他机器人），障碍物会对机器人$j$的速度产生一个斥力影响$R$，$R$满足：
$$R_j(k)=\sum _{l=1}^M\alpha (x_j(k)-x_{obs}^l)$$
其中$M$为障碍物个数,$\alpha =\frac{\frac{1}{d_j(k)}-\frac{1}{d_M}}{\delta \cdot d}$,$d_j(k)=\Vert x_R-x_j(k)\Vert$，$d_M$为探测距离，$\delta$为一常数，在此时刻避障响应$R(k)$会对机器人的速度控制产生一个影响，此时编队中领航者控制模型如下$$u_N(k)=m+k\cdot D(k)+\sum _{i\in N_i}{a}_{Ni}{r}_{Ni}(k)+\beta \cdot R_N(k)$$其中$\beta$为一常数，而跟随者的控制算法为$$u_i(k)=\epsilon \sum _{j\in N_i}{a}_{ij}(x_j(k)-x_i(k)-r_{ij}(k))+\beta \cdot R_i(k)$$这个影响使得机器人在最大限度保持原有编队的基础上能够有效躲避障碍物，并且会随着障碍物距离的变近而变大，从而保证机器人的安全。

避障算法仿真

在前文编队一致性算法的仿真上（ROS及SLAM进阶教程（八）多机器人协同编队算法原理及实现）加入上述避障算法，在编队的任务路径上设置障碍物，使障碍物干扰机器人的正常任务，观察机器人编队的运动结果，目的是使编队在躲避障碍物的同时尽可能保持编队稳定性。
MATLAB仿真结果如下所示

从上图可以看出，障碍物（即图中叉号所在）对编队产生了影响，可以看出编队在安全范围内检测到了障碍物后进行避障运算躲开了障碍物，但很快又恢复稳定，编队能继续保持稳定地执行任务。这种避障方法对于编队协同算法的效果受其实时恢复的影响。

MATLAB仿真源码详见多机器人编队及避障算法.zip
ROS下python源码详见
Ros-Formation-and-Obstacle-Avoidance

以上是关于多机器协同编队的人工势场法算法原理讲解。
码字不易，喜欢的话请点赞收藏关注哦，您的支持是博主最大的动力。

博主有两年多ROS的使用经验，目前仍在不停研究中。本系列多机器人编队将结合论文讲解多机器人编队的算法原理、稳定性分析等研究领域，持续不断更新中。如果大家有相关问题或发现作者漏洞欢迎私戳，同时欢迎关注收藏。
同时欢迎关注博主Git：
https://github.com/LiHongbo97