基于三维激光扫描仪的室内移动设备定位与建图*

赵梓乔 肖 力

(华中科技大学自动化学院 武汉 430074)



基于三维激光扫描仪的室内移动设备定位与建图*

赵梓乔 肖 力

(华中科技大学自动化学院 武汉 430074)

论文采用惯导与里程计组合辅助点云配准的方式实现了室内环境下移动设备的定位与地图建立，克服了自主研制的三维扫描仪数据更新频率低的缺点。首先，采用惯导与里程计组合完成移动设备的姿态测量与航位推算；然后，将航位推算的结果作为点云配准的初始值，之后采用基于正态分布变换的点云配准算法完成点云的顺序配准；最后，根据点云配准结果修正航位推算的定位结果，并在此基础上完成定位与建图。论文以轮式小车为实验平台，搭载自制的三维激光扫描仪，在楼道环境中验证了文中所述方法的有效性。

点云; 惯导; 正态分布变换; 定位; 建图

Class Number TN249

1 引言

在未知环境中定位与环境地图的建立是设备自主导航的基础，特别是在无GPS信号的室内环境中。定位与地图信息是设备运动与行为决策的基本前提。在室内环境中一般采用基于WLAN[1]、Zigbee[2]以及RSSI[3]等无线电技术的定位方式，此类方式需要在定位环境中布置大量无线节点，受障碍物的影响较大，定位精度无法满足设备自主导航的要求。

目前室内未知环境中主要采用机器视觉和二维或三维点云数据获取移动设备的运动信息，同时建立地图信息。其中基于视觉的同步定位与地图构建(Simultaneous Localization and Mapping, SLAM)系统发展较快，Stephen Se等利用机器视觉采用尺度不变的地标完成了移动机器人的全局定位与建图，但实际应用中机器视觉受环境光线影响较大，系统鲁棒性低[5]。Michael Bosse、 Robert Zlot等将型号为Hokuyo UTM-30LX的二维激光扫描仪安装在弹簧上使其旋转来获取三维点云信息，并由此完成三维空间的定位与建图，这种由二维扫描仪获取三维点云的方式在复杂环境中存在积累误差，不适合大范围场景的应用[6]。Diego Viejo使用型号为LMS-200 Sick的三维激光扫描仪实现了空间中六维位姿的估计，但该传感器成本较高，推广应用较为困难[7]。

三维激光扫描仪受环境光照影响小，测量精度高，可以直接获取空间的三维信息。本文中使用的三维激光扫描仪为自主研制的低成本三维扫描仪，但目前扫描仪数据更新频率较低。为克服其数据更新频率低的缺点，本文采用间隔扫描的方式采集点云数据，期间通过惯导与里程计组合进行航位推算得到设备的位姿变化量，并将其作为点云配准的初始值，之后采用基于正态分布变换的点云配准算法[4]完成点云的顺序配准。最后根据配准结果修正航位推算得到的位姿，同时建立三维点云地图。实验中对比了无惯导与里程计辅助的配准方式，证明了此方法在实际应用中的可行性与有效性。

2 惯导与里程计组合定位

2.1 基于kalman滤波器的姿态测量模型

(1)

(2)

(3)

(4)

采用陀螺仪的测量值进行卡尔曼滤波器的状态更新，加速度计与磁力计的测量值作为观测值，根据四元数微分方程与陀螺仪误差测量模型建立卡尔曼滤波器的7维状态方程[9]如下：

(5)

加速度计与磁力计的测量值作为观测量建立卡尔曼滤波器的观测方程：

z=H(x,t)+v(t)

(6)

其中观测矩阵

将系统状态方程(5)离散化，同时将卡尔曼滤波器的观测方程(6)线性化。根据卡尔曼滤波器的离散状态方程与线性化之后的观测方程，进行卡尔曼滤波器的迭代计算便可得到滤波之后的姿态信息。

2.2 姿态与里程计组合航位推算

采用光电编码器作为里程计传感器，相对于传统的里程计，光电编码器有更高的测量精度。车头方向为载体坐标系的X轴正方向，光电编码器的采样周期为Δt，则在Δt时间内车辆的位移在载体坐标系中可以表示为

(7)

