室内多移动机器人协同建图与路径规划*

顾家玮，唐 炜，张 乐，陈峻山，周 鲁，成志锋

(1.上海无线电设备研究所·上海·201109；2.西北工业大学 自动化学院·西安·710129)

0 引 言

多智能体协同在民用、工业、国防、军用等领域中有着重要的应用价值。特别在军事方面，多智能体协同作战平台正在成为各个国家武器装备家族中的一个重要分支[1-2]。具体地，在未知环境中，可以利用多智能体协同完成侦察、搜索等任务[3]。就多机器人系统中的协同控制、路径规划、协同感知、信息决策等方面而言，目前，军事任务中的多机器人协同模式尚不完善，如何对机器人群体进行有效的协调和控制，一直是机器人军事化应用中的关键。室内环境下多机器人的协同定位与路径规划，是考察多机器人系统协作机制的一种常用研究方法。

自主导航作为移动机器人的关键技术，需要考虑定位、任务规划、路径规划问题。针对定位问题，采用磁带或电磁导航[4-5]的第一代导航方式，以及依靠环境中信标或二维码[6]定位的第二代定位导航方式，都需要预先布置标记信息，而不能被应用于军事侦察任务中。SLAM技术[7]是目前机器人产业的主要研究方向，已产生了基于视觉、激光雷达、声纳等的SLAM算法[8-9]。Geppert将绝对位姿估计与视觉惯性里程计进行了结合，提出了多摄像机视觉惯性定位算法，使得系统可以提供无漂移的姿态估计[10]。Kim依靠卷积神经网络，利用环境信息将位置识别问题转化成了位置分类问题，通过学习点云描述符得到了一个定位器[11]。北京理工大学的马家辰[12]等对传统粒子滤波算法存在的粒子消耗问题进行了改进，通过引入遗传算法优化保持了粒子的多样性，从而提高了定位的精度；浙江大学网络系统与控制研究所利用以多状态约束卡尔曼滤波为基础的视觉惯性测量，依靠先验雷达地图提供边界三维导航，修正了视觉里程计的累计漂移[13]。为提高SLAM的定位精度，还可采用高精度传感器和采用多传感器融合等方法[14-16]。其中，多传感器融合可将各类传感器获得的数据进行分析处理，将不同的传感器数据进行融合，以实现互补，从而使系统在成本相对低廉时，鲁棒性和精度更高。

Thrun等人利用SEIF(Spare Extended Information Filter)方法实现了多机器人的同步定位与制图。该算法用高斯马尔可夫随机场模型表示机器人的位姿和地图信息，通过搜索局部地图路标和全局地图路标的匹配关系，将局部地图映射到全局地图。Howard提出了一种基于粒子滤波的多机器人同步定位和制图算法，每个机器人可用粒子集来近似表征机器人位姿的概率密度函数。该算法需要提前获得各个机器人之间的相对位置关系，通过坐标系变换得到完整的地图。Ryad Chellali等提出了一种基于环境特征的多机器人SLAM算法。每个机器人可利用激光测距仪和相机采集环境中垂直和水平的线特征，利用EKF结合里程计信息估计自身的姿态，同时利用线特征构建环境特征空间。在多机器人共有的环境特征匹配成功之后，就可以求出多个机器人之间的坐标变换关系。

