基于状态切换的分布式多机器人编队控制

卫恒， 吕强， 刘扬， 林辉灿， 梁冰

(1.陆军装甲兵学院 兵器与控制系, 北京 100072; 2.空军航空大学 信息系, 吉林 长春 130022;3.江西理工大学 信息工程学院, 江西 赣州 341000)

0 引言

近年来，随着计算机应用技术和数字信号处理技术的不断发展，使得微处理芯片的功能越来越强、处理速度越来越快，同时自动控制技术、网络通信技术的日趋发展使得微型自主移动机器人受到广泛关注[1]。多移动机器人系统比单个移动机器人具有更好的稳定性、更强的容错性和更高的工作效率，在侦查、巡逻、救援和环境勘测等领域都有很好的应用前景和研究价值。多移动机器人编队控制是多移动机器人系统的基础，已经成为目前机器人领域研究的热点[2]。

多机器人编队算法设计过程中，需要考虑机器人模型、外部环境干扰、算法控制精度以及不同队形可控性等诸多问题[3]。传统的多机器人编队控制算法主要有Leader-Follower算法[4]、基于行为的算法[5]、虚拟结构法[6]、人工势场法等。基于Leader-Follower算法的基本思想是在多机器人系统中选择一个机器人作为Leader，其他机器人为Follower，通过调节Leader与Follower之间的角度和距离来控制编队队形。文献[7]提出了距离- 角度与距离- 距离两种编队控制算法，距离- 角度算法通过控制Leader与Follower之间的距离和角度进行编队控制，距离- 距离算法则通过Follower与相邻机器人之间的距离来保持队形。文献[8]为了防止机器人之间出现碰撞，提出了基于虚拟引力的改进型距离- 角度算法与距离- 距离算法。Leader-Follower算法具有灵活的运动策略、良好的扩展性，但是算法中Leader与Follower之间不能形成稳定、有效的反馈，因此Follower的跟踪误差会随着环境的干扰不断扩大，且当Leader出现故障时整个多机器人系统也会无法工作。基于行为的算法其基本思想是为多机器人系统中的每个机器人设计基本的行为准则，例如避障、避碰、向目标运动、保持队形等行为。该算法可以有效地降低整个多机器人系统编队控制算法的复杂度，但是其对传感器探测能力和机器人之间的通信能力要求较高，且在运行过程中无法对机器人的行为进行准确描述，难以保证系统的鲁棒性。虚拟结构法最早由Lewis等[9]提出，其基本思想是将整个多机器人系统看成一个刚性整体，将各机器人看作是刚体上的某个点。该算法的优点是便于设计多机器人系统的编队行为，队形保持率高，但由于受到刚性结构的约束，其在避障、队形变换等方面缺乏灵活性。人工势场法的基本思想是在机器人周围假设具有类似电磁场的引力和斥力，通过设定适当的势能函数对多机器人进行编队控制。该算法结构简单、易于设计，能够有效地完成避碰和避障，但在队形保持时容易受到干扰，不易进行精确编队控制，若进行队形变换则需要重新设定势能函数，缺乏灵活性[7]。

近年来，切换系统逐渐受到学者们的关注，该系统是指一类具有模态切换且在指定范围内能够按照随机性或确定性规则改变系统动态模式的一类系统[11]。由于许多工程系统在实际运行中，需要根据环境变化而表现出不同的模态，切换系统在工程实践中具有十分重要的作用[112-12]。同时，随着多机器人系统机器人个数的增加以及单个机器人数据处理能力的不断提升，多机器人系统的协同控制算法受到广泛关注，学者们研究了多种分布式控制算法来解决从状态估计、环境监测到集体材料处理等各种各样的任务。分布式多机器人系统能充分利用机器人的数据处理资源，分担中央处理机的压力，在灵活性、容错性方面具有很大优势[13]。