其中ΔS=KDnD为载体在X轴方向上位移，nD为光电编码里程计的脉冲个数，KD为里程计的刻度系数。

结合上一小节得到的姿态，载体在导航坐标系中的位移可以表示为

(8)

其中ψ与θ分别表示偏航角与俯仰角。根据式(8)累加各个采样周期内载体的位移，进行航位推算即可得到设备的实时位置。

3 NDT点云配准与建图

(9)

NDT配准的具体步骤如下：

1) 根据参考点云P各点的位置，将各点存储到m个栅格中，栅格表示为B={b1,b2,…,bm}；

2) 计算各个栅格中点云的分布函数；

3) 计算NDT配准的目标函数的函数值，采用牛顿法迭代计算最优的六维位姿向量；

4) 牛顿迭代法收敛或迭代次数带到最大迭代次数时配准完成。

室内环境中如楼梯、过道等场景之间相似度高，当两幅点云相差距离较大时配准失败的可能性增加，同时配准迭代的次数增加，耗费更多时间。惯导与里程计组合的航位推算在短距离范围内的积累误差较小，将航位推算得到的位姿变换作为NDT配准的初始值可以减少配准的迭代次数，增加配准成功的可能性,同时利用点云配准结果可以减小航位推算的积累误差。本文中使用的三维激光扫描仪为实验室自主研制，360°全景扫描时的点云数据更新频率在20s左右，惯导与里程计组合航位推算的更新频率为20Hz。整个定位与建图过程采用停—走—停模式，总的步骤如下：

图1 NDT配准的流程图

1) 在起始位置全景扫描得到一幅点云数据作为初始参考点云加入全局点云地图；

2) 开始移动，期间惯导与里程计组合进行航位推算实时更新移动设备的位置与姿态；

3) 停止移动，全景扫描得到此处点云数据，以航位推算得到的位姿变换作为初始值将点云数据与上一幅点云数据进行NDT配准，将配准后的点云加入全局点云地图；

4) 根据配准结果修正航位推算的位姿，返回步骤2)。

4 实验结果与分析

本文中的移动设备为轮式小车，作为里程计的编码器与车轮刚性连接，用于获取三维点云的三维激光扫描仪为实验室自制，如图2所示。此外，姿态测量的惯性传感器采用ADIS16405，其内部集成陀螺仪、加速度计以及磁力计，通过SPI接口输出传感器的原始测量数据。实验在实验室外的楼道中进行，整个楼道呈长条状，总长约75m，每间隔3m～4m进行一次全景扫描。

图2 三维激光扫描仪实物

楼道环境单一，直接采用NDT配准后两点云之间仍存在较大偏差，如图3所示，图中的网格边长为1m。两幅点云中点个数都在16万左右，配准过程总共迭代27次，耗时6.597s。当采用航位推算辅助配准后，配准过程总共迭代16次，耗时3.968s，迭代次数减少了9次，时间减少了2.6s，如图4所示，局部放大部分两点云之间相差约2cm。图5中黑色线条为移动设备在楼道点云地图中的移动轨迹。点云配准完成后可以修正航位推算的积累误差，如图5中箭头所指的地方。图6为整个楼道的点云地图的俯视图，图中网格的边长为2m。

图3 无航位推算的配准结果

图4 航位推算辅助的配准结果

上述算法均在Qt 5.4.2环境下采用C++编程实现，点云与行进轨迹的显示采用OpenGL直接绘制。执行程序的PC平台为：Intel(R) Core(TM) i5-4430 CPU @ 3.00GHz 四核，RAM 4.00GB DDRIII。

图5 设备在点云地图中的移动轨迹

对比上述结果，可以发现惯导与里程计组合的航位推算不仅可以加快点云配准的速度，而且提高了点云配准的成功率。短距离高频率的航位推算可以很好地弥补实验室自主研制的三维扫描仪数据更新频率低的缺点，同时间断性的点云配准数据可以修正航位推算的积累误差，两者之间相辅相成。

图6 楼道点云地图

5 结语

