赵希宇，沈一鸣

(沈阳工业大学 机械工程学院， 辽宁 沈阳 110870)

0 引言

同时定位与地图构建(SLAM, Simultaneous Localization and Mapping)技术是移动机器人领域最热门的技术之一[1]。在SLAM中最常用的算法之一是由Besl和McKay开发的迭代最近点(ICP, Iterative Closest Point)算法，它用于估计两个点云的相对转换关系[2]。ICP算法是基于非概率的定位方案，它具有原理简单、实现成本低的特点[3]，但是由于ICP-SLAM算法的实时性很差，在大环境下的工作稳定性也下降明显，在目前地图工作环境越来越复杂的情况下传统的ICP-SLAM已经不再满足系统对实时性的要求。

针对以上的问题，本文提出一种高效的基于ICP的SLAM算法，通过将地图按照固定距离分割成若干子地图，分别在各个子地图中进行ICP计算，再利用子地图与全局地图坐标系的转换计算机器人的位姿。本方法在一定程度上减少了由于实时性降低所产生的系统累计误差，可以满足较大范围SLAM的精度需求。

1 ICP算法

ICP算法用于在同一个坐标系下对两个点集进行匹配。已知有两个数据点集P和Q，其中pi∈P,qi∈Q，通过迭代使两个点集之间最近点的距离平方和缩小来求得其变换矩阵。

二维平面中的两个2D点可用pi=(xi,yi)和qi=(xj,yj)来表示，其几何距离可表示为：

(1)

其中：qi=Rpi+T+Ni,i=1,2,…，N，R为旋转变换矩阵，T为位移变换矩阵，Ni为第i点的噪声，N为点的数目。两个点集当中所有最近点几何距离的平方和为：

(2)

再通过迭代使E收敛到设置的阈值。

1.1 基本ICP算法

根据Rusinkiewicz和Levoy提出的理论[4],ICP算法利用扫描匹配方法通过迭代使两个扫描数据对齐,在每次迭代中，算法选择最近的点作为对应关系，计算变换的齐次矩阵进行对齐。……