大环境下拓扑化栅格地图的多峰定位研究

王 翰，蒋 林,*，周 亮

(1.武汉科技大学 机械传动与制造工程湖北省重点实验室, 湖北 武汉 430081；2.武汉科技大学 机器人与智能系统研究院, 湖北 武汉 430081)

0 引言

机器人实现自主执行任务需要对环境进行地图构建和自定位[1]。为了实现室内大环境地图构建和机器人定位，国内外的研究人员开发了各种不同的地图构建和机器人定位的算法。基于ROS开源的SLAM框架Gmapping可以很好实现栅格地图创建和机器人定位[2]。

地图可以分两种，栅格地图和拓扑地图。栅格地图可以将环境中的障碍物信息通过二值化的栅格完整地表示在地图上，机器人可以通过激光雷达等传感器获得机器人在真实环境中的位置实现自定位[3]。另一方面，拓扑地图可以将环境用节点和边表示，可以大量简化地图的信息。基于已知地图的机器人定位需要建立环境的完整地图，EKFSLAM、EIFSLAM等基于KF的定位建图算法，可以很好地实现小场景的定位建图，算法会因为一致性问题[4]无法消除模型误差，最终导致机器人无法正常地运行。RBPF、FastSLAM等基于PF的算法可以处理非高斯非线性的系统，但是通过粒子估计后验概率，计算量过大，容易产生粒子耗尽等问题。为了解决这个问题，Murphy 和Doucet等人提出Rao-Blcakwellised一种新的滤波方式来解决SLAM问题。Montemerlo等在这个基础上提出新一代的SLAM定位算法FastSLAM 2.0，FastSLAM 2.0的优势源于粒子滤波能处理非线性机器人运动模型[5]。每一个粒子不仅表示瞬时的机器人位置，还关联机器人运动过程中的路径和环境的相关特征[6]。FastSLAM 2.0算法中提出一种求取重要性函数的方法，可以在一定的程度缓解粒子耗尽的问题。针对粒子耗尽问题，布伦瑞克技术大学的学者提出一种融合地标测量的似然与栅格地图信息来更新提议分布[7]，以近似后验分布的方式实现机器人的初始位姿未知下的快速定位。将机器人传感器的观测数据和环境的特征相关联，是提高定位精度和鲁棒性的关键。纽约城市大学的学者则通过将交互式模型和联合数据混合关联的方式提高定位的鲁棒性[8]。将环境进行拓扑化的目的是为了有效地提取环境中的特征数据。特征数据可以很大程度提高机器人的传感器数据和环境特征数据关联性。斯坦福大学智能机器人研究团队于2013年首先提出将栅格地图和拓扑地图相结合的混合地图实现算法，并在PM-3平台上进行了实验验证，但是实现的效果并不理想。虽然提取了环境拓扑信息，但是关联性不够。2015年谷歌公司开源SLAM框架Cartographer，该框架可以很好地实现大环境建图,该框架是采用基于优化的方法来实现机器人的定位和建图，并且实现了回环检测，可以很好地修正机器的运动过程中的累计误差[9]。Cartographer中回环检测的判定，采用当前帧的激光雷达数据匹配已经生成的子地图。广东工业大学学者在Cartographer回环检测的基础上，提出基于连续多帧的激光数据检测回环的方法，在很大程度上定位精度。只有在栅格地图保存了完整的环境的条件下，提取拓扑信息，才能使得拓扑化的地图被充分的利用。在研究室内已知地图的机器人定位发现，粒子的数目对粒子滤波定位算法的效果影响很大，正常的粒子数量可能没有办法收敛到机器人准确的位置，粒子过多会导致机器人无法正常运行。通过实验证明了基于拓扑化栅格地图的多峰粒子滤波方法可以快速准确地实现机器人在大环境中的定位。

1 栅格地图拓扑化

1.1 大环境下粒子分布问题

实验所选取的环境为标准的办公楼层，该办公楼短边长18 m，长边长76 m，总面积约为1 368 m2。在该办公楼中，对可进入的8个房间、电梯口、卫生间走廊进行地图构建，机器人在该环境中的可运行区域约为800 平方米。在已经建好的栅格地图中，通过单峰高斯分布与固定比率随机散布粒子对算法进行初始化，并采用粒子滤波算法进行定位测试实验。粒子初始化方式相同，仅将初始的粒子数分别设定为500、5 000与50 000个，500-5 000-50 000三种初始粒子数的分布点云图表明粒子散布密度随着粒子数目增加。三组初始粒子数量不同的定位实验中，500与5 000个粒子的实验由于初始粒子在真实位姿附近粒子分布过少，实验中得到粒子最终不收敛和错误收敛的结果；当初始粒子为5000，机器人的工控机因为内存不足而无法运行。实验证明三种粒子数值在该场景都难以很好的实现机器人的定位。实际大场景下机器人定位，必须对粒子的分布进行规划以平衡算法实时性与算法收敛成功率的关系。为了解决基于粒子滤波定位在大环境下的不足，提出一种基于拓扑化的栅格地图多峰粒子滤波定位算法，通过预处理过程中的节点预匹配，再通过多峰粒子滤波定位算法实现机器人的定位。

