基于Voronoi图的移动机器人SLAM算法

李羚¹，张奇志¹，周亚丽¹

(1. 北京信息科技大学 自动化学院，北京，100192)

摘要：针对移动机器人同步自定位与地图创建(SLAM)过程中最近邻点匹配耗费时间长问题，将一种基于Voronoi图的最近邻点算法应用到SLAM中，该算法根据Voronoi图原理对栅格地图进行2次横向扫描来获得地图上所有栅格的最近邻点，提高了机器人建图的效率，仿真结果验证了该方法的可行性和适用性。

关键词：Voronoi图；SLAM；扫描匹配；最近邻点

中图分类号：TP24          文献标志码：A         文章编号：1672-7207(2011)S1-0453-05

SLAM approach with Voronoi diagram

LI Ling¹, ZHANG Qi-zhi¹, ZHOU Ya-li¹

(1. School of Automation, Beijing Information Science & Technology University, Beijing 100192, China)

Abstract: For the time-consuming of scan matching in the SLAM, a closest point algorithm based on Voronoi diagram is applied in SLAM. It gets the closest point with two transverse scans to improve the efficiency of mapping for robot. The simulation results prove the exactness and applicability of the algorithm.

Key words: Voronoi diagram; SLAM; scan matching; closest point

近年来，随着计算机技术、微电子技术、网络技术的快速发展，移动机器人的应用领域越来越广泛，从制造领域开始向非制造领域发展，海洋开发、宇宙探测、采掘、建筑、医疗、农林业、服务、娱乐等行业都提出自动化和机器人化的要求。同时，移动机器人在军事侦察、扫雷排险、防止核污染和化学污染等危险与恶劣环境中的物料搬运，特别是在一些具有危险性的操作中也具有广泛的应用。为了获得更大的独立性，人们也对机器人的灵活性及智能提出更高的要求,要求机器人能够在一定范围内自主移动且在移动中能自主完成特定的任务，增强对环境的适应能力，因而移动机器人在陌生环境下实现同步自定位与地图创建(SLAM)问题已成为众多研究人员所关注的一个焦点^[1]。

1  基于粒子滤波的SLAM算法

基于Rao-Blackwellized粒子滤波的SLAM算法^[2]如图1所示，其具体算法如下：

(1) 根据初始坐标，随机生成由N个粒子位姿s₀ⁱ、粒子权值为1/N组成的粒子集；

(2) 根据k-1时刻粒子集中的每个粒子位姿s_k-1ⁱ及运动模型计算k时刻粒子位姿s_kⁱ；

(3) 根据由运动模型运算所得粒子位姿、机器人观测数据及观测模型对粒子集进行匹配；

(4) 根据匹配结果对粒子权值进行更新；

(5) 根据粒子权值对粒子集进行重采样，去除权值小的粒子，复制权值大的粒子；

(6) 对环境地图进行更新。

此算法中的每个粒子代表了机器人一种可能的运动轨迹，同时每一个粒子都具有自己的全局地图与该粒子的轨迹相对应，因此可以能够较好地近似移动机器人的位姿和环境地图的联合概率密度，同时采用一种自适应的重采样算法，将粒子集保持在误差允许范围内，减少粒子发散程度，从而降低此粒子滤波算法的复杂度，移动机器人最终获得的权重最高的粒子所代表的就是移动机器人运动轨迹和全局地图。

图1  SLAM流程图

Fig.1  Flow diagram of SLAM

在SLAM算法中，扫描匹配是最耗时的一个步骤，同时也是本文中改进的部分。它处理距离传感器在一定范围内的扫描信息，并且在传感器感知的信息和先验地图的信息之间找到符合度最好、一致性最好的匹配^[3]，从而可以确定机器人所在的位置，实现定位，同时将当前时刻与上一时刻构建的地图进行匹配矫正，新增的点作为全局地图的新信息来更新上一时刻的全局地图。

本文中采用的匹配算法为基于ICP规则的迭代最近点(ICP)算法，假设P={p₁，…， p_m}为观测点集，Q={q₁，…，q_n}为参考点集，则ICP算法可以表示为

s.t.               (1)

其中：R_α∈R^l×l为旋转变换矩阵；α为旋转角；T∈R^l为平移向量，即通过最小化误差和E_dist求得2个点集之间的相对坐标变换x=(α，T)。

