王钦钊， 程金勇， 李小龙

(陆军装甲兵学院控制工程系， 北京 100072)

近年来，随着人工智能和机器人技术的迅速发展，多机器人的协同技术受到越来越广泛的关注。与单机器人相比，多机器人系统具有更强的工作能力和鲁棒性。多机器人协同在资源探测、安全巡逻、地震救援、军事作战和智能交通等方面都有非常广阔的应用前景[1]。

多机器人编队避障控制，顾名思义就是多个机器人在向目标运动的过程中，既能够保持一定的几何队形，又能适应环境的约束安全避障[2]。目前多机器人的编队控制方法主要有虚拟结构法、人工势场法、基于行为法、领导-跟随法和基于图论法等。其中：领导-跟随法采用链式拓扑结构，跟随者跟踪领航者形成队形，但是该算法没有考虑整体的路径规划和编队的避障功能；人工势场法中机器人在势场力作用下形成队形，但是每种队形都需要定义相应的势场函数，而势场函数设计比较复杂；基于图论法采用一致性理论，使得各机器人能够达到距离的恒定和速度的一致，缺点是机器人之间的相互位置关系不确定，难以得到明确的队形结构。

针对上述方法的不足，笔者提出领导-跟随和人工势场相结合的分层群集编队避障算法。领导层中，领航者进行路径的预先规划和编队导航，以领航者为参考点，通过设定虚拟领航者的位置和数目形成明确的队形和位置关系。跟随层中，设计分布式控制器，跟随机器人通过虚拟领航者的引导向期望的目标位置运动，同时在避障避碰策略和群集一致性控制器的作用下实现编队控制，与常规的一致性方法相比减少了通信量[3]，另外，引入附加势场旋转力对队形控制器进行调节，有利于机器人及时跳出局部极小值和死锁状态[4]。利用机器人编队的势场能和动能构建李雅谱诺夫函数，论证了编队的稳定性。最后，通过仿真试验验证了该方法的有效性。

1 问题描述

1.1 机器人编队群体模型

假设n个机器人在二维平面上运动，将机器人视为质点并采用常用的二阶智能体模型，则机器人i(i=1,2,…,n)在二维平面中的动力学方程为

(1)

式中：qi(t)∈R2，pi(t)∈R2，分别为机器人i在t时刻的位置向量和速度向量；ui(t)∈R2，为机器人i在t时刻的控制输入。

另外,定义机器人群体中虚拟领航者r(r=1,2,…,m)的方程为

(2)

1.2 编队运动整体目标

跟随机器人在控制输入的作用下能够形成稳定的编队跟踪领航者，并在运动过程中既避免与邻近机器人相撞，又要与邻域范围内的机器人保持速度匹配，编队能够保持恒速和恒间距，即满足条件

(3)

定义1：假设n个机器人分布在二维欧式空间内，每个机器人的探测半径为h，以机器人i为圆心，探测半径内的区域为机器人i的邻域，邻域内的其他机器人j(j=1,2,…,n)为i的邻接集Ni，表示为

定义2：同一平面的多机器人用图论法可以方便直观地表示机器人之间的通信拓扑结构,用无向图G(t)=(V,h(t),A)表示系统通信结构，其中V={v1,v2,…,vn},为节点集合，vi代表机器人i节点；eij=(vi,vj)∈h(t),为图的边集，若机器人存在通信关系，则边(vi,vj)存在，同时邻接矩阵A中aij=aji=1，否则aij=aji=0。

2 群集一致性基础理论模型

1986年，REYNOLDS[5]根据自然界中群居鸟类的行为特点,设计了著名的Boids计算机模型，最早提出了模拟动物行为的群集一致性理论。基础理论分为3个部分：1)分离(Separation)，个体避免与群系统中邻近的个体发生碰撞；2)匹配(Alignment)，目标相同的智能体试图与群系统中邻近的个体保持运动速度匹配；3)聚合(Cohesion)，智能体试图与邻近的个体保持接近，并收敛到合适的位置，朝着一个共同的目标运动。根据上述理论，多机器人运动控制模型可简化为

(4)

3 多机器人编队避障控制策略

3.1 群集编队控制算法主体思想

