改进粒子群优化算法的多机器人地图构建

张 航 姚敏茹 曹 凯 高 嵩

(西安工业大学电子信息工程学院 陕西 西安 710021)

0 引 言

随着工业发展与人们生活水平的提高，机器人的工作环境与任务也变得更加复杂和多样。尽管单个机器人的性能和工作效率随着技术的发展得到很大提高，但仍存在很多单个机器人无法解决的问题，故多机器人控制技术逐渐得到广大学者的重视。与单个机器人相比，多机器人系统具有以下优势：多机器人系统中各个机器人并行工作，能够在更短的时间完成任务；在某些机器人出现故障无法正常工作时，则由其余的机器人代替，提高了系统的鲁棒性；多机器人系统可以携带更多的传感器，采集更加丰富的环境信息，提高环境检测结果的准确率。目前，利用机器人进行资源探测，灾区搜救等越来越多的实际应用都需要使用环境的地图模型，针对单个机器人构建的环境地图范围小、效率低、难以满足大范围环境地图构建的问题，利用多机器人系统高效地构建地图[1-3]逐渐成为新的研究热点。多机器人构建环境地图是在单个机器人分别绘制不同区域局部地图的基础上，综合各个机器人建立的局部地图，将局部地图融合成环境的全局地图的方法。该方法的核心步骤是局部地图间的地图配准，用来进行局部地图之间的数据关联，根据地图配准得到的局部地图间的转换矩阵实现地图融合，而地图配准[4]又分为直接法和间接法两类。

直接法是利用机器人之间位姿的相对观测数据，来求解局部地图坐标系之间的转换关系。文献[5]详细介绍了直接地图配准的具体过程，即各个不同的坐标系之间的几何约束关系，机器人利用自身携带的传感器进行相互测量，采用一定的数据处理手段实现地图配准。文献[6]对上述方法进行了改进，提高了局部地图的转换精度。直接法的优点在于简单易行，计算复杂度相对较低，但是地图配准精度过于依赖传感器测量精度，且要求机器人之间至少有一次相遇，在实际应用过程中可行性不高。

间接法是指利用数据关联算法识别出局部地图之间的重叠区域的公共特征点或是地图频谱特征，进一步求解局部地图之间的对应关系，从而完成地图配准。文献[7]研究一种迭代最近邻点算法进行地图融合，并利用相对熵滤波器来整合地图间的差异。文献[8]提出一种成对一致性最大化(PCM)算法，能够找到局部地图间最大匹配一致集，该方法在真实数据集上的性能优越，但需要采集的数据信息过多，不易于实施。文献[9]主要分析了基于栅格地图表征方式的特征匹配方法，提出了基于自适应随机漫步规划算法[12]，以一种随机搜索算法，搜索出局部地图之间最大的重合部分，实现了精确的栅格地图匹配。文献[10]提出了一种新型的计算栅格地图转换矩阵的方法，主要包括图像分割，数据关联以及转换矩阵的近似等步骤。文献[11]在文献[10]的基础上加入了一种基于神经网络和自组织地图的地图学习步骤，对地图的重叠区域进行聚类分析，再对聚类项进行匹配，从而得到地图间的转换矩阵。文献[13]设计了一套MAP-API框架，保证了地图融合时地图数据的一致性，增强了系统的鲁棒性。文献[14]提出了一种基于正弦图的PSO算法，能够有效解决由于缺少初始相对位姿引起的算法陷入局部最小问题，实现更为准确的地图融合。间接进行地图配准的方法依赖于可靠的数据关联方法，一般精度较高，但计算复杂度会随着搜索空间的增大而迅速上升。

综上所述，若机器人的初始相对位置已知，则在地图融合前即可获得各子地图间的转换矩阵；若各个子地图间存在共同地标，则利用地标即可实现地图间的转换；若机器人间能够实现相互观测，则可以通过计算机器人间的相对距离和方向求解转换矩阵。但在大多数实际场景下，上述信息通常是未知的，获取转换矩阵的唯一方法为匹配局部地图的重叠区域或外观[15]。文献[14，16]利用PSO算法计算精确的转换矩阵，但通常由于重叠区域较小或算法过早收敛导致地图拼接错位。为了解决这一问题，本文研究了一种基于SA-PSO算法的多机器人构建地图方法，该方法通过准确地找到局部地图之间的最优转换矩阵来进行地图融合。首先，每个机器人负责建立不同区域的局部地图，根据局部地图之间重叠区域的相似度设计适应度函数Z(x,y)；其次，利用粒子群优化算法搜索最优的旋转和平移矩阵，使得局部地图间能够获得最大重叠区域；最后，设计自适应概率函数Q(x,y)对局部地图进行重新配准。

