第42卷第5期2021年5月
白动化仪表
PROCESS A U T O M A T I O N INSTRUMENTATION
Vol. 42 No. 5
王雪，李登峰，黄杉杉，龚庆红
(长安大学嵌人式系统应用研究所，陕西西安710061)
摘要：近年来，激光雷达的同时定位和地图构建(SLAM)算法发展相对成熟。然而，移动底盘与激光雷达作为激光S L A M系统的主要传感器，当传感器的精度降低时，将会影响机器人定位的准确度与构建地图的效果。因此，针对精度低的激光雷达快速运动时而产生的畸变，论述了轮式里程计的辅助算法。由于轮式里程计的辅助方法是位置线性假设，因此导致该算法在地图构建精度方面存在不足。针对 位置线性假设没有误差线性假设合理的问题，提出了轮式里程计和迭代最近点（ICP)方法的融合。采用智能小车在室内进行移动构建环 境地图。试验结果表明，轮式里程计和I CP方法融合的算法能够有效地提高定位的精度和地图构建的准确性。 关键词:S L A M；激光雷达；运动畸变；里程计；IC P；融合；精度；传感器
中图分类号：TH-39 文献标志码：A D0I：10. 16086/j. cnki. issn 1000-0380. 2020100044
Design of Algorithm for Removal of Lidar Motion Distortion
WANG Xue,LI Dengfeng,HUANG Shanshan,G0NG Qinghong
(I n s t i t u t e of Embedded System Application,Chang' an University,Xi*an 710061 ,China)
Abstract：In recent years, the simultaneous positioning and mapping (SLAM) algorithm of lidar has been relatively mature. However,mobile chassis and lidar are the main sensors of the laser SLAM system. When the accuracy of the sensor decreases,it will affect the accuracy of the robot's positioning and the effect of building a map. Therefore,in view of the distortion caused by the rapid movement of the low-precision lidar, the auxiliary algorithm of the wheel odometer is described. Because the auxiliary method of wheel odometer is the assumption of linear position,the algorithm has insufficient map construction accuracy. Aiming at the problem that the linear assumption of position is reasonable without error, the fusion of wheel odometer and iterative closest point (ICP) method is proposed. Using smart cars to move indoors to build environmental maps,the experimenta
l results show that the algorithm fusion of wheel odometer and iterative closest point ( IC P) method can effectively improve the accuracy of positioning and the accuracy of map construction.
Keyw ords：SLAM；Lidar；Motion distortion；Odometer；ICP；Fusion；Accuracy；Sensor
〇引言
为了降低成本，当前市场上绝大多数智能小车都 使用一些低成本的移动底盘和激光雷达作为智能小车 的主要传感器。成本降低通常会降低智能小车里面传 感器的加工精度降低，从而导致里程计信息和激光雷 达所扫描出来的数据信息的准确性降低m。而传感器 轮式里程计的精度越差，智能小车运动过程中产生的 累计误差也就越大，使智能小车所构建的环境地图效 果也就越差。低成本的激光雷达一般采用单线雷达[2]，扫描频率较低。当扫描频率较低时，假如小车快 速运动，激光雷达所扫描出来的位姿信息就会发生严重的畸变。为了解决上述问题，本文介绍了轮式里程 计辅助方法的原理。但是由于轮式里程计辅助方法是 位置线性假设，位置线性假设没有误差线性假设合理。因此，本文提出了轮式里程计和迭代最近点（iterative closest point,ICP)方法的融合。
1传感器轮式里程计辅助算法
去除激光雷达运动畸变的方法有ICP、速度估计 迭代最近；存(velocity estimation iterative closest poi
标本夹nt，VICP)、里程计的辅助方法。但是IC P和VICP法存在 一些问题：一是当激光雷达扫描频率是低帧率时，对小 车匀速运动的假设不成立；二是数据预处理和状态估 计过程耦合。因此，提出了传感器的辅助方法，包括里
收稿日期：2020-10-23
基金项目：中国博士后科学基金资助项目（2016M600756)、陕西省重点产业创新链基金资助项目（2018ZDCXL-GY-05-03-02)
作者简介：王雪（1998—），女，在读硕士研究生，主要研究领域为智能机器人，**********************;
李登峰（通信作者），男，硕士，副教授，主要研究领域为嵌入式系统、智能机器人、新能源汽车，E-mail:1319597096@ qq. com
• 90 •自动化仪表第42卷
程计辅助方法和惯性测量组合（inertial measuring unit，
IMU)辅助方法。此方法的优点是可以比较准确地估
计局部位姿，还可以跟状态估计解耦。但IMU辅助方
法测量线性加速度的精度较差，并且二次积分在局部
的精度也很差。相比较于1MU辅助方法，轮式里程计
辅助方法局部位置测量和角度测量的精度较高，速度
更新更快[3]。
轮式里程计辅助方法的原理：运用分段线性插值
函数估计每个激光点对应的位姿，将得到的位姿进行
坐标转换，最后转换为激光数据并发布。
已知当前帧激光起始时间结束时间为两个
淀粉加工
激光束之间的时间间隔为。轮式里程计辅助算法
步骤如下。
①利用拉格朗日插值法得到一条直线L,，而
t可以得出^时刻的位姿P,。时刻的位姿P，求解
过程类似。
0[l]■〇[k](1)
\pt =
L'—
-p〖x k-
t t - l
-+P,X----
l k k-l(2)
(3)
② 用分段线性插值函数对二次曲线进行近似。
首先按照分割时间分段，然后对每个时间分段里
面的激光点进行线性插值[4]，从而得出该分段中每个
激光点在激光雷达坐标系下对应的里程计的位姿。
在人时间段内，一共取A个位姿|P,，P+,，...，P,+t_2，P,1。&个位姿通过线性插值获取。在这A个位
姿之间进行拉格朗日线性插值。
设 P,、八+1 之间有 y v个位姿 |P,,P,+,,P s+2,...，丨丨，则：
P，L(P'，P”V)(4)
③ 坐标系的转换。
根据步骤②，可以求出每个激光点对应的位姿尸,。设 <；为转换之后的坐标、为转换之前的坐标，则：
x\= P\x xi(5)
4 = (P,，P，)
r =V P,x p+ py x p}
a = atan2( P v，/\)
2轮式里程计方法和IC P方法的融合
(6)
(7)
上述轮式里程计方法采用的是位置线性假设，位置
误差的线性假设比位置假设更加合理。因此，本文提出
了融合的方法，即将轮式里程计方法和ICP方法进行
融合。
融合算法的步骤如下。
① 用轮式里程计辅助方法去除运动信息中绝大部 分的运动畸变。
② 用ICP方法进行匹配，把匹配的结果作为正确 值，将得到的里程计误差值假设是线性分布的。
③ 把匹配得到的误差值平均到每一个激光点上，重新进行激光点的修正。
④ 进行ICP匹配方法的迭代，直到收敛为止。
ICP方法原理如下。
给定两个点云集合为参考帧，p为当前帧
的点云[5]。
A_I尤1，太2，. . .，％/V1
*(8)
P - \p”Pi，-，.，P n)
式中：X，和P，为点云的坐标；' 和为点云的
北斗通信模块
数量[6]。
1N p
E(R,t)= -―^I x{ - R x p.- t\~(9)求解三维旋转矩阵和平移向量i，使得误差 £(/?，《)达最小。
分别计算两组点云的质心：
r i〜
/X= —V X.
1$
卜=设，
对两组点云去质心：
x\ = x i - ^Lx
要使£(/?,f)取最小值，t=/x，则：
(10)
(11)
Np Np Np
E(R^) - R x (p, - ^P)V =7r X(x，i~ R x p\)2\x\Tx\^ RT Rp\\=
t t Y, \x?x\+p?p\\+ t t X i ~ (12) iyP«'=i iyP*=i
要使m/^)取得最小值，士 i丨<；制丨应取得最大值，则：
/V M*'=l
第5期激光雷达运动畸变去除的算法设计王雪，等
• 91 •
N f x ’
:RP :=^(i «p >：t)
Nn
(13)
N p
令
i ":*:T =t /2>T。当 n
"T 时，M R H )
i =
1
取得最大值，则£(/?，〇取得最小值[71。
3试验结果与分析
3.1激光雷达性能参数
试验使用的激光雷达选用FLASH LIDIR F 4型号， 其性能参数如表1所示。
表1
FLA SH L ID IR F 4激光雷达性能参数
Fig . 1 Lid ar performance parameters
项目单位
太阳能恒温器
典型值储压器
测距范围m 0. 1~8扫描角度(°)0-360测距分辨率mm <；实际距离1%
角度分辨率(°)0. 45测量频率Hz 4 000扫描频率
Hz
7
3.2激光雷达运动畸变去除试验
为了验证激光雷达运动畸变去除算法对同步定位 与地图构建（simultaneous localization and mapping ,
飞机部件修理
SLAM )系统在实际环境中的影响[8]，在机器人系统 (robot operating system ，ROS )平台中通过其可视化界
面（ROS visualization , RVIZ )观察两种算法所建的环境
地图。智能小车移动的线速度设置为0.7 m /s 。
未去除畸变的建图效果如图1所示。
图1
未去除畸变的建图效果
Fig. 1 Mapping effect without removing distortion 试验结果如图2所示。
图2试验结果从图1和图2可以明显看出，此时激光雷达所扫描
得到的点云与原电梯间的地图情形已经偏移了一定的 角度，说明此时激光雷达所扫描出来的数据产生了较大 的畸变误差。图2(a )是使用轮式里程计辅助方法所构 建的图，图2(b )是使用轮式里程计和ICP 方法融合的 算法所构建的图。比较图2(a )与图2(b )可以看出，虽 然两种方法都可以有效去除激光雷达运动所产生的畸 变，但是图2(b )的地图精度比图2(a )的精度高。
综上所述，当智能小车在运动速率比较快的情况 下，激光雷达所扫描出来的信息会发生畸变[9]。通过比 较两种算法的建图效果，本文设计的激光雷达运动畸变 去除算法提高了 SLAM 系统构建环境地图的精度。
4结论
针对激光雷达加速运动时产生的畸变1，本文分
析了激光雷达运动产生畸变的原因，并阐述了轮式里 程计辅助方法去除激光雷达运动畸变的原理；分析了 轮式里程计辅助方法的不足，针对位置线性假设没有 误差线性假设合理的问题，最后提出了轮式里程计和 1C P 方法的融合的方法。在智能小车作室内移动时， 分别使用两种算法进行构建环境地图。试验结果表 明，轮式里程计和ICP 方法的融合方法比采用里程计 辅助方法所构建的环境地图精度高。
参考文献：
[1] 宋书亮.室内移动机器人SLAM 算法设计[D ].哈尔滨：哈尔滨
工业大学,2019.
[2] 丁业峰.室内移动机器人激光雷达导航算法研究[D ].哈尔滨：
哈尔滨工业大学,2019.
[3] 周乐天.建筑机器人移动定位技术研究[D ].成都：电子科技大
学,2019.
[4] 张丽娟.三种插值方法的应用与比较[J ].赤峰学院学报（自然
科学版），2010,26(3):1-3.
[5] 戴静兰，陈志杨，叶修梓.IC P 算法在点云配准中的应用[J ].中
国图象图形学报，2007,12(3) :5丨7-521.
[6] ZUO C,LU M,TAN Z G,et al. A novel algorithm for registration of
point clouds [ J ]. Chinese Journal of Lasers, 2012,
39(12) ： 1214004.
[7] ARUN K S. Least-squares fitting of two 3-D point sets[ J ]. IEEE
Transctions on Pattern Anal Mach Intell, 1987(9) ： 11-15.
[8] 吴波.基于激光雷达和反光板的移动机器人定位方法研
究[D ].武汉：华中科技大学，2019.
[9] 王博玮，陆中成.基于云的餐厅服务机器人系统设计[J ].自动
化仪表，2019,40(8) :65-69.
[10] 杨记周，胡广宇，许玉云，等.室内移动机器人平台设计与实
Fig. 2 Experimental results
现[J ].自动化仪表，
2019,40(5) :75-78.
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议