假设n个机器人组成多机器人系统,其组成的群集编队控制策略示意图如图1所示，其中：f1为队形力(f11和f12分别为2个邻接机器人的队形力)；f2为障碍力；f3为领航力；fv为速度耗散力。整体编队采用分层的思想，领航者和虚拟领航者作为导航层，领航者进行路径的预先规划，主要行为是驶向目标和引导编队运动，领航者存储队形结构和局部领航跟随关系的队形信息库，运动过程中由于环境影响或者指令需要变换队形时，以领航者为参考点，调整虚拟领航者的位置和数目，形成期望的编队结构。跟随层中，跟随机器人在领航力、障碍力、队形力的“合力”和群集一致性控制器的作用下，向着各自期望的虚拟领航者的位置移动，该编队中机器人不需要获取全局信息，仅仅依靠群组的邻居局部信息即可实现群集编队和状态同步，另外在编队中增添或减少机器人时，只需在队形点位置增加相应数目的虚拟领航者即可。

图1 群集编队控制策略示意图

3.2 领航者路径规划

领航者采用人工势场[6]方法对路径进行初步规划，实现编队导航。目标对领航者施加的引力和障碍物对领航者施加的斥力形成合力控制领航者的运动。引力和斥力场模型分别为

(5)

(6)

式中：Uatt(X)为引力场函数；X为领航者位置向量；Xg为目标位置向量；ka为引力增益系数；Urep(X)为斥力场函数；kr为斥力增益系数；d为领航者与障碍物相对距离；d0为斥力场作用距离。

引力和斥力均为引力场和斥力场函数的负梯度，表达式分别为

(7)

(8)

领航者受到的合力

F=Fatt+Frep

(9)

决定其下一步的运动方向。

3.3 群集编队避障控制器设计

本节设计分布式控制器，使多机器人在追踪虚拟领航者的过程中避免与障碍物或其他机器人发生碰撞，最终实现多机器人协调协作控制。根据势场理论和REYNOLDS[5]提出的一致性基础理论，第i个跟随机器人的分布式控制器的输入为

(10)

1) 避碰控制

为了实现机器人之间的避碰，引入队形势函数，该函数是关于距离的可微、非负、无界函数。设定Ni为邻接集，机器人i和j之间的期望距离为dij，实际距离为x，w为跟随机器人之间的作用距离。当x>dij时，体现为引力,将机器人之间的距离拉近，直至接近dij；当x

图2 队形势场函数

队形势场函数ψ1和队形力f1定义为

(11)

(12)

式中：k1为增益系数；ψij为队形势函数;e1为机器人i指向机器人j的单位方向向量。

同一编队内的机器人避碰时不仅要考虑到邻接机器人之间的位置关系，同时还要考虑到彼此之间速度的一致性，体现出编队内机器人的运动状态对同一编队内其他机器人的约束作用，对式(12)添加速度耗散力,改进为

(13)

式中：k0为增益系数。

2) 避障控制

为了增加机器人避障过程中的安全性，采用闵可夫斯基(Minkowskisum)方法对障碍物扩张处理为障碍圆[7]。机器感应到障碍物边缘时，在障碍物的边缘使用边界投影方法在圆上产生投影点，以投影点为基础产生虚拟的人工势场源，能够使以最差姿态靠近的机器人成功避障。s为跟随机器人距离障碍物边缘的距离，障碍物的圆心位置矢量为Ok，障碍圆半径为Rk，机器人的位置矢量为qi，投影点的位置矢量为qik，障碍物势场作用距离为R。增益系数k2的大小取决于机器人惯性向量的大小，数值过小不利于安全避障，数值过大则可能超出机器人的性能，投影点位置矢量为

(14)

图3为避障势场函数。

图3 避障势场函数

障碍势场函数和斥力分别为

(15)

(16)

式中：e2为机器人i指向投影点的单位方向向量。

3) 群集编队控制

虚拟领航者相当于跟随机器人的目标，跟随机器人要保持与虚拟领航者的速度、位置的一致性，定义一致性控制器

(17)

式中：c0,c1>0，二者均为增益系数。c0、c1的存在可以非常方便地设置机器人对虚拟领航者的追踪和自身形成队形二者之间的权重。

4) 死锁跳出控制

多机器人采用势场思想进行编队控制时，机器人之间或者机器人和障碍物之间有可能陷入局部极小点，即陷入死锁状态[8]。为解决这个问题，采用附加旋转力[9]的思想对控制器进行改进，如图4所示，附加势场旋转力为

fi=kτn

，

(18)

(19)

图4 势场旋转力示意图

综上可得编队控制器为

(20)

4 稳定性证明

由n个机器人组成的多机器人群体，机器人模型见式(1)，假设系统中各跟随机器人的初始速度不相同，那么机器人在虚拟领航者的引导和群集编队控制器(20)的作用下能够实现个体间速度匹配和间距的恒定，且个体之间不发生碰撞，系统呈现稳定的编队运动。

