碰撞锥检测改进的多智能体避障算法

许元云，何 明，刘锦涛，周 波，杨 铖

中国人民解放军陆军工程大学 指挥控制工程学院，南京 210007

1 引言

动物集群经常表现出有趣的集体行为[1]：它们在有限时间内建立有序的结构而不会发生碰撞，也可以对环境变化迅速做出反应，例如突然出现捕食者或障碍物。这些生物行为非常复杂，相关研究人员通过引入多智能体，并用抽象的数学形式模拟这些动物行为。多智能体不光用于解释这些行为，同时由于其扩展性强、适应性强、复杂性低等优点，广泛应用于航天航空控制、交通控制和能源控制等领域[2]。

通常，复杂网络可能包含大量节点，通过在网络内的所有节点上添加控制器来同步复杂网络既不实际也不经济，解决该问题的有效方法是仅在一小部分网络节点上添加控制器，这种方法称为牵制控制。由于牵制控制的优越性，文献[3-4]就多智能体牵制控制进行了深入的研究。

避障是多智能体能够适应复杂环境并顺利完成任务的必要条件之一。目前多智能体避障算法包括人工势力场法[5-6]、模糊控制法[7]、几何法[8]和强化学习[9]等，人工势场法在障碍物和智能体之间建立排斥力场来避免智能体与障碍物的碰撞，这种方法简单灵活，通常比其他避障方法更高效[10]，最早由Khatib[5]引入到机器人避障中。Olfati-Saber 算法[6]中的β-智能体是一个典型的基于人工势力场的多智能体避障算法，这个算法有两个部分构成，距离项部分是构建排斥力场来避免智能体与障碍物之间的碰撞，速度项部分是引导智能体沿着障碍物切线方向运动。Sakai等人[11]不区分障碍物和智能体，将感知的对象全都视为障碍物，使用β-智能体完成了蜂拥控制和避障，刘婵等人[12]介绍了Olfati-Saber 算法，并在多障碍物环境中进行了实验仿真。同其他人工势力场法一样，β-智能体也存在局部最小值问题和非最佳路径问题，后续的改进主要集中在这两点。在避免陷入最小极值点方面，Li等人[13]提出了一种新颖的障碍物几何表示规则以填充障碍物的非凸区域，高晶英[14]构建了两个β-智能体，其中一个与障碍物平面切线方向垂直，避免陷入最小值，Wu等人[15]将使用流体力场替代了智能体与障碍物之间的人工势力场。为解决非最佳路径问题，Hu等人[16]结合空间几何知识，确定圆锥体的最短路径为β-智能体轨迹从而实现圆锥体障碍物的快速避障，Wang 等人[17]结合几何法的碰撞锥检测方法[18-19]，根据α-智能体、β-智能体和γ-智能体三者的速度和位置关系确定单个智能体的速度大小和方向，使得智能体能够快速通过障碍物，但注意到此方法未涉及智能体之间的避免碰撞问题，Zhao等人[20]通过智能体与其邻接节点交互障碍物信息，将局部避障算法改进为全局避障算法，有效地避免了局部最小值问题，同时更好地实现了多智能体的一致性，但算法复杂度有所增加。

本文基于Olfati-Saber 算法提出了一种多智能体自适应避障控制算法，算法基于角度比较的碰撞锥检测方法引出智能体的避障系数，并结合牵制控制输入完成多智能体的快速避障。证明了在本算法作用下所有智能体最终会避开障碍，避免碰撞并最终达到一致。仿真实验表明在圆形障碍物和墙形障碍物环境中基于本算法多智能体避障所用时间较少，验证了本算法的有效性。

2 多智能体建模和障碍物表示

考虑m个智能体工作在n维欧几里德空间上，它们的动力学方程可以写为：

其中，qi,pi,ui∈ℝn表示多智能体i的位置向量、速度向量和加速度向量。

t时刻多智能体构成的无向图G(t)由节点和边组成，其中节点集合表示为V={1 ,2,…,m} ，边的集合表示为设r是α-智能体的感知半径，期望距离为dα，则多智能体的邻域为：