本文介绍了一种基于三维激光扫描仪的定位与点云地图构建系统。首先，建立基于kalman滤波器的姿态测量模型，由惯导的测量计算得到的设备的实时姿态；然后，与编码器测量得到的里程数据组合进行航位推算得到移动设备的实时位姿；之后采用基于正态分布变换的点云配准算法配准间隔扫描得到的点云数据，配准的初始值由航位推算得到；最后，根据配准结果修正航位推算的定位结果，并完成点云地图的构建。实验证明本系统有效地克服了三维扫描仪数据更新速度慢的缺点，实现了高频航位推算与低频点云数据的结合。

[1] 郎昕培,许可, 赵明.基于无线局域网的位置定位技术研究和发展[J].计算机科学,2006(6):21-24. LANG Xinpei, XU Ke, ZHAO Ming. Research &Development of WLAN-based Location Techniques[J]. Computer Science,2006(6):21-24.

[2] Yuh-Ming Cheng.Using ZigBee and Room-Based Location Technology to Constructing an Indoor Location-Based Service Platform[C]// Intelligent Information Hiding and Multimedia Signal Processing, 2009. IIH-MSP’09. Fifth International Conference on.12-14 Sept,2009:803-806.

[3] Junhui Zhao,Yuqiang Zhang,MengjieYe.Research on the Received Signal Strength Indication Location Algorithm for RFID System[C]// Communications and Information Technologies, 2006. ISCIT '06. International Symposium on.Oct.,2006,18:881-885.

[4] Peter Biber and Wolfgang Stra?er. The normal distributions transform: A new approach to laser scan matching[C]// In Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS). 27-31 Oct. 2003:2743-2748.

[5] Stephen Se, David G. Lowe, James J. Little. Vision-based global localization and mapping for mobile robots[J].IEEE Transactions on Robotics，2005,21(3)：364-375.

[6] Michael Bosse, Robert Zlot, and Paul Flick. Design of a Spring Mounted 3D Range Sensor with Application to Mobile Mapping[J].IEEE Transactions on Robotics,2012,28(5):1104-1119.

[7] Diego Viejo Miguel Cazorla.A robust and fast method for 6DoF motion estimation from generalized 3D data[J]. Autonomous Robot,2014,36(4):295-308.

[8] J. F. Guerrero-Castellanos,H. Madrigal-Sastre,S. Durand; N. Marchand.Design and implementation of an Attitude and Heading Reference System (AHRS)[C]// 2011 8th International Conference on Electrical Engineering Computing Science and Automatic Control (CCE).26-28 Oct. 2011:1-5.

[9] R. R. Lima, L. A. B. Trres.Performance Evaluation of Attitude Estimation Algorithms in the Design of an AHRS for Fixed Wing UAVs[C]// Robotics Symposium and Latin American Robotics Symposium (SBR-LARS),2012,16-19:255-260.

[10] Besl P J, McKay N D. Method for registration of 3-D shapes[J]. Robotics-DL tentative. International Society for Optics and Photonics,1992:586-606.

[11] Martin Magnusson, Achim Lilienthal,Tom Duckett.Scan registration for autonomous mining vehicles using 3D-NDT[J].Journal of Field Robotics,24(10):803-827.

Localization and Mapping Indoor for Mobile Devices Based on Three-dimensional Scanner

ZHAO Ziqiao XIAO Li

(School of Automation,Huazhong University of Science & Technology, Wuhan 430074)

In this paper, a solution is proposed to locate and map for mobile devices indoor by registration point cloud assisted with INS and odometer, it overcomes the shortcomings of the three-dimensional scanner's low data updating rate. First, the INS and odometer measure are used to accomplish mobile device's attitude and dead reckoning, then normal distribution transform (NDT) algorithm is applied to point cloud registration, as the initial value is the result of dead reckoning. Finally, the point cloud registration correction dead reckoning, and accomplish the localization and mapping. The effectiveness of the method described in the paper is demonstnated in a corridor, which using the wheeled trolley equipped with a three-dimensional laser scanner.

point cloud, INS, normal distribution transform, localization, mapping

2016年5月6日,

2016年6月25日

赵梓乔，男，硕士研究生，研究方向：点云处理，同时定位与建图。肖力，男，硕士生导师，研究方向：三维扫描技术与导航系统。

TN249

10.3969/j.issn.1672-9722.2016.11.032