李金芝，张志安，程 志，江 涛

(南京理工大学机械工程学院，江苏 南京 210094)

1 引言

目前，多机器人协同控制系统已成为控制领域和系统工程的研究热点之一，受到机器人、数学、物理、生物等相关领域专家的广泛关注[1]。为提高机器人任务完成的效率，通常在执行任务过程中，采取协同编队的控制方法[2]。

在目前的编队控制算法中，领航-跟随编队控制法[3]采用链式拓扑结构，跟随者跟踪领航者形成队形，具有数学分析简单、易保持队形、通信压力小等优点，被广泛应用于多机器人系统编队中。但是该算法针对位置环境下的编队控制研究甚少，缺少整体路径规划功能，特别是领航机器人如何在线进行路径规划、实时自主避障有待更进一步研究；人工势场法是在机器人周围设计一种类似电场的势场，机器人的运动是靠势场力驱动。在生成路径时只需要局部和静态的信息、有着很强的灵活性和稳定性，尤其在复杂环境下[4]。同时人工势场法也存在着缺陷，文献[5]指出该法依托局部环境信息运算，缺少全局信息的指导，容易陷入局部极小值。

针对上述方法存在的问题，本文采用基于改进领航-跟随法和改进人工势场法相结合的编队控制算法。领航-跟随法负责整体编队的队形控制，引入虚拟跟随机器人构成虚拟领航-跟随结构队形，确保编队的稳定性；采用改进人工势场法进行在线局部路径规划，引入虚拟斥力旋转势场，避免局部极小值。最终实现多机器人编队的无碰撞路径到达目标点。

2 全向移动平台建立

本实验研究采用基于麦克纳姆轮的全向移动机器人作为研究平台，该平台能够实现前进、后退、左移、右移以及旋转等运动，具有水平、垂直和旋转三个自由度，相比于其它移动机器人平台具有更高的灵活性。每个轮子由多个按照固定角度安装的被动滚轮组成，四个轮子所提供的轴外力可以使其进行全方位运动[6][7]。

运动学建模是从理论上分析麦克纳姆轮全向移动的原理，为此建立图1的运动分析示意图。

图1 麦克纳姆轮运动分析示意图

当轮轴线和棍子轴线夹角α为45度时，四个车轮的转速与平台中心速度之间的映射关系算为

(1)

式(1)中，l1、l2-轮子中心到X轴和Y轴的距离；Vω-每个车轮的转速；Vx、Vy-平台在X轴和Y轴方向上的分速度；ωz-yaw轴自转角速度。图1表明了机器人速度和位置状态间的关系，在给定平台的驱动速度和机器人初始位姿[xiyiθi]下，可以求得任意时刻t的机器人位姿[xi(t)yi(t)θi(t)]。

通常情况下，多机器人编队系统所用到的坐标系主要分为：全局坐标系、机器人自身局部坐标系以及传感器坐标系。全局坐标系定义为xoy，坐标原点在机器人起始位置处，第i个机器人t时刻在全局坐标系下的位姿可表达为：zi=[xi(t)yi(t)θi(t)]；此外机器人还拥有自己的局部坐标XOY，用来描述机器人之间相对位置关系，通过传感器获取的坐标和运动控制指令的下达都是基于局部坐标系进行的。机器人在双重坐标系下的运动示意图如图2所示。

图2 机器人在全局坐标系下的位姿

3 多机器人编队控制算法

3.1 传统领航-跟随法

领航-跟随法主要有两种编队模式：l-φ和l-l，其中l-φ应用较多，主要原理是跟随者和领航者之间保持一定的距离和角度，才能形成期望队形。如图3所示：控制目标是limt→∞(ld-l)=0和limt→∞(φd-φ)=0，其中ld和φd是跟随机器人和领航机器人间期望的距离和角度，l和φ是实际测得的相对距离和角度。

图3 l-φ领航-跟随编队结构模型

3.2 虚拟结构领航跟随法

本文在传统领航跟随编队法的控制思想上，基于l-φ的编队模式，提出了虚拟结构的领航-跟随法。虚拟结构描述了机器人编队的刚性队形，领航机器人作为队形中一个固定的参考对象，跟随机器人跟随刚性队形中相应的虚拟机器人保证队形。将编队控制问题转化为跟随机器人跟随虚拟机器人的跟踪问题，通过不断改变l、φ值，可以达到变换任意队形的目的，具有一定的可行性。以三台机器人为例，本文提出的虚拟结构的领航-跟随队形的结构的模型如图4所示。

图4 虚拟结构的领航-跟随型结构模型

其中ZL为领航机器人，ZF1和ZF2为跟随机器人，并且ZFV1、ZFV2表示ZF1、ZF2的期望虚拟机器人，通过转化与计算领航者ZL的位姿，可以得出两个虚拟机器人ZFV1、ZFV2的位姿表达式为：