在ICP迭代开始前，需要预设移动机器人相邻时刻的初始相对位姿变化，这个设置对匹配结果影响很大，设置不合适会增加算法所需迭代次数，甚至会导致算法收敛到某个局部最优值或匹配失败。根据移动机器人运动的连续性，将前一时刻ICP的配准结果作为下一时刻的初始相对位姿变化假设。由于SLAM过程中匹配模型等计算过程中需要频繁调用栅格地图中点的最近邻点，为了方便存取数据，本文建立一个三维的最近邻查找表，存放最近邻距离及其对应点坐标。采用传统的最近邻点算法直接计算得到所有点的距离。当栅格地图尺寸为n×n，其中背景点(障碍点)个数为n_b时，传统算法涉及的平方运算次数为n²×n_b，在整个SLAM程序中耗时最长。对于移动机器人创建的栅格地图^[4-5]，环境空间的分辨率与栅格尺寸有关，增加分辨率将会增加运算时间和计算及内存消耗^[6]，因此传统的最近邻点算法不易进行分辨率高的地图创建。

本文通过引入基于Voronoi图的最近邻点算法，可大大缩短建立最近邻查找表的时间，进而缩短整个SLAM程序运行的总时间。

2  Voronoi图定义

Voronoi图^[7]是关于空间邻近关系的一种基础数据结构(如图2所示)，Voronoi图把平面分成n个区域，每个区域包括1个顶点，该点所在区域是到该点欧几里得距离最近的点的集合。对平面点集P为元素的Voronoi图来说，平面被划分为凸多边形区域，P中每个元素p_i对应一个区域V(p_i)，使得V(p_i)内的任何点距离p_i都比距离其他元素近，V(p_i)定义如下：

  (2)

图2  Voronoi图

Fig.2  Example of Voronoi diagram

3  基于Voronoi图最近邻点算法

3.1  定义和定理^[8-9]

定义1  (2-D栅格地图) 令R₁和R₂是栅格地图grid中的2点，定义顺序关系_y，_y如下：

     (3)

定理1  如图3所示，有背景点u，v和w(u_xxx),uv为点u与点v垂直平分线穿过直线y=r的点，vw为点v与点w垂直平分线穿过直线y=r的点，则观察图形可知，当uv_x≥vw_x时，直线y=r与点v所属的Voronoi多边形区域无交集，即对于所有纵坐标为r的点的最近邻背景点可排除点v。

图3  当uv_x≥vw_x时，点v的Voronoi区域与直线y=r无交集

Fig.3  Voronoi region of v does not intercept line r if uv_x≥vw_x

3.2  算法流程^[10]

基于Voronoi图的最近邻点算法基本思想是：只将直线y=r(前景点纵坐标)穿过的Voronoi区域的背景点列为最近邻点的候选点，以此提高对批量前景点求最近邻点的效率。具体流程如下：首先对栅格地图进行_y顺序扫描，求出前景点在直线y=r下方的最近邻点，然后再对栅格地图进行_y顺序扫描，求出前景点在直线y=r上方的最近邻点，最后对2次得出的最近欧几里得距离求最小值，以此类推，即可得到整张地图上所有点的最近邻点。基于Voronoi图的最近邻点算法详见函数Algorithm CDT()。

Algorithm CDT()

输入：栅格地图grid

输出：栅格地图中每个点的最近邻距离CDT及对应的背景点集合Closest_p

第1步：对栅格地图进行_y顺序扫描

for i←1,nl do

F←Φ;

if i ≥2 then Cr_i ←Lr_i-1

else Cr_i ←Φ;

end if

for all center p of a cell R∈ε_ri do

if R is backgound labeled then

Cr_i ←Cr_i U_x{p};

CDT(R)←0;

else

F←F U_x{p};

CDT(R)←∞;

end if

end for

Lr_i=delete_sites(Cr_i );

k←1, l←size(Lr_i);

for all points p∈F do

while(k≤l) do

d←d_e(p,Lr_i[k]);

if d

CDT(R)←d;

Closest_p(R)←Lr_i[k];

else

break;

end if

k=k+1;

end while

end for

end for

第2步：对栅格地图进行_y顺序扫描

在Algorithm CDT()函数中，nl为栅格地图行数，例如，分辨率为100×100的栅格地图，其行数为100；F为前景点集合；Cr_i为前景点(纵坐标为r_i)的最近邻背景点的候选点集合；Lr_i为直线y=r_i 穿过的Voronoi区域的背景点集合；p为栅格地图中的点；ε_ri表示栅格地图中纵坐标为r_i的点的集合；F U_x{p}表示将点p添加到集合F中，然后将集合F按横坐标x由小到大排序；d_e函数的输出是两输入点的欧几里得距离。函数delete_sites()实现将候选点集合Cr_i中直线y=r_i未穿过的Voronoi区域的背景点删除，以此减少计算欧几里得距离的背景点个数，其原理是本文中的定理1。在遍历过程中，此算法可减少栅格地图中纵坐标为r_i及大于r_i的点的最近邻候选背景点个数，以此缩短计算所有点的最近邻背景点的总时间。

