基于视觉的多自主移动机器人动态编队技术研究

Research on Dynamic Formation Technology of Multiple Autonomous Mobile Robots Based on Vision

摘要: 目的：针对多自主移动机器人动态编队过程中，机器人产生的位置和角度误差问题，提出一整套基于视觉相机的空间坐标计算方法。方法：首先搭建出一套相机测距定位系统，介绍系统中各部分的构成及对应的职能；其次，构建出三位空间坐标系，详细介绍坐标系中关键数据的测算过程和计算公式；最后，搭建出含有多自主移动机器人的编队系统，对上述坐标角度计算方法进行测试和验证。结果：实验结果表明，机器人坐标角度的计算符合设计预期，良好地实现了整个编队过程。结论：基于视觉的多自主移动机器人动态编队技术符合工程应用要求。

Abstract: The work aims to propose a set of spatial coordinate calculation methods based on vision cameras to solve the problem of position and angle errors generated by the robots during the dynamic for-mation of multiple autonomous mobile robots. Firstly, a set of camera ranging and positioning sys-tem is built to introduce the composition and corresponding functions of each part in the system. Secondly, a three-dimensional space coordinate system is constructed, and the measurement pro- cess and calculation formula of key data in the coordinate system are introduced in detail. Finally, a formation system with multiple autonomous mobile robots is built to test and verify the above cal-culation method. The experimental results show that the calculation results of the robot coordi-nates meet the design expectations, and the entire formation process is well realized. It may be concluded that the dynamic formation technology of multiple autonomous mobile robots satisfies the requirements on engineering applications.

1. 引言

工业生产中，如何利用机器人完成定位和抓取一直是一个热门话题 [1] [2]。倪鹤鹏等人提出基于时间与物品位置的图像去重复算法，旨在判断去除重复图像信息 [3]；陈海永等人提出定位起始点和判断位置的视觉控制算法，在保持较好鲁棒性的前提下实现精确定位控制 [4]。上述文献在研究过程中，解决了图像提取、处理工作，实现了精准定位的功能。这些功能技术不仅仅可以适用于工业中的机器人抓取动作上，还能够在多机器人动态编队中发挥一定的价值。

多机器人系统主要研究由多个机器人组成的队伍通过相互协调配合，完成单一机器人难以实现的复杂操作，因而其适用范围更为广泛 [5]。原魁等人介绍了在机器人编队系统中的编队控制问题 [6]。Balch T等人提出了基于行为的编队控制算法 [7]；杨丽等人提出了未知环境下的多机器人动态编队控制方法，将全局控制问题转化为各个机器人的自我调整问题 [8]。

在多机器人动态编队过程中，由于机器人是存在于空间中的三维物体，因此许多工业中的视觉定位技术在应用时无法直接套用。为了解决此问题，本文在前人研究基础上设计出能基于视觉的多自主移动机器人动态编队定位技术。通过创建视觉三维坐标系，仅需将相机捕捉到的关键点数据发给云端进行处理计算，就能获得目标机器人的中心点坐标和面朝角度。将相应数据与理想路径数据进行对比，得到位置及角度误差，并通过基站控制进行误差调整，使整个动态编队过程表现出较好的鲁棒性。

2. 相机测距定位系统的搭建

整个系统包含有MCU、智能相机和多个参与编队的自主移动机器人，如图1所示。

智能相机拍射范围可网络化为5 × 6的阵列，机器人放置在网络的中心位置。其中在编队时每个机器人按照匀速v的速度运动；机器人运动过程中首先经过智能相机，根据本文设计的算法，允许智能相机自上而下正对机器人运动区域，也允许智能相机位于机器人运动区域侧方，以倾斜的角度拍摄记录区域内的机器人位置情况；数据传输至MCU后，MCU根据捕捉到的相应数据算出目标运动机器人中心点M的坐标 $\left(x_{\text{M}},y_{\text{M}},z_{\text{M}}\right)$ 、机器人在编队过程中产生的摆放角度误差 ${\theta }_e$，最后根据计算得到的最终应在的目标位置坐标。

Figure 1. Camera ranging and positioning system

图1. 相机测距定位系统

3. 视觉坐标系的创建与定位算法

为描述清楚相机视觉下的空间坐标系，构建OXYZ三维坐标系，如图2所示。细节描述如下：以相机为点S，坐标为 $\left(0,0,h_1\right)$， $h_1$ 为已知相机的悬挂高度；记S在OXY平面上的投影为点O，坐标为 $\left(0,0,0\right)$ ；以机器人运动方向为X轴，构建右手坐标系下的Y轴和Z轴。

Figure 2. 3D coordinate system based on camera vision

图2. 相机视觉下三维空间坐标系

3.1. 相机拍摄所得数据分析