(2)

(3)

ZL(xL，yL，θL)为领航者位姿，ZF1(xF1，yF1，θF1)、ZF2(xF2，yF2，θF2)是跟随机器人位姿，跟随机器人各自的虚拟跟随机器人位姿为ZFV1(xFV1，yFV1，θFV1)、ZFV2(xFV2，yFV2，θFV2)，三者构成了虚拟刚性三角形队形。

以跟随机器人ZF1为例，其中还可以得出跟随机器人与虚拟机器人的位姿误差表达式如下：

(4)

在本文搭建的全向移动平台上，根据麦克纳姆轮速度解算公式(1)，将平台在X轴上的分速度Vx看作是跟随机器人与虚拟跟随机器人之间距离在X轴上的误差分量Δx；平台在Y轴上的分速度Vy看作是跟随机器人与虚拟跟随机器人之间距离在X轴上的误差分量Δy；平台围绕yaw轴自转角速度ωz看作是跟随机器人与虚拟跟随机器人之间的角度误差Δθ。将公式(1)和公式(3)进行融合，四个麦轮的速度如下：

(5)

通过改变领航机器人和跟随机器人之间的距离l和角度值φ，即可改变四个麦轮的速度输入值。选择的控制行为(l，φ)T需要使得Δx、Δy、Δθ都逼近于零，即limt→∞ΔFiVi(t)=0，i∈[1，n]，当上式成立时，意味着编队中每一个跟随机器人都到达其对应的虚拟跟随机器人位置，最终形成期望的队形完成编队任务。

4 改进人工势场法的编队避障控制

4.1 传统人工势场法

1986年KHATB首次提出了人工势场法，将机器人所处的环境虚拟为人工势场，机器人在虚拟环境中运动，障碍物周围存在斥力场对其产生斥力，目标点周围存在引力场对其产生引力，在引力与斥力的合力作用下，机器人沿最小化势能方向运动[8]。人工势场法具有简单的数学模型、依赖信息少等优势，尤其是在避障方面的独特优势，非常适合未知复杂环境下的在线局部路径规划任务。但正是由于能够实现快速优化的特点，该方法在寻找最优解的过程中往往容易陷入非真正最优的次优位置，即局部最小值[9]。在python中仿真如图5所示。

图5 陷入局部最小值示意图

4.2 虚拟斥力旋转势场

上述局部最小值的形成主要是人工势场产生的斥力和引力反向共线，如图6所示。

图6 局部最小值形成示意图

由图6可知，当机器人、障碍物和目标点位于同一条直线时，机器人所受的斥力和引力共线。当FaFr时，即引力大于斥力，机器人会遇到障碍物；当Fa=Fr时，即引力和斥力相等，机器人则会认为到达目标点。最终机器人会在局部最小值处徘徊，无法到达目标点。

针对以上出现的局部最小值情况，本文在传统人工势场法的基础上添加了一种虚拟斥力旋转势场，如图7所示。

图7 虚拟斥力旋转势场

当机器人运动到位于目标点和障碍物连线上，即位于障碍物的背面，此时除了原有斥力势场和引力势场作用外，还将受到水平于障碍物影响范围边缘的虚拟斥力旋转势场的作用，在三力的共同作用下逐渐远离障碍物的影响区域。因此编队中的领航者在平面区域中受到的总势场函数为

(6)

式中：ΣU(x)-机器人在环境中x位置时的综合势场，Ua(x)-引力势场，Uobs(x)-斥力势场，Ur(x)-虚拟斥力旋转势场，F(x)-总势场力。

在图7中，领航机器人RL受到的引力势场函数为

(7)

式中，(x，y)-领航机器人实时位置；xgoal-目标点位置；ka-引力增益系数。

同样，机器人只有在和障碍物间的距离小于某常数D，才被视为进入斥力势场作用范围内。对势场函数求解负梯度，即可得到斥力势场力的表达式。RL和障碍物间的斥力势场函数为

(8)

(9)

式中：f(x)-领航机器人处于x位置时传感器探测与障碍物间距离，γ-斥力增益系数，D-障碍物斥力势场影响区域半径。

同时机器人还将受到虚拟斥力旋转势场的作用。虚拟斥力旋转势场的作用就是为打破引力和斥力之间的平衡，改变机器人受到的合力方向，使得机器人能够跳出局部最小值陷阱，朝着目标点前进。势场函数如下

(10)

Fr(x)=-∇Ur(x)

(11)

图8 虚拟斥力势场曲线图