路径规划作为整个系统的关键技术之一，是多机器人协作系统顺利运行和多机协同效能的重要保证。针对共享工作空间的多机器人系统的路径规划，需要解决以下两方面的问题：(1)机器人相对于静态障碍物的无碰撞路径的规划；(2)多机器人相互之间的无碰撞协调路径规划。Pan等针对以上两个问题提出了基于编队控制的虚拟弹簧方法，各机器人根据控制律和虚拟目标搜索方法，在预定的编队中计算到达目标的最优路径[17]。针对机器个体难以识别周围机器人属性的问题，Bae等将深度Q学习与传统的卷积神经网络(Convolution Neural Network，CNN)进行了结合。其中，CNN利用其环境中的图像信息分析精确的情况，机器人则根据这种情况进行导航[18]。Bader与Binder在假设各机器人定位信息完备的情况下，将时间空间优先思想用于多机路径规划[19-20]，将多机器人规划问题分解成了多个单机规划问题，从而降低了问题的复杂度。个体按照优先级的顺序移动，由单机路径规划器检查潜在碰撞，然而并未被部署在实际的机器人系统中。在实际的多机器人路径规划中，路径冲突的种类繁杂且需要考虑全局效率优化的问题，而定位问题常被作为已知假设来进行路径规划研究。针对以上两点，目前国内多数的研究成果依然处于理论阶段，并没有将其结合部署在实际的多机器人系统中，并进行实验验证。

本文针对一般复杂室内环境，结合SLAM技术，研究并建立了多机器人协同SLAM系统。系统采用多传感器融合的方式，以提高机器人的定位精度。考虑到环境信息未知，在由多个机器人构建得到子图的基础上，系统基于坐标变换进行地图融合。在全局地图信息已知的情况下，对文献[20]所提出的多机路径规划方法，本文采用SLAM技术解决其中的定位假设问题。最后，在实际环境中，利用三台机器人给出了多机协同地图融合的结果，并验证了多机路径规划方案的有效性。本文的重点在于多机器人SLAM系统和平台的实验验证。

1 场景和问题描述

基本的室内环境表述为如图1所示的场景：(1)蓝色栅格表示环境中的障碍物，红色方块表示机器人r={robot_0,robot_1,…,robot_n}。其中，机器人的总数量为n+1个，g0、g1、g2分别对应robot_0、robot_1与robot_2在某次任务中需要到达的目标位置。机器人执行第i次任务的目标点集合为G={g1i,g2i,…,gni}；(2)场景中的可行路径如图1所示。障碍物间的路径为单行道路，即同一时刻只允许单个机器人通过。在多机器人系统对该室内环境信息未知的情况下，本文中机器人协同的目的为，利用多个机器人协作完成该未知区域地图信息的获取；在得到地图信息后，考虑多机器人路径规划，控制多机器人到达各自的指定目的地，并完成各自的任务。

图1 室内环境模型Fig.1 Model of indoor environment

2 多机器人位姿估计和地图融合

多机器人系统需要机器人在完成自身定位的基础上，分享自身的位姿和传感器信息。对各机器人构建的环境地图进行拼接，可完成全局地图的构建，并以此提高构图效率。

2.1 机器人位姿估计

机器人位姿估计作为导航的重要部分，是进行后续构图以及规划的基础，而编码器的自身误差等问题会降低机器人的定位精度。本文通过多传感器融合提高了机器人的定位精度，机器人个体主要通过惯性测量单元(Inertial Measurement Unit，IMU)来纠正里程计的误差，其原理图如图2所示。对加速度计和陀螺仪进行零漂校准后，陀螺仪根据加速度计和磁力计的漂移补偿进行互补滤波，并将其作为滤波的观测更新方程数据，将陀螺仪作为状态更新方程数据，随后输出姿态角信息。当室内机器人启动时，利用IMU 初始值替代里程计数据，获取较为准确的初始位置；在机器人移动后，里程计每间隔一段时间访问经加速度计修正后的二次积分位移值，并将其与里程计自身的位移进行比较。若积分位移很小，里程计换算位移较大，则说明发生了车轮打滑或悬空现象。此时，里程计位移应采用IMU数据，否则仍应使用编码器位移数据。

图2 里程计校正框图Fig.2 Principle of odometer correction

多传感器系统模型如式(1)所示。在该模型中，每个传感器对应的观测量及其观测方程均可不同。前一个传感器更新之后的系统状态量数据，可以作为下一个传感器的预测状态量，以实现融合，最后一个传感器输出的数据可以用来进行下一个时刻的迭代。

x(t+1)=Ax(t)+Bu(t)+ε
y1(t)=C1x(t)+δ1
⋮
yn(t)=Cnx(t)+δn