1.2 大环境的拓扑化节点

日常所涉及的建筑物内部空间会按照房间的功能性进行区分。栅格地图中空白区域用白色表示，障碍物用黑色表示，所以房间的空间和边界大小会有所不同，根据房间的大小不同将其拓扑化为单个的节点。提出的拓扑化的流程如下:

1)根据地图的大小设定相关区域的内核(即图1中圆形的区域)，内核只能存在于环境中的空白区域。

总体的区域O，空白区域的内核Xi：

O={X1,X2,…，Xn}。

(1)

2)遍历地图中的房间区域，根据可以容下内核的区域划分节点，内核的相关距离小于一定的距离D，默认为同一节点。

X1-X2

(2)

3)内核的大小根据环境的大小可以自行调节，重复步骤2)，得到新的节点信息，与已经划分的区域信息进行比较，若存在变化则更新节点信息，若没有更新就不改变原有的节点信息，得到最后的k个节点为机器人定位所用，

O′={X1,X2,…，Xk}

(3)

4)节点的分布满足相应的要求停止划分，得到相应的节点。

楼层节点划分如图1，依据图1可以发现，房间区域大小不同，所能容纳的内核数目不一样。同时地图中的房间都有很明显的边界，所以样本节点的边界也相对较明显，按照一定的剔除规则将节点分布到各个区域。本次节点的划分过程将电梯口以及卫生间走廊排除在外，选取到8个房间为样本节点。最终的节点的划分如图2所示，本次实验提取房间作为机器人可能存在的位置作为样本集，分成数字1～8如图3。

图1 拓扑化节点图

Fig.1 Graph of topological node

图2 节点划分表示图

Fig.2 Graph of node partition

图3 节点样本集

Fig.3 Sample set of node

2 基于拓扑节点室内定位算法

2.1 节点的预匹配定位

节点的预匹配定位是通过观测模型匹配和先验知识计算得到。栅格地图的匹配即通过激光雷达的点云数据和机器人所处环境的进行匹配。先验概率通过贝叶斯递推和机器人运动模型得到。为了消除相似的节点造成导致机器人定位失败，采用预匹配的定位方式提高定位的精度。通过机器人在不同的节点位置的激光点云生成的局部地图与节点样本集如图3所示进行匹配，实验数据如表1所示。

表1 节点匹配结果

Tab.1 Result of node matching

匹配率节点1节点2节点3节点4节点5节点6节点7节点8节点10.9960.8510.8940.4720.5760.3770.4120.471节点20.8510.9450.9670.4620.5570.3800.4140.462节点30.8940.9670.9850.4620.5570.3800.4140.462节点40.4720.4620.4620.9850.5670.5740.5850.641节点50.5760.5570.5570.5670.9120.4600.4940.554节点60.3770.3800.3800.5740.4600.9850.5880.574节点70.4120.4140.4140.5850.4940.5880.9550.584节点80.4710.4620.4620.6410.5540.5740.5840.955

分析可以发现只有当机器人所在节点的位置是机器人真实的位置，才会得到该节点比较高的匹配率，这也就很好地避免机器人定位失败的情形。但是实验可以发现，在节点1、2、3，还是发生了相互耦合情况，因为环境比较相似，单帧激光雷达的数据不足以很快的区分。机器人准确定位则需要联合运动模型的数据来匹配得到准确机器人的位置。

2.2 多峰粒子滤波定位

多峰粒子滤波定位算法是一种基于递归的Rao-Blcakwellised滤波器实现的在线SLAM算法。多峰粒子滤波定位算法是标准的粒子滤波定位算法的改进版本。通过将原有的单峰高斯分布的建议分布改进为多峰分布，结合机器人可能存在的多个位置进行概率预测，合理分布粒子的数目，通过预匹配可以得到机器人可能存在的位置，将粒子合理的分布在机器人可能存在的位置附近，从而加快机器人的定位。算法步骤如下：

采样该步骤中，机器人在t时刻，利用t-1时刻的状态估计xt-1,以及t-1时刻至t时刻的控制量ut，通过运动模型得到状态转移分布p(xt|ut,xt-1),并从状态转移分布中随机采样获取t时刻的假定状态xt[k]。

(4)

权重计算通过t时刻的观测模型函数，返回每一个粒子包含的激光信息与环境的匹配程度即粒子的置信度wt[k],

wt[k]=p(zt|xt[k])。

(5)

重要性重采样根据上一步骤得到的置信度，多峰粒子滤波定位同样基于粒子的顺序排序同时按照顺序抽样的法则进行重采样，顺序采样保证重采样产生的粒子向高权值粒子区域移动，以趋近于目标分布即机器人的位置。重采样粒子的概率与wt[k]成比例，产生的粒子按照建议分布和权重wt[k]的乘积分布：

bel(x0:t)=wt[k]p(xt|xt-1,ut)·p(x0:t-1|z1:t-1,u1:t-1)。

(6)