本文基于上述传统编队控制算法的优缺点及发展趋势，考虑在不改变算法基本框架的前提下将各个算法的优点有效结合，从而增加编队控制算法的灵活性和适应性，将切换系统的思想引入多机器人编队控制中，提出了基于状态切换的分布式编队控制策略。同时，基于超宽带(UWB)测距构建了多机器人系统之间的队形反馈机制，在加强机器人之间联系的同时降低了编队控制算法的复杂度和对机器人数据处理能力的要求。最后依托试验室搭建的多机器人平台进行了试验，验证了编队控制策略的有效性和稳定性。

1 预先知识准备

1.1 3轮全向机器人建模

本文采用的机器人为3轮全向机器人，其坐标系如图1所示。图1中，Oxy为全局坐标系；OBxByB为机体坐标系，OB为机器人质心，xB为机器人正面朝向，yB朝向垂直于xB，θ为机体坐标系与全局坐标系的夹角；β为机器人xB与机器人质心OB与O点连线的夹角；φ为相邻两个轮子轴的夹角,φ=120°；L为质心OB到轮子的距离；v1、v2、v3为3轮全向机器人轮子的线速度；f1、f2、f3为3个电机提供的驱动力。

假设vx、vy为机器人在全局坐标系下x轴和y轴的速度分量，ω为机器人的自转角速度，以机器人质心来代替机器人在全局坐标系下的位置，则机器人坐标为(x,y,θ)，二者之间关系可以表示为

(1)

全向机器人3个轮子的线速度可以表示为

(2)

联立(1)式和(2)式，得3轮全向机器人的运动学方程为

(3)

假设机器人质量为m，机器人绕自身中心旋转的转动惯量为I，全向轮半径为R，每个电机电压为U1、U2、U3，电机的电枢电阻为Ra，电机转矩常数为Kt. 令Fx为机器人在x轴方向上所受合力，Fy为机器人在y轴方向上所受合力，M为机器人所受力矩，由牛顿定律可得

(4)

由(4)式可得

(5)

(6)

联立(5)式、(6)式，得3轮全向机器人的动力学方程为

(7)

1.2 图论基本概念

机器人之间的通信拓扑结构通过有向图G={V,E}完成，其中，V为节点集合，每一个机器人代表1个节点；E为边集合，代表机器人之间的连接状态。有向图通过拉普拉斯矩阵表示，即L=D-A，其中D为度矩阵，表示每一个节点都有几个节点与其相邻；A为邻接矩阵，表示节点之间的邻接关系。设一共有n个机器人，i、j分别表示第i和第j个机器人，则拉普拉斯矩阵定位如下：

(8)

考虑到机器人之间通信拓扑结构会因环境不同而变化，本文设计了权重矩阵B=[b10,b20,…,bij]表示机器人之间的通信连接关系，定义矩阵H=L+B表示多机器人之间的通信拓扑关系。本文算法中，设定机器人之间的通信是双向的，因此图G是无向图，B是对称正定矩阵。由拉普拉斯矩阵性质可知，L也是对称正定矩阵，因此矩阵H也是对称正定矩阵。

2 基于状态切换的分布式多机器人编队控制策略

多机器人编队控制中，学者们已经提出了大量基于不同环境下对各种算法的改进方法，但是并没有任何一种算法能较好地适应各种环境。本文将切换系统的思想引入多机器人编队控制算法中，针对不同的任务需要和机器人状态，采用相应的多机器人编队控制算法，在不影响稳定性的前提下，增加了系统的灵活性和适应性。首先分析了多机器人编队控制中机器人的状态和可能遇到的环境，设计了基于复合信息的状态切换触发策略；其次设计了基于UWB测距的队形反馈机制，通过其保持系统队形，并为触发机制实时提供系统队形状态信息；最后提出了分布式状态估计算法，充分利用机器人自身的数据处理能力，降低中央处理器的负担，并为触发机制提供机器人的运行状态。总体设计框图如图2所示。

2.1 基于复合信息的状态切换触发策略