(1)

式中，x(t)代表t时刻的系统状态量，u(t)代表t时刻对系统的控制量，A、B为系统参数，yi(k)(i=1，…，n)代表某个测量值，Ci(i=1，…，n)代表某个测量值的观测矩阵，ε和δi(i=1，…，n)分别为过程中的噪声。

系统状态量可定义如下

[x,y,z,θ,φ,ψ]T

(2)

向量中，x,y,z指移动机器人在空间坐标中的三维坐标，θ为移动机器人绕X轴旋转的角度，也称俯仰角，φ为移动机器人绕Z轴旋转的角度，也称翻滚角，ψ为移动机器人绕Y轴旋转的角度，也称偏航角。

系统输入为

[u1,u2]T

(3)

系统方程为

x(t)=x(t-1)+u1(t)cos(ψ(t-1))
y(t)=y(t-1)+u1(t)sin(ψ(t-1))
ψ(t)=ψ(t-1)+u2(t)

(4)

通过数据信息，分别得到轮式里程计、IMU的观测值和观测协方差矩阵，可运用多传感器信息融合的流程对系统状态进行更新。

里程计的观测量为[x,y,z,θ,φ,ψ]T，对应的物理参数为移动机器人在空间中的坐标值、俯仰角、翻滚角和偏航角，对应的观测矩阵为

(5)

IMU的观测量为[θ,φ,ψ]T，物理参数为俯仰角、翻滚角和偏航角，其对应的观测矩阵为

(6)

数据融合的程序流程为

(1)当传感器数据开始发布时，开始监听里程计和IMU话题。

(3)控制量u1和u2分别指的是在一个固定的时间内，由机器人左右轮的速度数据进行计算而得到的移动距离和偏航角度，同里程计发布的话题数据中的坐标系偏移程度来计算得出，然后进行状态预测。

(4)获取里程计信息，进行数据对比。如果符合偏差条件，则更新系统状态，否则转至(3)。

2.2 多机器人地图融合

系统中的每个机器人都有自身的坐标系，基于单个机器人SLAM得到的子地图均以各自的起点为地图原点来进行构建。考虑到机器人在三维空间中的运动主要以平移和旋转为主，本文采用了基于坐标变换的地图对齐方法。该方法的本质是寻找各个机器人坐标系之间的相对关系，利用相对变换关系对各自的子地图做相应变换，从而将其合并到同一个坐标系下，以建立全局地图。

在移动机器人运行地图构建程序时，都会产生一个属于自身的参考坐标系和局部地图。局部地图和机器人的参考坐标系之间为刚性连接，环境中的障碍物通过栅格地图的形式保存在局部地图内。在机器人出发前，需确认机器人之间相对的坐标关系。在系统进行地图拼接时，机器人通过计算自身参考系与参考机器人的坐标系的转换矩阵，便可推算出机器人的局部地图的变换关系。同各机器人运动的齐次坐标变换，可以将局部地图中的所有信息映射到参考机器人的地图坐标系中，并且将参考机器人的地图作为全局地图进行保存和使用。

(7)

(8)

将中间矩阵定义为一个矩阵R，其由两组基的内积组成，用来描述旋转前后同一个向量的坐标变换关系。矩阵R描述了旋转关系。在欧氏变换中，某个向量α经过一次旋转和一次平移t后，得到了α,，则有

α,=Rα+t

(9)

其中，t被称为平移向量。由此，可得到欧氏空间内不同机器人坐标系的变换关系。基于这个坐标变换关系，可进行地图的匹配与拼接。

3 基于冲突规避的多机路径规划

