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

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

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

图1 SLAM流程图 Fig.1 Flow diagram of SLAM

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

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

m2 （ Edist( ,T) min|Rp T） q| ij R ,T,j {1,2, ，n} i 1

T

s.t. R R Il,det(R ) 1

×

(1)

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

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

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