首先，考虑了多机器人编队控制中针对任务需要的不同，机器人采取不同队形和运行状态时的情况。依据第1节所述各算法的优缺点匹配各状态所需算法，编队行为主要分为4种：1)组建编队，是指机器人从任意位姿按照任务需要组建成期望的队形，为了提高组建编队的效率，一般采用基于行为的算法来完成编队组建。2)编队行进，是指多机器人系统依据任务目标按照组建的编队行进，由于编队行进过程中机器人之间的拓扑结构已经固定，为了提高编队行进的精度，一般采用虚拟结构法或Leader-Follower算法进行编队控制。3)队形变换，是指为了适应环境或根据任务需要对多机器人编队队形进行动态变换，由于虚拟结构法相对于其他算法在编队变换时只需要动态设计虚拟质点矩阵，因此采用该算法进行编队控制。4)编队避障，是指在编队行进过程中，在行进路线上出现障碍物时多机器人系统采取的避让行为。本文将避障情况分为2类：第1类是强制保持队形避障，由于某些任务需要，多机器人系统在避障时仍然需要保持队形，此时采用虚拟结构法或Leader-Follower保持队形，采用人工势场法进行避障；第2类是采用最有效率的方式完成避障，此时采用基于行为的方法进行避障，避障后再重新完成编队组建。

其次，考虑了多机器人编队控制中，机器人因为不同的队形，采用不同通信拓扑结构时的情况。拓扑结构主要分为2类：第1类是链状拓扑结构，如图3所示，此时采用Leader-Follower算法来进行编队控制；第2类是环状拓扑结构，如图4所示，此时采用虚拟结构法来进行编队控制。

最后，考虑了环境对编队控制的影响，所处环境主要分为2类：第1类是在编队行进中出现障碍物，一般采用基于行为的算法进行编队避障；第2类主要是针对特定环境下的编队队形，例如采用虚拟结构法行进遇到狭长的通道，此时需要进行两次状态切换：第1次是队形变换，组建适用于该环境的狭长队形；第2次是依据队形重新组建通信拓扑结构，将编队控制算法由虚拟结构法切换为Leader-Follower算法。

控制策略设计中，采用基于任务驱动、自上而下的设计方案，首先确立切换系统模型，设计基于复合信息的状态切换机制，然后以模块化方式设计机器人控制部分。控制部分分为内环控制和外环控制。外环控制为3种编队控制算法模块：Leader-Follower算法模块、虚拟结构算法模块、基于行为的算法模块。内环控制主要为移动机器人底层控制驱动算法。同时，因为在多机器人避碰方面，人工势场法具有结构简单、易于与其他编队算法融合的特点，所以将人工势场算法模块也写入机器人底层控制驱动算法中[7]。具体控制策略如下：

设切换系统为

(9)

式中：x(t)为状态变量；算法中选用常值切换规则，σ(t){1,2,3}；p1、p2、p3分别代表3种算法模块下的子系统。因为在切换系统中采用模块化设计各个子系统，且已经将人工势场法嵌入底层驱动程序，用于编队中各个机器人之间的实时避碰，不需要考虑状态切换时可能对系统产生的不稳定情况，所以采用阶跃式切换策略，避免对各子系统之间连接的复杂设计，即采用常值切换规则。

基于复合信息的状态切换机制，以多机器人系统编队实时矩阵S′与机器人状态为判断依据，其中编队实时矩阵由每个机器人与其他机器人之间的距离以及基于自身机体坐标系与其他机器人的夹角构成。由于构建的切换系统为事件驱动，机器人编队矩阵S由任务发布或在运行中由环境突变触发改变。

设有一个由N个机器人组成的多机器人系统，通信拓扑结构用H=L+B表示，qi=(xi,yi,θi,vi)T表示第i个机器人在t时刻的状态，r表示机器人圆形邻域的半径，第i个机器人的邻域Ni(t)={j|‖xj(t)-xi(t)‖≤r}，nr表示在半径为r的邻域内，共有nr个机器人与机器人i相邻，则