当目标移动机器人编队时，由相机进行拍摄抓取。假设机器人为立方体，共8个顶点，相机可抓取得到距离点O最近的顶点A，而点A位于OXY平面上；同时可抓取到得到距离O点最远的顶点C在相机视角方向上位于OXY平面上的投影C'。

已知相机的成像比例为k，由相机拍摄结果可以获得向量 $OA$ 的长度 $l_1$ 及偏转距离 ${\theta }_1$，同时可以获得向量 $O{C}^{\prime }$ 的长度 $l_2$ 及偏转距离 ${\theta }_2$。各个数据的对应关系已在图2中标记表示。

3.2. 相机数据处理过程

此刻向量 $OA$ 于空间坐标系下可表示为：

$\left(l_1\ast \cos {\theta }_1,l_1\ast \sin {\theta }_1,0\right)$ (1)

向量 $OC$ 于空间坐标系下可表示为：

$\left(l_2\ast \cos {\theta }_2,l_2\ast \sin {\theta }_2,0\right)$ (2)

由图2可知，为求得目标移动机器人的中心点M的坐标 $\left(x_{\text{M}},y_{\text{M}},z_{\text{M}}\right)$ 及角度误差 ${\theta }_e$，关键在于求得顶点B坐标，即向量 $OB$。

分析点O、点S、点C、点C'、点B之间的关系，如图3所示。在实际多自主移动机器人编队过程中，目标移动机器人的高度已知为 $h_2$，因此可由图3获得向量 $OB$ 的计算方法为

Figure 3. Relationship graph of some key points in spatial coordinate system

图3. 空间坐标系中部分关键点间关系图

从而向量 $OB$ 于空间坐标系下的长度关系可表示为：

$\frac{C^{\prime }B}{C^{\prime }O}=\frac{\text{BC}}{\text{OS}}$ (3)

向量 $OB$ 可表示为：

$\left(\frac{h_1-h_2}{h_1}\ast l_2\ast \cos {\theta }_2,\frac{h_1-h_2}{h_1}\ast l_2\ast \sin {\theta }_2,0\right)$ (4)

结合向量 $OA$，求得目标物体中心点M坐标为：

$\left(\frac{1}{2}\ast \left(l_1\ast \cos {\theta }_1+\frac{h_1-h_2}{h_1}\ast l_2\ast \cos {\theta }_2\right),\frac{1}{2}\ast \left(l_1\ast \sin {\theta }_1+\frac{h_1-h_2}{h_1}\ast l_2\ast \sin {\theta }_2\right),\frac{h_2}{2}\right)$ (5)

假设此时目标物体的理想角度为 ${\theta }_0$，误差角度 ${\theta }_e$ 计算方式为：

${\theta }_{\epsilon }=\arctan \left(\frac{y_{\text{B}}-y_{\text{A}}}{x_{\text{B}}-x_{\text{A}}}\right)-{\theta }_0$ (6)

考虑到机器人与工业中的产品不同，存在着正面与反面，因此可以对式(6)进行修正，具体实现方式为：以机器人的正面为对称轴，将左侧四个点(含点A在内)全部标记为红色，将右侧四个点(含点B和点C在内)全部标记为蓝色，通过判断相应点的颜色来判断是否对式(6)进行修正。当红色点距离原点O较近，说明一切正常，无需修正；当蓝色点距离原点O较近，而红色点距离原点较远时，说明此时机器人面朝方向正好相仿，需要对式(6)进行 $\pi$ 的修正。

考虑到机器人以匀速 $v$ 沿X运动，经过任意时间t后，应产生位移向量 $OS$ 为 $\left(v\ast t,0,0\right)$，在此情况下，目标运动机器人中心点M应沿向量 $OS$ 移动至空间坐标系的点M'，M'坐标为：

$\left(v\ast t+\frac{1}{2}\ast \left(l_1\ast \cos {\theta }_1+\frac{h_1-h_2}{h_1}\ast l_2\ast \cos {\theta }_2\right),\frac{1}{2}\ast \left(l_1\ast \sin {\theta }_1+\frac{h_1-h_2}{h_1}\ast l_2\ast \sin {\theta }_2\right),\frac{h_2}{2}\right)$ (7)

当相机捕捉到移动机器人经过时，记录下文中所述分析过程中所需的对应数据，并发送给MCU，MCU处理计算后，得到空间坐标系下机器人为完成捕捉动作应移动至的准确坐标，并根据计算得到的 ${\theta }_e$ 进行偏转，从而在经过时间t后准确将目标移动机器人捕捉。

4. 动态编队的试验及结果分析

4.1. 多自主移动机器人编队队形