参照文献[6]，定义β-智能体构成的集合为V′={1 ′,2′,…,m′} ，则β-智能体邻域为：

其中，r′是α-智能体感知β-智能体的距离，期望距离为dβ，这里假设r=r′。

圆形障碍物的β-智能体如图1（a）所示，其数学定义为：

其中，yk,Rk分别是障碍物的圆心位置和半径；μ=。

墙形障碍物的β-智能体如图1（b）所示，其数学定义为：

其中，yk是一个固定点，这里假设为墙形障碍物线段的中心，ak是墙形障碍物的单位法向量。

图1 β-智能体示意图

3 自适应快速避障控制算法

3.1 问题提出与算法设计

一个基于Olfati-Saber算法的避障实例如图2所示，可以看出当整个网络经过半圆后时在β-智能体的速度项作用下仍然绕着圆运动，这增加了智能体绕过障碍物的时间，降低了多智能体的避障效率。为避免这类现象的发生，提出一种自适应避障系数gi(t),gi(t)在智能体遇到障碍物以后调节智能体是否受到β-智能体速度项的影响，再结合牵制控制输入完成多智能体的避障。

图2 一个多智能体避障实例

3.2 避障系数

避障系数gi(t)的取值根据碰撞锥检测方法来确定，本文通过角度比较来完成碰撞锥检测。当智能体的速度和碰撞锥相交时，首要任务是避开障碍物，需要β-智能体速度项的引导；当智能体运动到速度方向和碰撞锥不相交时，需要考虑多智能体的一致性问题，而β-智能体速度项与智能体的平均速度、虚拟领导者速度都无关，此时可以通过调节gi(t)排除β-智能体速度项的影响。

圆形障碍物环境中的碰撞锥如图3（a）红色区域所示，由对称性θ角可表示为：

当智能体感知到墙形障碍物时，由于墙形障碍物边缘的存在，墙形障碍物环境中的碰撞锥是可能是三角和长方形、两个三角形或者两个长方体的组合，这取决于智能体i能否感知到障碍物边缘。如图3（b）所示，设点O1和O2是墙型障碍物的两个端点，则θ角可表示为：

其中，j=1,2。

图3 单个障碍物环境中智能体的碰撞锥

智能体速度pi和智能体指向圆心的向量的夹角φ满足：

避障系数gi(t)为：

3.3 算法设计和一致性证明

结合文献[6]和避障系数gi(t)，定义ui(t)如下：

其中，(t)实现多智能体聚合、分离和速度对齐三个规则，(t)用于避障，(t)接收虚拟领导者信息，数学定义为：

其中，qγ(t),pγ(t)∈ℝn代表虚拟领导者的位置向量和速度向量，这里虚拟领导者为常速虚拟领导者；gi(t)只作用于(t)的速度项，(t)的位置项用于保持障碍物与智能体的距离；hi(t)值的选取采用度中心性选择，具体选择方法同文献[4]；势能方程ψα、ψβ和速度交互矩阵{aij}、{ai,k} 的定义同文献[6]。

整个避障控制算法流程如图4 所示，其中Δt是时间步长，一般较小，T是算法终止时间，一般根据终止条件来确定合适的值。

图4 避障控制算法流程图

定理1多智能体运动方程如公式（1）所示，控制输入如公式（10）所示，初始能量Q0是一个有限值，则在圆形障碍物和墙形障碍物中所有智能体最终会避开障碍，避免碰撞并趋于一致。

证明多智能体的总能量由总势能和总动能构成，设q˜i(t)=qi(t)-qγ(t),p˜i(t)=pi(t)-pγ,q˜ij(t)=qi(t)-qj(t),根据公式（1）、（10），总能量Q(t)为：

其中：

Q(t)对时间t求导得到：

代入公式（10）～（13），化简得到：

初始能量Q0是一个有限值，由公式（14）、（15）可知，均是有限量，由此可以推得也是有限量。根据LaSalle 定理，多智能体最终会趋于的一致性状态。