(10)

依据1.2节分析，系统间连接关系由L确定，B为权重矩阵，因此机器人i的邻接关系为

F=I+D-1(I+L)，

(11)

由此可得

(12)

从而得到差值矩阵为

(13)

设定指标权重矩阵k=(k1,k2,k3,k4)T，依据环境及小车状态分别对di中的元素进行加权，从而对状态切换机制进行调节。设定阈值矩阵为d0，当kdi>d0时进行状态切换，当kdi≤d0时保持状态不变。

2.2 基于UWB测距的队形反馈机制

基于复合信息的状态切换机制中，编队实时矩阵S′由基于UWB测距的队形反馈机制给出，通过与任务发布的编队矩阵S进行比较，对多机器人系统状态进行切换触发。考虑到UWB测距系统性价比高、能耗低，且UWB数据结构简单，利于在微型移动机器人上进行实时处理，同时依托试验室在UWB室内定位方面已经做的工作，搭建了基于UWB测距的多移动机器人平台，因此本文设计了基于UWB测距的队形反馈机制。算法原理如图5所示。图5中，A、B两点为1号机器人上的两个UWB模块，D为机器人的质心，C为其他机器人上的UWB模块，l0为基线，l为1号机器人到2号机器人垂直距离，θ0为1号机器人与2号机器人朝向之间的夹角，θ1、θ2、l1、l2为计算过程的中间变量。

由图5可得

(14)

(15)

(16)

l=bsinα,

(17)

(18)

由(17)式和(18)式即可得到多机器人系统中各个机器人之间的距离及夹角，从而对整个编队队形进行反馈。(14)式～(18)式是当前方机器人在后方机器人逆时针方向时解得的二者之间姿态角；当前方机器人在后方机器人顺时针方向时，(16)式和(17)式可变为

(19)

l=asinβ.

(20)

(14)式～(20)式为基于UWB测距的队形反馈机制的数字推导过程，当UWB模块≥3时，构建的多机器人系统即可完成协同定位，每个机器人都可以将自身位置相对于其他机器人位置解算出来，从而组建编队队形，并对整个编队队形起到保持、反馈作用。算法流程图如图6所示。

2.3 分布式状态估计算法

考虑到Leader-Follower算法、虚拟结构法及基于行为的算法都需要对跟随点状态进行估计，本节提出分布式状态估计算法，依照通信拓扑结构，仅利用有限的局部信息对跟随点的位置坐标、方向以及线速度等状态量进行估计。

假设跟随点的状态变量q0为

q0=(x0,y0,θ0,v0)T，

(21)

第i个机器人对虚拟跟随点状态变量的估计值li为

li=(xi0,yi0,θi0,vi0)T，

(22)

机器人i与其相邻机器人的状态估计误差为

(23)

设计如下估计率：

(24)

定理1当至少有一个机器人可以直接由中央处理机得到其虚拟跟随点状态，即bi0>0时，li收敛于q0.

证明将(23)式对ei关于时间求导，得

(25)

(26)

3 试验结果及分析

为了对编队控制算法进行验证，基于试验室构建的多机器人网络架构，以自行设计的3轮全向机器人和UWB测距模块为载体，构建了多移动机器人研究平台[15-16](见图7)，两片UWB测距模块之间的测距误差为±7 cm. 在长4 m×宽5 m的室内环境分别进行多机器人编队队形变换、编队行进、编队避障试验，验证算法的有效性和稳定性。

3.1 试验1：多机器人编队队形变换试验

将4辆全向机器人置于任意位姿。试验步骤如下：1)通过人工势场法组建第1个队形；2)通过中央处理机下达队形变换命令，触发状态切换机制，基于虚拟结构法完成第2个和第3个队形变换。试验中设定编队距离为30 cm，避碰距离为15 cm，试验结果如图8、图9所示，试验视频地址为https:∥v.youku.com/v_show/id_XMzY2MjkxMTk0MA==.html?spm=a2hzp.8244740.0.0.