本文所采用的多机器人路径规划方案如图3所示。首先，以随机优先级排序方法和一个建议速度对系统进行优先级规划和速度规划。在方案中，单机路径规划采用A*算法。在A*算法的每次迭代中，检查当前扩展的路径点是否与已生成的路径存在冲突。若路径搜索出现冲突，则尝试扩展搜索图，使其能够找到可以规避当前冲突的路径点。在此过程中，若搜索到当前机器人的目标点，则初步规划完成，得到候选路径，并可检查该路径是否有效。如果路径有效，则将其添加到路径列表中，然后根据优先级列表，为下一个机器人进行单机器人路径规划。如果生成的路径无效，则重新进行速度规划。如果通过重新选择机器人行驶速度仍然无法解决问题，则需要重新规划机器人优先级顺序。如果成功为每一个机器人找到了各自合适的路径，则路径生成器会输出对应的路径列表。

图3 多机器人路径规划Fig.3 The process of Multi-robot path planning

由于以某一优先级序列规划得到个体路径只是问题的可行解而并不是最优解，故在此方案中需要重新分配优先级，按照上述步骤多次进行路径规划，对比所有优先级生成路径的代价，选择代价最小的路径作为最优解。

AC在平面构造等深图上不是气水内外边界的宽度，而是代表了气水内外边界深度差，因此可以用FB表示，相当于在两个坐标系下进行了转换，这一点是该方法的关键。

在此方案中，单机路径规划的实现是整个多机全局路径规划的基础，每个机器人都需要按照优先级顺序规划生成自己的行驶路径。传统A*算法是针对单机器人建立的，因此在多机器人路径规划中需要考虑以下两方面的问题并对其进行完善：

(1)路径占用：单机器人路径规划时，必须考虑已完成规划的候选路径，并以此约束新路径生成；

(2)多机冲突规避：针对同一条路径，还需考虑多机器人之间的等待、避碰问题。

4 实验验证

4.1 实验环境和平台

实验选定了一个长、宽分别为8.6m与7.3m的室内房间，利用一套由多个自主移动机器人构成的多机器人系统进行了多机器人协同地图构建与路径规划实验。该系统的网络结构如图4所示，其中包括三台机器人和一台PC机。每个机器人以树莓派3B作为上位机，其中安装了ROS系统，下位机为基于STM32的运动控制器。基于TCP/IP网络通信协议完成多机器人信息交互，构成了一个多机器人实验系统。实验系统利用Pibot机器人作为机器人个体，均配备有IMU和光电编码器来完成室内的相对定位，并配备了激光雷达等避障传感器。单个机器人可实现基于激光雷达的SLAM技术，完成地图构建、导航避障等功能。

图4 实验系统网络结构Fig.4 Composition of the experiment system

4.2 实验结果与分析

(1)多机地图融合

本节采用两台Pibot机器人进行多机器人地图拼接实验，实际拼接结果如图5(a)所示。实验采用2.2节所述的变换关系完成地图拼接。其中，图5(b)为由机器人robot_0构建得到的子地图，机器人robot_1由另一点出发构建地图对应时刻的实验结果如图5(c)所示。对照真实室内环境，两台机器人均可正确返回环境信息。在此基础上，将同时刻两台机器人所构建的子地图进行融合，得到的结果如图5(d)所示。实验结果表明，两台机器人地图拼接结果中的公共区域高度重合，且没有尺度上的偏差。多机器人协同探测所获得的地图包含了全部的地图信息。与单个移动机器人构建的地图相比，多机器人系统能够在保证地图信息正确且完整的情况下，有效提高探测效率。

(a)机器人出发前的相对位置

(b)机器人0的子地图

(c)机器人1的子地图

(d)融合之后生成的全局地图

(2)多机器人路径规划