4  实验结果

实验环境：装有SICK公司LMS200激光测距仪的移动机器人SunⅡ(如图4所示)，Intel(R) Core(TM) Duo CPU T2450主频 2.00GHz，内存为2.00GB的计算机，Matlab软件平台。

测试数据：移动机器人在实验室大厅(如图5所示)采集到的激光数据。

将未改进与改进后的最近邻点算法分别应用到SLAM程序中，最终绘制的地图如图6所示。从图6可以看出：改进后的SLAM程序可以很好地绘制出大厅的地图。

图4  移动机器人SunⅡ

Fig.4  Mobile robot SunⅡ

图5  实验室大厅

Fig.5  The lab hall

图6  2种最近邻点算法的SLAM运行结果

Fig.6  Results of SLAM with two closest point algorithms

在不同地图尺寸，即栅格地图点数不同的条件下，2个SLAM程序的运行时间对比如表1所列。从表1可以看出，本文所采用的基于Voronoi图的最近邻点算法的SLAM程序运行时间远小于基于传统最近邻点算法的SLAM程序运行时间，且地图尺寸越大，其优势越大。

表1  采用不同算法的SLAM程序运行时间

Table 1  Execution times of SLAM with two closest point algorithms               s

5  结论

将基于Voronoi图的最近邻点算法运用到移动机器人SLAM应用中，有效地减少了栅格地图中数据匹配的时间。采用此算法绘制的地图比较精确，该算法能够很好地减少移动机器人SLAM程序的运行时间，有较强的适用性，同时对于其他类似的求解最近邻点的问题有一定的借鉴和参考意义。

参考文献：

[1] Wang W H, Chen W D, Xi Y G. Uncertain information based map building of mobile robots in absolutely unknown environment[J]. Robot, 2001, 23(6): 563-568.

[2] Montemerlo M, Thrun S. Simultaneous localization and mapping with unknown data association using Fast SLAM[C]// Proceeding of the 2003 IEEE International Conference on Robotics and Automation. Taipei, China: IEEE Press, 2003: 1985-1991.

[3] 杨明, 王宏, 张钹. 基于激光雷达的移动机器人位姿估计方法综述[J]. 机器人, 2002, 24(2): 177-183.
YANG Ming, WANG Hong, ZHANG Bo. Overview of laser radar based pose estimation for mobile robots[J]. Robot, 2002, 24(2): 177-183.

[4] Elfes A. Sonar-based real world mapping and navigation[J]. IEEE Journal of Robotics and Automation, 1987, 3(3): 249-265.

[5] Elfes A. Occupancy grids: A probabilistic framework for robot perception and navigation[D]. Pittsburgh: Carnegie Mello University. Department of Electrical and Computer Engineering, 1989: 10-30.

[6] Avots D, Lim E, Thibaux R, et al. A probabilistic technique for simultaneous localization and door state estimation with mobile robots in dynamic environments[C]//Proceeding of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems. Lausanne, Switzerland: IEEE Press, 2002: 521-526.

[7] Aurenhammer F. Voronoi diagrams-a survey of a fundamental data structure[J]. ACM Computing Surveys, 1991, 23(3): 345-405.

[8] Coeurjolly D, Zerarga L. Supercover model, digital straight line recognition and curve reconstruction on irregular isothetic grids[J]. Computers & Graphics, 2006, 30(1): 46-53.

[9] Fabbri R, Da L, Costa F, et al. 2D Euclidean distance transform algorithms: A comparative survey[J]. ACM Computing Surveys, 2008, 40(1): 1-44.

[10] Antoine Vacavant. Fast distance transformation on irregular two-dimensional grids[J]. Pattern Recognition, 2010, 43(10): 3348-3358.

(编辑 赵俊)

收稿日期：2011-04-15；修回日期：2011-06-15

基金项目：北京市属高等学校人才强教深化计划资助项目(PHR201106131)

通信作者：张奇志(1963-)，男，辽宁彰武人，博士，教授，从事噪声智能控制和机器人智能控制等研究；电话：13693240372；E-mail：zqzbim@yahoo.com.cn