定位结果利用粒子集中每个粒子的位姿及最新的观测量来更新维护地图，得到地图的后验概率p(M|zt,ut)同时确定机器人在t时刻的位置。多峰定位根据样本集较高的权值的合理的选择建议分布函数可以的得到，通过提取匹配得到机器人可能存在的节点的概率，

p(Mt=Mi|z1:t,u1:t)=ηp(zt|Mt=Mi,z1:t-1,u1:t)p(Mt=Mi|z1:t-1,u1:t)=ηp(zt|Mt=Mi)p(Mt=Mi|z1:t-1,u1∶t)，

(7)

η表示归一化参数，Mt表示t时刻机器人的位置，Mi表示第i个节点,z1:t、u1:t-1分别表示机器人从开始到t时刻机器人的运动模型参数和观测模型概率。通过对每个节点中的粒子的权重求和归一化得到所有粒子的重心及机器人的真实位置，

(8)

3 定位实验数据对分析

实验选取图1的办公楼环境，本次实验设定房间1为机器人的初始位置。粒子的初始粒子数目为5 000，分别进行初始分布为多峰和单峰的对比定位实验。

3.1 多峰粒子滤波定位实验

多峰定位实验，机器人旋转提取多帧的激光数据融合得到房间1的全方位激光点云图。以该点云图生成的局部地图与节点样本预匹配，得到机器人的初始位置属于节点样本集1、2、3。如表1中的数据，以样本集1、2、3的房间中心点为高斯分布均值建立3个独立高斯分布粒子云作为该次机器人定位中的初始粒子点云，地图中的多峰高斯粒子点云图如图4所示。

3.2 单峰随机粒子滤波定位实验

全局随机单峰分布初始化下初定位实验为多峰高斯分布实验的对照试验。在本次实验中，粒子云只包含随机点云而未加入高斯分布点云以增大估计粒子搜寻到最佳位姿的可能性。全局随机分布初始化粒子点云图如图5(a)所示。算法将机器人的初始最优位姿设定为地图的中心，图5(b)为机器人定位完成的点云图，该图表明粒子随着机器人的运动不断向激光雷达数据匹配度较高的方向运行，此刻机器人的位置仍在房间1中，但由于粒子的重采样过程根据激光的点云数据匹配得到，由于存在相似的环境以至于重采样过程中大量的粒子收敛到房间2中导致定位失败。

图4 多峰分布定位点云图

Fig.4 Point cloud map of multi-peak distribution localization

图5 单峰随机分布定位点云图

Fig.5 Point cloud map of single-peak random distribution localization

3.3 实验数据对比分析

对比分析图4(a)、图5(a),定位初始化时，地图中都包含大量的粒子，每一个即为一个位姿估计粒子。初始化时每一个粒子被分配相同的权重，机器人的最优位置由所有的粒子云的重心位置决定。根据马尔科夫贝叶斯递推，下一刻的粒子权重由上一时刻的粒子重采样得到。通过节点预匹配将机器人可能的位置限制在节点样本集1，2，3内，故粒子的初始点云有3个，但随着机器人的运动、粒子的重采样，样本集中权重较小的粒子数会逐渐减少、逐渐向正确的粒子点云峰趋近。粒子最后都会收敛机器人的真实位置。但是通过对比可以发现，多峰分布实验中粒子收敛到机器人的真实位置，即放置机器人的房间1，但是单峰随机分布实验中，粒子也收敛但是定位在房间2中，定位失败。

机器人运行过程中的粒子均权重的变化曲线如图6所示，粒子的均权重总体呈上升趋势，最终会收敛到一个稳定的状态附近。在收敛的过程中粒子的均权重会出现较大的波动，分析可知，由于相似环境的存在，所以在匹配的过程中，粒子的权重变化会比较剧烈。后续的过程中均权重也会有起伏的变化，这是因为机器人在运行的过程中，由于里程计累计误差造成定位不准。总体分析可以发现，多峰高斯分布的收敛速度总体是要优于单峰随机分布的。

粒子均权重w：每一个粒子通过运动模型和观测模型计算得到可能表示机器人的真实位置的概率。

迭代次数n：机器人运行过程中粒子更新的次数。

图6 粒子均权重变化对比图

Fig.6 Particle weight change comparison chart

4 结论

本文针对室内大场景下基于粒子滤波的定位算法难以有效接近机器人的真实位姿，无法定位、定位错误的问题，提出了一种基于拓扑化栅格地图多峰高斯分布的估计粒子初始化方法。该方法相较于单峰随机分布粒子滤波定位算法，加入局部地图节点预匹配的过程，通过预匹配的过程改进定位的流程，该方法在根据房间的大小将不同的房间分化为不同的节点。在机器人开始定位前，机器人先通过采集四周环境信息，生成局部地图导入节点样本集模型中进行匹配，求解出机器人所处的最大概率节点。机器人的初始估计粒子将以样本集内的样本中心位置为均值构造的高斯分布组成，样本集内所含样本数决定该次初始化中高斯分布峰值个数。实验表明，初始化粒子在相同的条件下，定位更快更准确。