由公式（17）可得Q˙(t)=0 的充分必要条件为：

即对于所有的智能体都有：

由于圆形障碍物的β-智能体速度一直变化，墙形障碍物β-智能体方向与γ-智能体方向不同（如果相同，则智能体不会碰到障碍物），Q˙(t)=0 成立的充分必要条件是gi(t)≡0，即所有的智能体都不在障碍物环境的危险区域中。

由pj(t)=pi(t)，则qj(t)-qi(t)为一个恒定的值，由公式（20）可知qi,k(t)-qi(t)也为一个恒定的值，可以得到pi,k(t)=pi(t)，从上文可知，β-智能体沿着障碍物表面移动，速度不会始终与虚拟领导者的速度保持一致，故所有智能体不会感知到障碍物，即最终避开了障碍物。

初始能量Q0是一个有限值，由文献[3]，多智能体之间不会发生碰撞。

4 实验结果与分析

本实验研究多智能体在常速虚拟领导者的作用下避开圆形障碍物和墙型障碍物。

4.1 圆形障碍物的避障实验

多智能体由区域区间为[0 ,50]×[0 , 50]、速度区间为[-1,1]×[-1,1]随机生成的10个智能体构成，通信距离r=r′=6，理想距离dα=dβ=5。

一组多智能体如图5（a）所示，其中蓝点和红色箭头分别表示智能体位置和速度，黑圈表示该节点为牵制节点，红色五角星表示虚拟领导者，位置向量为(2 5,25 )，速度向量为(0.5,0.5 )，圆形障碍物的半径为20，圆心位于(1 00,100 )，采样时间 Δt=0.1 s。图5（b）为多智能体在[1 0 0 s,350 s]时间段内的运动轨迹，可以看出多智能体最终成功避开圆形障碍物，并且在绕过了半圆后没有像图1一样绕着圆周运动，而是朝着虚拟领导者的方向运动。

图5 一组多智能体自适应避障实验

图6 多智能体速度差异

图7是避障过程中智能体与障碍物的距离差异，其中未遇到障碍物时设置距离为6。从图7 可以看出，多智能体在避障过程中只有5个智能体感知到障碍物，遇到障碍物时这些智能体在输入控制的作用下避免了与障碍物的碰撞，同时其他智能体通过智能体之间的相互作用绕开了障碍物。

图7 多智能体与障碍物的距离差异

随机选取20组智能体开展避障实验。设多智能体避障所用时间为多智能体遇到障碍物到所有智能体离开障碍物的时间间隔。图8 是Olfati-Saber 算法和本文所提算法避障所用时间的比较，可以看出在圆形障碍物中本文所提算法避障所用时间较少，验证了算法的有效性。

图8 圆形障碍物中两种算法避障所用时间的比较

4.2 墙形障碍物的避障实验

多智能体和虚拟领导者的设置同3.1 节，墙型障碍物的中点为(1 00,100 )，长度为20，法向量为(3,2)（未与虚拟领导者的速度平行是避免多智能体陷入最小值）。图9 为一组多智能体在时间段[1 0 0 s,235 s]的轨迹，可以看出图9（a）采用Olfati-Saber算法部分智能体在235 s未通过障碍物，而图9（b）采用本文所提算法所有智能体都通过了障碍物。

图9 墙形障碍物中两种算法下多智能体的运动轨迹

随机选取20组多智能体开展避障实验。从图10可以在看出在墙形障碍物中本文所提算法避障所用时间较少，验证了算法的有效性。

图10 圆形障碍物中两种算法避障所用时间的比较

5 结束语

本文提出了一种多智能体避障控制算法，本算法由避障系数和牵制控制输入构成，能够使多智能体避开障碍，避免碰撞并最终达到一致。通过在圆形和墙形障碍物中的多智能体避障实验验证了该算法能够快速避障。下一步将主要就非光滑障碍物、多个障碍物和运动障碍物的避障问题以及避障算法的局部最小值问题进行深入研究。