1 地图配准

1.1 问题描述

对于非结构环境或是一些难以提取特征的复杂环境，通常用栅格地图来表示。当机器人构建栅格地图时，环境被划分为相同分辨率的二维栅格，计算机把地图中的栅格以矩阵的形式储存，每个矩阵元素对应一个栅格，栅格中的参数表示其存在障碍物的可能性。将栅格地图离散化为N行、M列的矩阵。用函数表示为：

m:[0,N]×[0,M]→R

(1)

式中：函数m表示该地图的置信度模型，m(x,y)表示地图(x,y)处的栅格被障碍物占据的可能性。例如m(x,y)=1表示栅格(x,y)处有障碍物，m(x,y)=0则刚好相反。

在地图融合的过程中，需要利用适当的转换函数对局部地图进行平移和旋转。假设tx、ty和θ三个参数分别代表向x轴方向平移，y轴方向平移和旋转角度，可将该转换矩阵定义为：

TRtx,ty,θ(x,y):R2→R2

(2)

(3)

以两个机器人为例，将协作建图问题描述为：机器人R1和R2从不同的位置出发，分别对环境进行探索并建立两个局部栅格地图m1和m2。让m1保持静止，m2根据不同的转换矩阵Ttx,ty,q(x,y)进行平移和旋转，直到两个局部地图的重叠区域达到最大。两个局部地图的重叠区域被定义为：

(4)

式中：m1[i,j]=m2[i,j]时，Eq(m1[i,j],m2[i,j])=1，否则Eq(m1[i,j],m2[i,j])=0，函数Z(x,y)表示两幅地图重叠区域的相似程度。最终地图融合问题被定义为：给定局部地图m1∈IN,M、m2∈IN,M，确定Ttx,ty,q(x,y)中的参数{tx,ty,θ}使重叠函数Z(m1,Ttx,ty,θ(m2))达到最大值。

当机器人的个数n>2时，需要对以上的函数进行拓展：

(5)

同理，当m1[i,j]=m2[i,j]=…=mk[i,j]时，Eq(m1[i,j],m2[i,j],…,mk[i,j])=1, 否则Eq(m1[i,j],m2[i,j], …,mk[i,j])=0。

1.2 粒子群优化算法

粒子群优化算法[17-19]是基于种群的一种演化算法，根据个体对环境的适应度来决定个体的移动方向。与其他演化算法不同的是，粒子群优化算法不需要对每个个体使用演化算子，而是把个体当作没有体积和质量的粒子，并给每个粒子赋予初始位置和速度。每个粒子会在搜索空间以一定的速度飞行，假设第i个粒子表示为Xi(xi1,xi2,…，xin)，其中n为未知变量的个数。它所经历的最优位置(即适应度最高的位置)记为Pi(pi1,pi2,…，pin)，也称为pbest，另一个是整个粒子群体经历过的最好位置gbest。第i个粒子的速度用Vi(vi1,vi2,…，vin)来表示，则n维变量根据以下的公式进行迭代：

(6)

(7)

粒子群优化算法的具体流程如图1所示。

图1 粒子群优化算法流程图

粒子按照迭代公式与算法流程图描述的方式从不同的位置开始搜索，由各个粒子最佳位置的参数来确定Ttx,ty,θ(x,y)中参数{tx,ty,θ}的最佳赋值。惯性权重ωt的引入大大提高了粒子群优化算法的搜索效率和质量，增强了算法跳出局部最优进行全局寻优的能力。

1.3 适应度函数

适应度函数通常选取待优化的目标函数，本文采用地图重叠区域的相似度Z(x,y)作为寻优函数。假设两幅局部地图的地图矩阵为m1和m2，m1和m2之间的相似度函数Z(m1,m2)表示为：