证明：定义系统中的跟随机器人i的运动学误差方程为

(21)

假设x=[x1x2…xn]T，v=[v1v2…vn]T，u=[u1u2…un]T，计算过程中不考虑势场旋转力，将式(20)、(21)代入式(1)，则跟随机器人模型变换为

(22)

为证明稳定性,取李雅普诺夫的能量函数[10]为

(23)

对其求导得

可知

(24)

与上述结论中机器人总能量小于无穷大相矛盾，可得反证法的假设不成立，跟随机器人之间不会发生碰撞。由此系统的编队稳定性得以证明。

5 仿真试验

试验考虑5个机器人的编队运动，首先设置相应的编队参数：n=5，k0=11，k1=26，k2=12，c0=3，c1=6，k=10。

领航机器人以1 m/s的速度沿y=x正方向作直线运动，5个机器人成前三角编队运动。领航者为leader。跟随机器人分别为R1、R2、R3、R4，与领航者的期望编队距离分别为6、6、12、12 rn。障碍物中心的坐标为(25,35)、(8,17)、(25,14) ，障碍物的半径设置为3 rn，障碍势场作用距离为2 rn，机器人之间的势场力作用范围为9。图5为多机器人编队运动轨迹；图6为跟随机器人与虚拟领航者的位置误差；图7为跟随机器人速度、角速度变化曲线；图8为跟随机器人的相对距离；图9为跟随机器人受到的合力。

图5 多机器人编队运动轨迹

图6 跟随机器人与虚拟领航者的位置误差

图7 跟随机器人速度与角速度变化曲线

图8 跟随机器人的相对距离

图9 跟随机器人受到的合力

从机器人运动轨迹中可以看出：多机器人总体成前三角编队运动，跟随机器人实现了编队避障。编队过程中，机器人的间距始终不为零，说明机器人之间能够体现出避碰效果。进一步分析可知：机器人之间的间距趋向于期望的编队距离。

综上，在本文提出的机器人群集编队避障算法的作用下，多机器人实现了运动过程中的编队控制和避障行为，在避障过程中能够及时调整机器人的运动状态，实现避障避碰和编队过程的统一，仿真试验验证了算法的正确性。

6 结论

利用虚拟领航者和领航者的相对位置关系表

示机器人编队结构，用邻接矩阵表示机器人之间的通信关系，建立队形力、领航力、障碍力模型，用势场法思想对一致性编队集群理论进行改进，新的编队控制律能够使多机器人达到期望的队形，并能适应环境约束成功避障。用李雅普诺夫稳定性理论进行分析，在控制器作用下多机器人能够稳定地编队，最后二维平面的编队形成、队形保持和编队避障的试验结果表明了算法的有效性。由于条件所限，暂时只能在MATLAB中仿真，将算法应用到实际中将是下一步的研究方向。

[1] 夏梁盛. 基于群集的机器人编队控制[J].火力与指挥控制，2013,38(1)：18-22.

[2] 曹建福，凌志浩，高冲,等.基于群集思想的多智能体编队避障算法研究[J].系统仿真学报，2014，26(3)：563-567.

[3] 赵海，刘倩，邵氏亮,等.一种面向多智能体群集的避障算法[J].东北大学学报，2014，35(3)：347-351．

[4] 曲正刚，曹喜滨，张泽旭.人工势场和虚拟领航者相结合的多智能体编队控制[J].哈尔滨工业大学学报，2014,46(5)：2-6.

[5] REYNOLDS C W.Flocks,heirds,and schools:a distributed behavioral model[J].Computer graphics，1987，21(4):25-34.

[6] 陈浩，雷斌，高全杰.基于领导者群体机器人编队导航控制[J].武汉科技大学学报，2016,39(3)：220-223.

[7] 潘无为，姜大鹏，庞永杰,等.人工势场和虚拟结构相结合的多水下机器人编队控制[J].兵工学报，2017,38(2)：326-334.

[8] 陈世明，丁贤达，化俞新,等.大规模机器人群体的分层编队控制算法[J].华中科技大学学报(自然科学版)，2015,42(10)：52-57.

[9] 刘丹.基于拓扑连通性的多智能体系统目标追踪群集控制[D].秦皇岛：燕山大学，2011.

[10] 楼晓春，谢宇.一种基于虚拟力的多机器人编队控制系统[J].中国机械工程，2014，25(24)：3294-3302.