基于视觉伺服的移动机器人编队控制研究与设计*

赵 伟 陆 群 刘甜田 夏菽兰 陈 健

（盐城工学院电气工程学院 盐城 224000）

1 引言

随着机器人技术的不断发展，多移动机器人编队控制问题受到了广泛地关注，成为了研究的热点之一［1～3］。多移动机器人编队控制的研究在各个领域都有实际的应用价值，如：服务型机器人、恶劣环境探、军事监视侦察、灾难现场搜救、航空器编队飞行等［4～6］。

目前依据国内外的研究，多移动机器人的编队控制主要有人工势场法、虚拟结构法、图论法以及主从跟随法等［7］。文献［8］中研究了基于人工势场理论的多移动机器人编队控制，利用基于相对位置的新势场理论解决了全局最小问题。文献［9］中研究了虚拟结构法的分布式无人机编队控制，利用分布式通信减轻了中心节点的通信压力。文献［10］中基于主从跟随法，将编队问题转化为了经典控制理论中的误差跟踪问题，并结合虚拟斥力旋转势场实现编队避障控制。

以上编队控制方法中，都需要利用通信设备来估计领导机器人速度的设计不仅增加了整个系统的成本，而且还需要跟随者机器人的硬件支持，同时数据丢包和通信的不方便也限制了操作性能。因此基于视觉伺服的编队控制方法被提出，即利用摄像机获取视觉图像信号进行实时反馈，从而控制移动机器人到达指定的目标位置，且在编队控制中还可以减少领导者与跟随者之间的通信。文献中

［11］中通过相机选取特征点，实现了移动机器人的视觉跟踪，但是其特征点与相机高度位置是固定不变的具有局限性。

轮式移动机器人是一个欠驱动系统，因此对控制器的设计有更高的要求。相比传统的线性控制方法，非线性控制在处理不确定性和扰动更有优势。文献［12］研究了非完整约束移动机器人的跟踪问题，设计了滑模控制器提高了跟踪性能，但仅在仿真中进行了验证。

针对以上问题，本文基于领航-跟随模型，通过检测AprilTag标签［13～14］获取移动机器人之间的位姿关系，并设计滑模双幂次趋近律控制器提高移动机器人的编队控制效果以及执行效率。最后在麦克纳姆轮全向移动机器人平台上验证了所提出方案的有效性和可行性。

2 移动机器人硬件系统与功能简介

本文实验设备是启智CV移动机器人。移动机器人如图1所示，使用的是麦克纳姆轮底盘结构的全向移动机器人平台。

图1 启智CV移动机器人

四个轮子按照对角一致的原则安装，可以实现前后，左右以及旋转运动，具有三个自由度。该平台相较于其他移动机器人平台具有更高的灵活性。该移动机器人的主要硬件构成如下。

1）机器人控制器模块：具有接口丰富，功能强大，使用简单便捷等特点。控制器采用意法半导体的Cortex-M3处理器作为主处理器，处理器频率为72MHz，内部自带265K程序存储器和48K数据储存器，并通过外扩的Flash芯片，具有高达2MB的用户程序储存空间。

2）伺服电机模块：采用直流电机，电机额定功率为20W。光电编码器闭环控制，在有效负载内均能保持匀速运动，可完成比较高精度的机器人运动控制。此外，伺服电机模块具有完善的电流保护功能，避免过流过热造成的硬件损坏。

3）姿态检测模块：启智CV移动机器人搭载了一枚MPU6050惯性测量芯片，用来检测机器人的姿态参数。MPU6050是一款6轴传感器模块，包括3轴加速度计，3轴陀螺仪，其体积小巧，性能出色。

4）视觉模块：启智CV移动机器人使用的是由奥比中光公司推出的型号为Astra的3D结构光摄像头，具备高精度、低功耗优势，可助力实现人机交互、三维建模等多种功能，适用于40cm～200cm识别距离的智能终端设备。

3 AprilTag视觉定位系统

在移动机器人编队控制研究中，其位置信息和偏航信息是移动机器人编队控制最为关键的信息，控制器的设计也是基于这些状态信息。

美国密西根大学Ed Olson团队提出的一种类似于QR码的二维信息标签AprilTag，在增强现实，机器人，相机校准等领域受到广泛使用。降低二维码自身的复杂程度，满足了应用领域中实时性的要求，可以快速地检测标志，并计算出相对的位置。

3.1 AprilTag检测步骤

1）线段聚类

如图2（a），根据梯度检测出图像中的各种边缘。聚类算法是基于图的算法，图像上的每一个像素为图的节点，图像上相邻的两个像素皆存在一条边，且边的权重为梯度方向之差，根据增加的边权值对这些边进行排序，并根据权重进行边的合并。

2）轮廓查找

接下来需要寻找标签的轮廓，连通域查找的方法计算出图像中黑白的边缘，且每个连通域都有唯一的ID，防止共用一条边产生错误。

3）寻找四边形

得到图像的轮廓后，如图2（b），需要对每一个轮廓进行分割，找到四边形轮廓，作为可能的标签。首先对无序的轮廓点按照对重心的角度进行排序；有了排序的轮廓点，然后就是按照顺序进行直线拟合；然后取角点间的点拟合直线，求得四条直线的角点作为标签的顶点，如图2（c）。

4）编码与解码

上一步检测出的四边形不一定是标签，所以需要进行编码，匹配和解码。对标签最外层和次外层两层色块的灰度均值作为点阵中每个点判别黑白的阈值，大于改制编为1，否则为0，从而得到二进制编码。将得到的编码与标签家族编码库进行比对校验，得到相应的标签信息，如图2（d）。

图2 AprilTag检测步骤

3.2 相机标定