UWB定位系统采集到的试验1数据如图9所示。

由试验1的结果可知，在多机器人编队队形变换试验中，队形内每个机器人的误差在±10 cm范围，机器人能够按照任务需要完成编队队形变换，同时在队形变换中实时避碰，且在避碰后能继续完成相应任务，验证了基于复合信息的状态切换机制和基于UWB测距的队形反馈机制的有效性和稳定性。

3.2 试验2：多机器人编队行进试验

共完成两组试验：1)首先将4辆全向机器人置于任意位姿，然后基于人工势场法组建方形编队队形，最后通过状态切换，基于Leader-Follower算法完成编队跟踪方形轨迹；试验中设定编队距离为50 cm，避碰距离为10 cm，试验结果如图10所示。2)在3辆全向机器人基于人工势场法组建一字形编队队形后，通过触发状态切换机制，由虚拟结构法完成编队跟踪sin函数轨迹；试验中设定编队距离为40 cm，避碰距离为20 cm，虚拟质点的期望轨迹为

y=1.25+1.25×sin (2πx/2.5).

(27)

UWB定位系统采集到的试验2数据如图11所示。

由试验2的结果可知，相对于试验1，试验2不仅完成了编队组建和队形变换，还进行了编队行进，分别验证了人工势场法与虚拟结构法和Leader-Follower算法之间状态切换机制的有效性和稳定性。同时，与其他算法仅在编队控制的某个环节有较好的效果相比，试验2中的两组试验，也证明了本文所提算法在保证有效性的前提下，增加了系统的灵活性和适应性，同时降低了算法的复杂度。试验中，多机器人编队轨迹跟踪状态良好、编队队形保持稳定，验证了分布式状态估计算法在编队行进时的有效性和稳定性。

3.3 试验3：多机器人编队避障试验

通过对状态切换机制中触发条件的权重进行修改，将试验分为两组：1)强制保持队形进行编队避障；2)采用最有效率方式进行编队避障。为验证机器人在编队行进中遇到障碍物时，能否触发状态切换并有效避障，在1号机器人上安装了RoboPeak公司生产的Rplidar A1激光雷达(见图12)，用来感知周围环境，将数据传输给中央处理机，并在编队行进的路径上放置长35 cm×宽50 cm的障碍物。试验中，首先将3辆全向机器人置于任意位姿，然后基于人工势场法组建一字型队形；其次通过状态切换机制，由虚拟结构法保持编队行进，当遇到障碍物时，触发状态切换机制分别由虚拟结构法强制保持队形进行避障、由基于行为的算法进行编队避障；最后在避障后重新组建编队并保持行进。试验中设定编队距离为30 cm，避碰距离为10 cm.

UWB定位系统采集到的试验3数据如图13所示。

由试验3的结果可知：第1组试验结果为基于虚拟结构法强制保持队形进行避障，第2组试验结果为基于行为的算法进行编队避障；通过改变状态切换的阈值矩阵，可以方便、有效地对多机器人编队状态进行改变，从而增加系统的灵活性和适应性。试验结果证明了本文所提算法能够有效地完成编队避障并实时避碰，且在避障完成后能重新组建队形并保持队形前进。相对于试验1和试验2，在完成编队组建、编队行进后，进一步考察了编队避障，验证了基于状态切换的分布式多机器人编队控制策略在实际编队控制中的有效性和稳定性。

4 结论

本文基于UWB测距，将切换系统的思想引入编队控制中，提出了基于状态切换的编队控制策略。在充分发挥各传统编队控制算法优点的同时，最大化摒弃各算法的缺点，有效降低了编队控制算法的复杂度。提出了分布式状态估计算法，降低了对机器人数据处理能力的要求。最后通过搭建的多机器人平台，完成了编队队形变换、编队行进、编队避障等试验，实验结果表明，所提策略能够在多任务条件下有效完成编队控制。