本文运用改进的人工势场法局部避障时，领航者根据目标点产生的引力、自身的行驶速度，来计算下一时刻领航者以及跟随者的坐标轨迹。根据车载传感器不断探测周围环境，当传感器探测距离f(x)大于D时，机器人只受到目标点产生的引力作用；由图8可得，当传感器探测距离f(x)小于D且Fa(x)=Fobs(x)时，此时机器人走进局部极小值陷阱，本文引入的斥力旋转势场力陡然增大，方向和原有的斥力方向互相垂直，领航机器人受到的合力就会发生改变，在引力、斥力和虚拟势场力的共同作用下，打破引力和斥力反向共线等值的局面，逐渐逃离局部极小值的陷阱区域；而当传感器探测距离f(x)小于D、Fa(x)≠Fobs(x)且二者方向不共线时，即引力和斥力平衡不存在时，斥力旋转势场则会逐渐减小，对机器人造成的影响可忽略不计，领航者就会带领跟随者在朝着目标的合力作用下慢慢靠近目标点。

5 仿真校验

本文利用python中的pygame模块搭建了仿真环境，选用三台全方位移动机器人来验证该领航-跟随型编队控制方法的有效性。分别在无障碍环境和有障碍环境下验证编队算法的可行性，不仅可以形成任意队形、保持队形稳定，还可以随时切换队形，并且在遇到障碍物时能够及时躲避并顺利到达目的地，从而实现优化编队控制的目的。

5.1 无障碍环境下编队仿真

在无障碍环境下，通过设定三种期望队形：三角形、柱形和线性的编队队形，队形的l、φ和机器人起始点参数见表1。在改进的领航-跟随编队控制算法下，机器人从起始点出发形成目标队形，仿真结果如图9所示。

表1 预设队形的参数

图9 编队队形形成

其中，R1是领航者，R2、R3是跟随者，Rv2、Rv3是虚拟跟随者。由仿真可得，在构建的400×400地图上，跟随机器人能够通过自身与虚拟跟随机器人的位置误差，不断调整行进步长，最终到达虚拟机器人的位置点，形成期望队形，验证了该算法的有效性。

为了进一步验证本文提出的虚拟结构领航-跟随法的高效性，现进行队形变换仿真。三台机器人位于地图左下角，通过队形变换到达目的地。仿真结果如图所10示。

图10 编队队形变换过程

由图10可得，在构建的650×600地图上，R1(160，590)是领航者，R2(100，590)和R3(230，530)是跟随者，A(230，80)点和B(400，40)点是队形变换点，C(500，400)点是目标点，图中虚点圆是三台机器人的运动轨迹。在提出的虚拟结构领航-跟随编队控制法下，机器人队伍首先形成三角形编队队形，在A点变换为线形编队队形，接着在B点变换为柱形编队队形，最后到达目的地C点。通过仿真实验，体现了队形切换的灵活性和队形保持的稳定性，从而验证了本文改进算法的高效性。

5.2 障碍环境下编队避障仿真

在静态障碍物环境下，运用本文提出的虚拟结构领航-跟随法进行编队控制，改进人工势场算法进行避障仿真实验，仿真时设定ka=0.8，γ=0.9，ε=0.5，D=10，实验结果如图11所示。

图11 复杂环境下机器人队伍避障实验

首先构建600×600的环境地图，并在地图上安排好3台机器人的出发点以及目标点。R1(130，540)是领航者，R2(80，540)和R3(180，560)是跟随者，图中的虚点圆是三台机器人运动过程中的轨迹。从图11可知，在障碍物环境下，通过本文提出的结合算法，领航机器人依然能够规划一条无碰撞路径，两台跟随机器人通过不断跟随虚拟跟随机器人，从起始点顺利到达目标点。机器人队伍首先从起始点出发形成三角形编队队形，并保持该队形结构前行，在遇到拱形障碍物A(200，245)时，切换队伍形状为柱形和线形来脱离拱形障碍物，在B(310，240)处以柱形编队队形穿过狭窄通道，并在C(370，240)处遇到了局部最小值点，此时在本文提出的虚拟斥力旋转势场的作用下摆脱局部极小值，如图12所。

图12 改进人工势场法摆脱局部极小点过程

由图12可知，在C处机器人、障碍物和目标点位于同一直线，即局部极小值点，在原有的引力场Ua(x)和斥力场Uobs(x)作用下，机器人队伍并不能摆脱局部极小值，此时在虚拟斥力旋转势场Ur(x)作用下，机器人队伍能够跳出局部极小点，朝着目标点前进，并恢复到开始的三角队形到达目的地。

6 结论

本文主要在基于麦克纳姆轮的全向移动平台上研究了多机器人的编队控制和协同避障问题，提出了虚拟结构领航-跟随法实现多机器人编队队形控制，在传统人工势场法的基础上设计了一种虚拟斥力旋转势场，帮助多机器人队伍跳出传统人工势场法中的局部最小点的陷阱，达到多机器人编队无碰撞避障。通过两组仿真证明了算法的可行性，多机器人队伍不仅能够形成任意队形，保持队形的稳定性，同时在障碍环境下成功避开障碍物，进一步验证了算法的有效性和稳定性。