本文使用的标定方法是一种基于平面棋盘格的标定方法［15］，于1998年由张正友提出。该方法实用性强，精度高，相机和标定板可以任意放置。下面对该方法做简要介绍。

其中，P=(U,V,W)为世界坐标系中的一个点，p=(u,v)为像素坐标系下的像素坐标，Z为尺度因子。等式左侧从左到右的矩阵分别对应：仿射变换、透视投影和刚体变换。

接着将矩阵化简：

得到的矩阵为相机的内参矩阵，其参数由相机自身所决定。其中ƒ为像距，dX，dY分别表示X，Y方向上的一个像素在相机感光板上的物理长度，u0，v0分别表示相机感光板中心在像素坐标系下的坐标，θ表示感光板的横边和纵边之间的角度。而矩阵为相机的外参矩阵，R表示旋转矩阵，T表示平移矢量。最后的相机成像模型如下：

使用标定板以各个角度出现在相机视野中，相机标定结果如图3所示。

图3 相机标定结果

最终计算出相机的内参矩阵K以及畸变向量P：

3.3 坐标系转换和标签设置

在机器人编队过程中，机器人坐标系与世界坐标系对齐，以X轴为移动机器人前进方向。标签坐标系，机器人坐标系，相机坐标系由图4、图5所示。

图4 标签坐标和机器人坐标的关系

图5 相机坐标和机器人坐标的关系

相机坐标和机器人坐标关系式可以由旋转矩阵表示为

求得旋转矩阵中的值为

则AprilTag标签的参数设置为

在原版AprilTag定位系统中最后输出的是世界坐标系到相机坐标系的变换，而本文最后要得到的是移动机器人的位置信息是机器人坐标系到世界坐标系的变换，则根据图4和图5坐标系的关系设计公式中的旋转矩阵R和平移矢量T。

4 移动机器人编队控制算法

4.1 传统领航-跟随法

领航-跟随法如图6，其主要有两种编队模型：l-φ模型和l-l模型。通过检测AprilTag标签，可以获取到X轴和Y轴的相对位置以及姿态关系，所以本文采用l-φ模式，其原理是控制跟随者与领航者保持一定的距离和角度，进而实现编队控制。

图6 传统领航-跟随法

4.2 虚拟结构领-航跟随法

本文在原有的基于l-φ编队模式的基础上，采用虚拟结构的领航-跟随法，即预先设计好这个编队的刚性结构，跟随机器人跟踪预先设计好的虚拟机器人，从而实现编队控制。

图7 虚拟结构的领航-跟随编队模型

对该表达式求导可得到运动学误差模型：

其中，v，w为跟踪移动机器人的移动速度和旋转速度，在运动学误差模型中它们是控制量，vr和wr为虚拟移动机器人的移动速度和旋转速度。跟随机器人只需要到达虚拟跟随机器人的位置，即可完成期望队形的编队任务。

4.3 编队控制器的设计

对于轮式移动机器人这样的欠驱动非线性系统，滑膜控制是一种非常有效的控制方法。该方法需要调节的参数少且对扰动有很强的抑制能力。其设计步骤可分为两步：

1）设计滑模面。滑模面是系统的线性或非线性函数，系统的状态将沿着滑模面趋近于0，到达滑模面以后，便处于滑动模态，其滑模运动完全由切换面方程s(x)=0决定。

2）设计趋近律。为了保证系状态最终趋于零，需要设计趋近律。本文采用的是双幂次趋近律。

根据虚拟结构的领航-跟随模型开始设控制器，首先设计滑膜切换面：

对其求导可得：

然后趋近律为双幂次趋近律：

其中，a＞0，b＞0，0＜α＜1，β＞1。当系统状态趋近滑膜面时，-a|s|αsign(s)项起主导作用，当系统状态远离滑膜面时，-b|s|βsign(s)项起主导作用。

将运动学误差模型代入，可得

最后可得滑膜控制律为

在控制律v和ω的作用下，可以使得误差系统渐近稳定。证明过程参考文献［16］。

5 实验验证与分析

设计实验为领航机器人以vr=0.1m/s，ωr=0.04 rad/s的速度运行。跟踪机器人和领航机器人保持如下队形编队移动。

移动机器人的初始位置如图8所示。

图8 领航跟随机器人初始位置

在初始位置，摄像头检测AprilTag的结果如图9所示。

图9 初始位置AprilTag检测结果

此时可以获得位置关系和姿态关系，姿态关系以四元数的方式显示，需要将其转换为角度，其偏航角误差为

则初始位置的误差为

选取参数k1=1，k2=1，k11=0.2，k12=0.22，k21=0.1，k22=0.2，α1=0.5，β1=1，α2=0.5，β2=2。

同时设计线性控制作为对照实验：

其中选取参数Kv=0.22，Kω=0.2。实验结果如图10～图14所示。

图10 xe误差曲线

图11 ye误差曲线

图12 θe误差曲线

图13 滑模双幂次控制律控制量v

图14 滑模双幂次控制律控制量ω

从图10～图14可以发现采用滑模双幂次控制律的xe，ye，θe的收敛速度明显快于线性控制，且都能较快收敛到零，说明本文设计的控制律具有良好的性能。从图11看出，θe和收敛到零但是存在一定程度的抖动，主要是因为摄像机获取的数据存在跳动。

6 结语

本文研究了基于视觉伺服的多移动机器人编队控制问题。通过移动机器人的摄像机检测AprilTag标签获取位姿信息进行实时反馈，从而控制移动机器人到达指定的目标位置，减少多移动机器人之间的相互通信。为了保证编队控制的效率，和抗干扰能力，设计了滑模双幂次趋近律控制器，实验结果表明了控制器的有效性和整体方案的可行性。