(8)

(9)

式中：c表示地图m1和m2中栅格设定的参数值，m1[p]表示地图m1中位置p=(x,y)处的参数值为c。例如m(x,y)=1表示栅格(x,y)处有障碍物，m(x,y)=0则刚好相反，m(x,y)=-1表示(x,y)处为未知区域。md(p1,p2)表示点p1和点p2之间的曼哈顿距离[19]。numc(m1)表示地图m1中参数值为c的栅格个数。

计算地图的相似度函数Z(x,y)核心在于计算d-mapc，d-mapc就是地图中的点p1=(x1,y1)到最近的参数值为c的点p2=(x2,y2)曼哈顿距离[20-21]组成的矩阵：

d-mapc[x1][y1]=min{md(p1,p2)|m2[p2]=c}

(10)

图2(a)为Gazebo仿真平台搭建的场景地图。图2(b)表示机器人建立的栅格地图，栅格参数由矩阵T1表示。图2(c)表示栅格参数c为1的d-mapc地图，栅格参数由矩阵Tdmap表示。

(a) 场景地图 (b) 栅格地图 (c) d-mapc图2 构建d-mapc

假设a1、a2分别是地图m1和m2中相似的重叠区域，由于机器人在采集数据的过程中受到噪声的干扰，导致单个机器人建立局部地图的效果也各有差异，因此当PSO算法得到最优参数{tx,ty,θ}时，a1、a2之间的图像距离通常是近似为0或大于0。为了解决由于算法过早收敛引起的误差，设计函数逐一对比a1、a2中相同位置p=(x,y)处的参数值：

sam(m1,m2)=num{p=(x,y)|m1[p]=m2[p]∈C}

(11)

dif(m1,m2)=num{p=(x,y)|m1[p]≠m2[p]∈C}

(12)

2 实验与仿真

2.1 算法仿真实验

本文的所有仿真实验是在Linux操作系统下的物理仿真平台Gazebo上完成的。仿真过程采用的机器人模型是Turtlebot3 Waffle，Turtlebot3是一个低成本，可编程的基于ROS的移动机器人。每一台Turtlebot3可独自通过ROS提供的gmapping功能包来建立自己所在周围环境的局部地图，该功能包提供包括对激光雷达的SLAM，根据激光雷达的输入及姿态数据建立一个基于栅格的2D地图。假设有两台Turtlebot3分别建立了局部地图m1和m2，以Gazebo中的世界中心作为世界坐标系的坐标原点，确定了m1和m2初始位置与方向。将m2相对于世界坐标系的初始位姿按每逆时针3°旋转进行配准，以及局部地图原点的位置按照(+50,0,-50)进行平移，共有120×23=960种可能，利用粒子群优化算法从中找出相似度高的重叠区域，进行地图合并。所有的地图都由397×397的矩阵来表示，每个栅格都有对应的参数值，如果这个栅格未被机器人检索，则参数值被设置为-1，使用相同大小的矩阵来表示地图大幅缩短了算法运行的时间。

如图3所示，该场景是在Gazebo室内场景，共有3个房间，每个房间都有一台Turtlebot3机器人在单独进行SLAM(机器人由上往下依次为：TB0,TB1,TB2)。Turtlebot3建立的局部地图的初始角度各不相同，通过SA-PSO算法对图中待拼接的栅格地图进行实验，得到的实验结果如图4所示。

图3 Gazebo仿真场景

(a) TB0构建的地图 (b) TB1构建的地图

(c) TB2构建的地图 (d) PSO算法过早收敛

(e) SA-PSO算法进行地图配准图4 多机器人构建地图仿真结果

