一种移动机器人运动中三维激光测距数据动态补偿方法
【技术领域】
[0001] 本发明涉及移动机器人自主环境感知技术领域，主要用于移动机器人在未知动态 环境实时定位以及场景重构，特别涉及到对移动机器人在运动过程中通过三维激光测距传 感器获取的测距数据的动态补偿方法。本发明以移动机器人为运动载体，以三维激光测距 平台及里程计为测量装置，以车载计算机为数据收集及处理平台，对运动中移动机器人获 取的三维激光测距数据进行动态补偿。
【背景技术】
[0002] 激光传感器是移动机器人领域最重要的感知设备之一，具有测量精度高、受环境 干扰小等优点，但随着移动机器人技术的不断发展，基于单点或二维平面的测量方式已经 无法满足移动机器人的应用需求，如何获取精确的三维激光测距数据成为该领域的一个研 宄热点。
[0003] 目前，获取三维激光测距数据的方法主要分为三种。一种方法是通过对激光 点列进行高速旋转获取三维激光数据，其优点是可以保证三维数据的实时获取，缺点是 数据稀疏，垂直方向的视角有限。文献（Yu Y，Gao Z，Zhu B，et al.Recognition and Classification of Vehicle Target Using the Vehicle-Mounted Velodyne LIDAR[R]. SAE Technical Paper,2014. ；Huang P S，Hong W B，Chien H J，et al.Extrinsic calibration of a multi-beam LiDAR system with improved intrinsic laser parameters using v-shaped planes and infrared images[C]//IVMSP Workshop,2013 IEEE 11th. IEEE，2013:l-4.)中所使用的是Velodyne HDL-64E型三维激光测距传感器，该 设备能在水平方向以最快15Hz的频率进行高速旋转，在垂直方向上每个激光点列仅由64 个激光点组成，使得获取的三维激光数据比较稀疏，无法满足场景重构、环境理解等应用需 求，因此在使用时需要通过机器人位姿信息对多组三维激光数据进行整合。
[0004] 第二种获取三维数据的方法利用旋转平台给高精度二维激光增加一维旋转， 这种方法虽然可以保证测量系统的可视范围和数据密度，但是受二维激光本身特性 的限制，旋转云台的转速不能太快，从而导致三维激光数据采集速度较慢。文献（Hao Men,Biruk Gebre，Kishore Pochiraju. Color Point Cloud Registration with 4D ICP Algorithm[C]. Proceedings of IEEE International Conference on Robotics and Auto mation，Shanghai，China，2011，1511-1516. ; Jorge L Martinez，Antonio J. Reinaj Anthony Mandowj Jesiis Morales. 3D registratin of laser range scenes by coincidence of course binary cubes [J]. Machine Vision and Applications，2012, 23:857-867.)使用 二维SICK激光传感器配合横向或纵向旋转云台定点采集三维激光数据，再依靠惯导系统、 GPS定位系统或匹配算法对多个定点三维激光数据进行融合，从而获得完整的环境信息。这 种定点式的扫描方法破坏了机器人运动的连贯性，且由于云台旋转需要一定的时间，降低 了机器人的工作效率。
[0005] 第三种方法是将二维激光系统以一定角度固定在机器人平台上，在机器人 运动的同时积累二维激光数据，组成三维激光点云。该方法成本较低，实现简便，但 是三维点的精度完全依靠于每组二维激光数据采集时刻的机器人位姿精度。如文 献(Jan Elseberg, Dorit Borrmann and Andureas Nilchter. Algorithmic Solutions for Computing Precise Maximum Likelihood 3D Point Clouds from Mobile Laser Scanning Platforms[J]. Remot Sensing,2013, 5(11):5871-5906. ； Jose-Luis Blanco-Claraco, Francisco- Angel Moreno- Duenas and Javier Gonzalez-Jimenez. The Malaga urban dataset:High-rate stereo and LiDAR in a realistic urban scenario[J]. International Journal of Robotics Research, 2014, 33(2):207-214.)米 用固定角度激光传感器结合里程计、惯导系统等定位传感器采集室外环境的三维激光点 云，为了获得高精度的环境数据，对定位传感器的定位精度有较高的要求，而高精度定位系 统会大大增加整个系统的构建成本。
[0006] 对于移动机器人来说，获取准确详细的环境信息是其实现地图构建、场景理解、路 径规划等后续任务的先决条件，而数据的连续采集往往是决定任务成败的关键因素。仅仅 依靠三维激光传感器很难从数据密度、采集连贯性、系统成本等多个方面满足移动机器人 的多种应用需求。基于上述情况，本发明提出了一种适用于移动机器人运动过程中三维激 光测距数据动态补偿方法，实现移动机器人持续运动过程中对三维环境的扫描，提高了机 器人运行效率，并且以价格相对低廉的设备获取精确的三维激光数据。
【发明内容】
[0007] 本发明提出了一种里程计初步分段校正结合三维激光测距数据配准的方法，具体 技术方案如下：
[0008] (1)获取三维数据匹配模板
[0009] 移动机器人在初始位姿利用360度全景三维激光测距传感器对环境进行扫描，获 取三维环境的激光测距数据，通过坐标变换将激光测距数据变换到全局坐标系下构成点云 作为匹配模板。
[0010] ⑵连续数据分段
[0011] 在移动机器人运动过程中，设旋转云台旋转半周时间为一个周期，将每个周期分 割成5等份，在每一小段时间内利用移动机器人的里程计数据推算机器人位姿，并利用预 测的机器人位姿对运动过程中构建的点云进行初步校正。此分段校正方法利用里程计短距 离准确数据保证每小段时间内的激光点云局部形状的准确性。
[0012] (3)提取平面特征
[0013] 本发明采用基于区域扩张的平面特征提取方法。先拟合局部邻域平面，然后将局 部平面合并得到完整的平面特征。具体的，每个激光点有8个邻居（边界除外），直接反映 了激光在空间的邻域关系。定义一个kXk的邻域，将邻域内的激光点进行平面拟合。k可 以根据实际情况取不同的值，一般情况下取k > 3。每个邻域包含Pl，p2,... pN* N个激光 点，（邻域中可能出现无效扫描点，故N不一定等于kXk，）每个点Pi= [X，y, z] 7用向量表 示。设它们的重心为m有：
[0014]
【主权项】
1. 一种移动机器人运动中三维激光测距数据动态补偿方法，其特征在于：先获取三维 环境的点云作为匹配模板；对平面点云进行栅格化处理并计算各栅格内点云形状参数，构 建含有形状参数的向量，基于最小二乘法计算两幅点云之间的变换关系，校正机器人位姿， 进而对三维激光测距数据进行补偿；具体步骤如下： a) 移动机器人在初始位姿利用360度全景三维激光测距传感器对环境进行扫描，获取 三维环境的激光测距数据，通过坐标变换将激光测距数据变换到全局坐标系下构成点云作 为匹配模板； b) 在移动机器人运动过程中，设旋转云台旋转半周时间为一个周期，将每个周期分割 成5等份，在每一小段时间内利用移动机器人的里程计数据推算机器人位姿，并利用预测 的机器人位姿对运动过程中构建的点云进行初步校正； c) 定义一个kXk的邻域，将邻域内的激光点进行平面拟合，一般情况下取k> 3 ;拟合 N 的目标是使得D = Σ I)2取得最小值，其中Pi= [X，y，Z] τ为一个激光点，m为激光 f=l N 点重心，η为单位化法向量；引入邻域点集的协方差矩阵^ = Σ ((A - · (A - m))2得到D i = \ =nTAn，欲使D最小，引入拉格朗日乘子λ，使用拉格朗日乘子法得+ Ml -，《)) = 〇， CU 由此可得An = λη，从而可以得到D = ητλη = λητη = λ，D能达到的最小值，恰恰是协 方差矩阵A的最小特征值，而使D能取得该最小值的法向量η，就是A的最小特征值所对应 的特征向量；设定一个阈值Pthresh，如果求出A的最小特征值λ min小于P thMsh，则认为平面拟 合成功，否则，认为拟合失败，删除局部邻域拟合平面失败的点云； 又 -A d) 对保存的点云进行栅格化处理，计算栅格内的形状参数p =八2乂，P为栅格 内点云平面相似度，入"入2,人3为<5 =上Σ (f,)-仲'的三个特征值且λ 3， ? ,. = 1 其中η为栅格激光点的数目，Pi为栅格内第i个激光点，μ为栅格内点云重心；为每个栅格 构建含有其形状参数的向量=，其中￥1是5特征值最小的特征向量，即平面法向量，α P V1 用来修正栅格内点云重心位置和局部形状之间的权重；基于向量间的欧几里得距离最小准 则完成栅格匹配，结合配准约束CT (_[ μ ] χ δ r+ δ t)，其中C = ν,/Α，δ R为旋转分量， S t为平移分量；利用最小二乘方法求出两幅点云之间的旋转平移关系，校正机器人位姿， 进而校正三维激光测距数据。
【专利摘要】本发明提供了一种面向动态过程中的三维激光数据动态补偿方法，属于移动机器人自主环境感知技术领域；本发明最大的创新点在于改善了在运动过程中三维激光数据采集方式，且通过合理的数据精简方法，提高算法的执行效率；为此本发明采用动态数据匹配方法，矫正由里程计引入的机器人位姿误差，提高动态过程中获得三维激光数据的精度，另一方面，采用平面提取方法滤除杂乱的激光数据，降低匹配激光点数量，使得所提方法可以在线实时运行；本发明可用在移动机器人结构化场景重构等领域。
【IPC分类】G01C3-00, G06T7-00
【公开号】CN104657981
【申请号】CN201510006980
【发明人】闫飞, 庄严, 张思航, 吴乃亮
【申请人】大连理工大学
【公开日】2015年5月27日
【申请日】2015年1月7日