多自主移动机器人系统由云服务器、MCU+摄像头、多自主移动机器人组成。编队采用领航跟随算法(Leader−Follower)实现，把动态编队过程的运算部分存放在云端，定位的信息传输到云端进行数据库实时更新，在云端存储和计算 [9]。其中的编队队形如图4所示进行规划。

Figure 4. Formation transformation diagram

图4. 队形变换图

多机器人编队采用领航跟随算法，位置坐标的校正通过视觉定位实现。机器人的编队通过两步完成，第一步云端传输队形信息，即每个机器人当前的实际位置坐标和队形变换后的目标位置坐标，进行路径规划计算，给出每个机器人前进、后退、左移、右移的距离；第二步MCU把云端传来的控制信息一次性下发给每个机器人并确保机器人接收到信息；第三步机器人自身的控制器驱动机器人移动目标位置；第四步摄像头检测机器人到达的位置坐标与实际坐标是否一致，不同时通过前后左右移动到实际坐标位置，进行位置误差的微调。当机器人变换队形时，云端服务器发送编队指令控制领航者移动到对应的坐标，跟随者随着领航者发送的信息开始移动，云端调用数据库中的坐标与实际坐标进行判断，检测每个机器人的位置是否正确。编队变换坐标变化如表1所示，单位用cm。

Table 1. Coordinate change table of team changing

表1. 变队坐标变化表

可以看出，当心型变换为K字形编队时，① ③ ⑦号坐标不变，②右移，④下移右移，⑤ ⑥右移，⑧左移，⑨下移左移，按照图4移动，其中一格为10 cm。由于机器人同时进行移动操作，考虑防碰撞问题，经过最优移动路径规划编队，再结合云端大数据存储和计算功能，使机器人编队效率更高。

4.2. 视觉定位

在机器人编队视觉定位的过程中，领航者位置坐标信息由智能相机进行检测，形成坐标定位 [8]，视觉定位主要运用MCU进行处理，MCU利用OPencv库对图像进行处理，通过检测物体标签的形状和颜色，对所需颜色以及形状进行分析识别，识别计算所得的坐标信息即时存储于云端，检测过程中用不同颜色的正方体代表不同的机器人进行位置检测，图5(a)为模拟摄像头的定位检测图，图5(b)为实际摄像头的定位检测图。

(a) 模拟摄像头定位检测图 (b) 实际摄像头定位检测图

Figure 5. Location map

图5. 定位检测图

多组检测坐标与实际坐标对比如表2所示。选择① ② ③号进行数据比对，检测所得的坐标单位为毫米(mm)，检测的数据与实际坐标之间的误差范围为0~54 mm。误差在可行的范围内，对机器人的编队不会有太大影响。

Table 2. Comparison table of detection coordinates and actual coordinates

表2. 检测坐标与实际坐标对比表

4.3. 编队队形变换仿真

与采用backsteeping (逐步后推，反推)的控制(简记做BSMC)做对照，以误差浮动，收敛速度等做评价指标进行分析。对三角队形进行仿真，理想编队队形控制与基于视觉的编队队形控制(VS)中某一个机器人的误差分析如图6所示。

Figure 6. Simulation results of formation transformation

图6. 编队队形变换仿真结果

三个机器人完成从随机状态形成三角形编队并在外界干扰下沿直线轨迹保持编队运动。从仿真图中可以看出，在BSMC控制或VS控制的作用下，多移动机器人可从初始状态生成期望编队队形的L = 60 cm正三角形，并保持队形前行。在运行过程中能够完成从L = 60 cm至L = 150 cm的正三角形的编队队形的切换，并且在此过程中机器人并未出现掉队的现象。从仿真图中可以看出，变换队形时，跟随者1和跟随者2的角速度和线速度会发生较大跳变，这是由于瞬间队形的改变，导致距离误差过大造成的，当每一次队形变换结束后，线速度和角速度均会趋于稳定，均与领航者保持相同的速度运动，达到一致性。同样改进的VS控制采用误差补偿因此线速度和角速度的变化相对比较平稳。

5. 结论

本文提出了一套基于视觉的多自主移动机器人动态编队技术，在工业抓取技术之上构建出完整的三维空间坐标系，利用视觉相机抓取某时刻目标移动机器人在空间坐标系中关键点的视距与角度，传送给云端，由云端计算目标移动机器人中心点M的准确空间坐标，并推得该机器人最终应在的目标位置坐标。为了实现该目的，本文设计出基于智能相机的定位测距方法，搭建了完善的试验系统。通过实际数据，证明系统的实际可行性和适用价值。下一步，将在本文已有的研究成果基础上，继续研究空间坐标系下相应关键点和向量的数据分析，改进相应算法，使整个系统能够更为广泛地适用于各种工业产品模型，同时使系统对各种形状的机器人编队具备更强的适用性。

基金项目

2020年度河南省高等学校重点科研项目20B120003。