在仿真实验中，为了验证加入概率函数对地图融合算法的影响，对实验中2组地图分别进行实验，考虑到机器人建立局部地图的误差，设定Qs(x,y)达到90%属于地图融合成功案例。仿真结果如表1所示，当Qs(m1,m2)为64.09%时，SA-PSO算法重新进行地图配准，使得Qi(m1,m2)达到91.35%。同理，Qs(m2,m3)为44.73%时，Qi(m2,m3)达到89.53%；相反，Qs(x,y)在75%至90%之间时，SA-PSO算法跳出局部最优解的能力减弱，很难改善地图融合效果。例如，当Qs(m1,m2)为83.64%以及Qs(m2,m3)为81.16%时，由于此时SA-PSO算法重新进行地图配准的概率较小，得到的Qi(m1,m2)和Qi(m2,m3)不发生改变。此外，当各地图之间没有足够的重叠区域时，Qs(x,y)很难达到设定的成功率。例如Qs(m2,m3)为68.34%时，由于重叠区域较小，SA-PSO算法重新进行地图配准后的Qi(m2,m3)仅为69.82%。图5和图6为地图融合结果分析图。(表1中Qi(x,y)为SA-PSO算法搜索重叠区域的栅格匹配成功率，e与ei分别是Qs(x,y)与设定成功率90%间的误差比较以及Qi(x,y)与设定成功率90%间的误差比较)。

表1 地图融合结果及误差

续表1

图5 地图m1和m2的融合结果

图6 地图m2和m3的融合结果

为了验证实验的可靠性，又分别对3组地图进行100次实验。同样假定Qs(x,y)达到90%属于地图融合成功案例，将地图融合失败案例分为Qs∈[10%,30%]、Qs∈[30%,50%]、Qs∈[50%,70%]、Qs∈[70%,90%]、Qs∈[90%,100%]几种情况，分析PSO算法和SA-PSO算法在300次实验中得到的Qs在不同区间出现的次数，实验结果如表2所示。虽然在Qs∈[10%,30%]时，SA-PSO算法重新进行地图配准的概率较大，但Qs∈[10%,30%]通常是由于构建的局部地图质量较差导致，重新进行地图配准也很难改善Qi。在Qs∈[70%,90%]时，算法重新进行地图配准的概率较小，通常得到的Qs=Qi，最终SA-PSO算法在Qs∈[30%,70%]适用度较好。

表2 地图融合成功率分析表

2.2 机器人实验

在实验室场景下使用Turtlebot3机器人，机器人之间利用ROS中的节点进行通信，所有机器人完成对应区域的地图构建后，将局部地图数据发送给被设置为节点管理器的PC，统一进行地图融合。场景1为地面移动机器人控制研究实验室，实验室中间为障碍物区域；场景2为水下机器人控制研究实验室，中间的长方形水池为水下机器人运动控制平台；场景3为学术交流会议室和走廊，场景1，场景2和场景3分别如图7所示。

(a) 实际场景1与机器人构建的地图

(b) 实际场景2与机器人构建的地图

(c) 实际场景3与机器人构建的地图

(d) PSO算法融合后的地图 (e) SA-PSO算法融合后的地图图7 实际场景下的实验结果

场景1和场景2中的Turtlebot3机器人分别作为从机器人，构建当前场景的局部地图，并利用ROS话题通信机制将局部地图数据回传给场景3的主机器人。在主机器人的控制PC上分别执行两种算法进行地图融合，PSO算法的地图融合结果如图7(d)所示，Qs为69.37%。在此基础上，SA-PSO算法跳出局部最优重新进行地图融合，结果如图7(e)所示，Qi为91.04%。需要注意的是，在实验过程中机器人的移动速度不能过快，否则会导致构建栅格地图精度不高。为了防止走廊的场景过于单一导致的局部地图建立产生误差，在走廊及实验室的角落人为放置一些圆柱体，以提高SLAM建图中特征匹配的精度。

3 结 语

本文提出了一种基于SA-PSO算法的多机器人构建地图方法，利用地图重叠区域匹配问题建立数学模型，求解出适应度函数最大时的局部地图间转换矩阵，进行局部地图的配准。同时还设计了自适应概率函数，用来解决优化算法得到的解为局部最优解的问题，提高了地图融合的成功率。本文分别在Gazebo平台和实际物理环境中使用Turtlebot3机器人进行实验。实验结果表明，该方法可以用于大规模环境栅格地图建立，将多幅局部栅格地图融合成全局栅格地图，提高了构建地图的实时性与可靠性。本文主要研究了多机器人的二维栅格地图融合问题，未来主要研究如何在融合后地图上实现多机器人的编队，以及在完善本文方法的基础上着重研究三维点云地图的拼接问题。