本节首先基于stage平台搭建了仿真环境，选取四台虚拟机器人R1、R2、R3、R4，起点对应A、B、F、G。对应的目标点为G1、G2、G3、G4。采用第三节所述的路径多机规划方法进行仿真实验，规划路径如图6(a)所示，分别为R1：A-B-C-E(红色)；R2：B-C-D-C-G2(绿色)；R3：F-H(紫色)；R4：G-I(黄色)。其中，R1和R2具有相同的路径段，即发生了路径占用。各个机器人依据所规划路径向目标行进的过程如图6(b)～图6(d)所示。由于R3、R4的路径无任何冲突占用问题，所规划的路径是驶向目标点的最短路径，两者在77s时到达目标点，即R3、R4的子系统用时为77s。R1与R2存在路径冲突，此时R2到达C点。若此时R2直接到达目标，则会导致C-G2段路径堵塞，从而使得R1目标不可到达，故使得R2机器人在周围节点等待R1先行通过。如图6(c)所示，第82s时R2行进至D点等待，当R1通过该路径段后，R2沿D-C-G2段到达目标点。在第188s时，机器人全部到达目标点。系统用时为188s。仿真结果表明，该多机路径规划算法可以成功地为多个机器人规划出可行驶的路径，方法可以有效解决路径冲突及堵塞问题。同时，由于需要解决冲突规避问题，算法效率受到了机器人数量、具体路径占用种类等因素的影响。

(a)第0s

(b)第77s

(c)第82s

(d)第188s

为验证本文多机器人系统的实际路径规划能力，基于第三章多机路径规划流程进行了实验，实验环境与相应路径搜索图如图7所示。本节使用三台Pibot机器人进行了实验，三台机器人在图7(b)中的白色字符自下至上可分别表示为robot_0、robot_1、robot_2。其中，蓝色线段为路径搜索图中的可行驶路径。

(a)实验场景

(b)对实验场景构建的全局地图和机器人的初始坐标图7 实验场景与路径搜索图 Fig.7 The experimental scene

多机路径规划过程如图8所示。机器人的初始位置如图8(a)所示。在上位机终端中，设定机器人各自目标点为G0、G1、G2。robot_0、robot_1、robot_2所得路径分别标识为绿色(path0)、黄色(path1)、橙色(path2)。图8(a)～图8(d)是目标点设定后，终端界面的实时位置显示。图8(e)～图8(h)分别为机器人在某时刻的实际状态图。

(a)设置目标点

(b)机器人1在等待

(c)机器人2在等待

(d)到达各自目的地

(e)机器人在出发点

(f)机器人1在等待

(g) 机器人2在等待

(h) 到达目的地

在图8(a)中，path0与path1发生了部分重合，且robot_2目标点G2处于path0上，这两种情况均表明机器人路径存在占用冲突。各机器人同时按照各自路径行驶必然会导致机器人发生碰撞或堵塞，而从图8(b)与图8(f)可以看出，所采用的规划方案使得robot_1在该位置等待robot_0优先通行。在图8(c)和图8(g)中，robot_2等待robot_0优先通行，从而使得机器人个体顺利得到各个目标位置，并且在行驶过程中不发生机器人与环境、机器人与机器人之间的碰撞。实验结果表明，所采用的多机器人路径规划方法可成功地为多个机器人规划出路径，达到了预期效果。

5 结 论

本文首先针对基于坐标变换的地图融合算法进行了实验验证，有效提高了机器人系统针对未知环境地图构建的效率，在完成了全局地图拼接的基础上，对基于冲突规避的路径规划算法进行了实验验证，主要研究了规划流程和潜在冲突解决算法。在实验中，机器人系统顺利完成了路径规划，解决了路径规划中潜在的路径冲突和规划死锁的问题。在运动过程中，机器人规避了环境中的静态障碍物，顺利到达了各自的目的地。基于坐标变换的地图融合算法可适用于仓储物流等应用环境。

本文的研究还存在一些不足之处。首先，栅格地图的特征信息相对匮乏。在后续的研究中，可以尝试融合环境的图像信息，利用栅格地图表征障碍物分布和路径信息，利用图像特征进行特征匹配和地图扩展；此外，本文使用的自主定位方式主要基于移动机器人自身的传感器来进行匹配和定位，缺少相对于环境的绝对定位方式。在移动机器人的动态运行过程中，往往会出现定位不及时或定位更新失败的情况，进而导致误差的产生。在实际工程应用中，可以尝试加入无线电定位或关键点二维码，以提高移动机器人相对于环境的绝对定位精度。