激光雷达运动畸变去除的算法设计

王 雪，李登峰，黄杉杉，龚庆红

(长安大学嵌入式系统应用研究所，陕西 西安 710061)

0 引言

为了降低成本，当前市场上绝大多数智能小车都使用一些低成本的移动底盘和激光雷达作为智能小车的主要传感器。成本降低通常会降低智能小车里面传感器的加工精度降低，从而导致里程计信息和激光雷达所扫描出来的数据信息的准确性降低[1]。而传感器轮式里程计的精度越差，智能小车运动过程中产生的累计误差也就越大，使智能小车所构建的环境地图效果也就越差。低成本的激光雷达一般采用单线雷达[2]，扫描频率较低。当扫描频率较低时，假如小车快速运动，激光雷达所扫描出来的位姿信息就会发生严重的畸变。为了解决上述问题，本文介绍了轮式里程计辅助方法的原理。但是由于轮式里程计辅助方法是位置线性假设，位置线性假设没有误差线性假设合理。因此，本文提出了轮式里程计和迭代最近点(iterative closest point，ICP)方法的融合。

1 传感器轮式里程计辅助算法

去除激光雷达运动畸变的方法有ICP、速度估计迭代最近点(velocity estimation iterative closest point,VICP)、里程计的辅助方法。但是ICP和VICP法存在一些问题：一是当激光雷达扫描频率是低帧率时，对小车匀速运动的假设不成立；二是数据预处理和状态估计过程耦合。因此，提出了传感器的辅助方法，包括里程计辅助方法和惯性测量组合(inertial measuring unit,IMU)辅助方法。此方法的优点是可以比较准确地估计局部位姿，还可以跟状态估计解耦。但IMU辅助方法测量线性加速度的精度较差，并且二次积分在局部的精度也很差。相比较于IMU辅助方法，轮式里程计辅助方法局部位置测量和角度测量的精度较高，速度更新更快[3]。

轮式里程计辅助方法的原理：运用分段线性插值函数估计每个激光点对应的位姿，将得到的位姿进行坐标转换，最后转换为激光数据并发布。

已知当前帧激光起始时间结束时间为ts、te，两个激光束之间的时间间隔为Δt。轮式里程计辅助算法步骤如下。

①利用拉格朗日插值法得到一条直线L1，而l

(1)

(2)

(3)

②用分段线性插值函数对二次曲线进行近似。

首先按照分割时间分段，然后对每个时间分段里面的激光点进行线性插值[4]，从而得出该分段中每个激光点在激光雷达坐标系下对应的里程计的位姿。

在ts、te时间段内，一共取k个位姿{Ps,Ps+1,...,Ps+k-2,Pe}。k个位姿通过线性插值获取。在这k个位姿之间进行拉格朗日线性插值。

设Ps、Ps+1之间有N个位姿{Ps,Ps+1,Ps+2,...,Ps+N-2,Ps+1}，则：

(4)

③坐标系的转换。

根据步骤②，可以求出每个激光点对应的位姿Pi。设x′i为转换之后的坐标、xi为转换之前的坐标，则：

(5)

(6)

(7)

2 轮式里程计方法和ICP方法的融合

上述轮式里程计方法采用的是位置线性假设，位置误差的线性假设比位置假设更加合理。因此，本文提出了融合的方法，即将轮式里程计方法和ICP方法进行融合。

融合算法的步骤如下。

①用轮式里程计辅助方法去除运动信息中绝大部分的运动畸变。

②用ICP方法进行匹配，把匹配的结果作为正确值，将得到的里程计误差值假设是线性分布的。

③把匹配得到的误差值平均到每一个激光点上，重新进行激光点的修正。

④进行ICP匹配方法的迭代，直到收敛为止。

ICP方法原理如下。

给定两个点云集合X,P，X为参考帧，P为当前帧的点云[5]。

(8)

式中：xi和pi为点云的坐标；Nx和Np为点云的数量[6]。

(9)

求解三维旋转矩阵R和平移向量t，使得误差E(R,t)达最小。

分别计算两组点云的质心：

(10)

对两组点云去质心：

(11)

要使E(R,t)取最小值，t=μx-R×μp，则：

(12)

(13)

3 试验结果与分析

3.1 激光雷达性能参数

试验使用的激光雷达选用FLASH LIDIR F4型号，其性能参数如表1所示。

表1 FLASH LIDIR F4激光雷达性能参数

3.2 激光雷达运动畸变去除试验

为了验证激光雷达运动畸变去除算法对同步定位与地图构建(simultaneous localization and mapping,SLAM)系统在实际环境中的影响[8]，在机器人系统(robot operating system,ROS)平台中通过其可视化界面(ROS visualization,RVIZ)观察两种算法所建的环境地图。智能小车移动的线速度设置为0.7 m/s。

未去除畸变的建图效果如图1所示。

图1 未去除畸变的建图效果

试验结果如图2所示。

图2 试验结果

从图1和图2可以明显看出，此时激光雷达所扫描得到的点云与原电梯间的地图情形已经偏移了一定的角度，说明此时激光雷达所扫描出来的数据产生了较大的畸变误差。图2(a)是使用轮式里程计辅助方法所构建的图，图2(b)是使用轮式里程计和ICP方法融合的算法所构建的图。比较图2(a)与图2(b)可以看出，虽然两种方法都可以有效去除激光雷达运动所产生的畸变，但是图2(b)的地图精度比图2(a)的精度高。

综上所述，当智能小车在运动速率比较快的情况下，激光雷达所扫描出来的信息会发生畸变[9]。通过比较两种算法的建图效果，本文设计的激光雷达运动畸变去除算法提高了SLAM系统构建环境地图的精度。

4 结论

针对激光雷达加速运动时产生的畸变[10]，本文分析了激光雷达运动产生畸变的原因，并阐述了轮式里程计辅助方法去除激光雷达运动畸变的原理；分析了轮式里程计辅助方法的不足，针对位置线性假设没有误差线性假设合理的问题，最后提出了轮式里程计和ICP方法的融合的方法。在智能小车作室内移动时，分别使用两种算法进行构建环境地图。试验结果表明，轮式里程计和ICP方法的融合方法比采用里程计辅助方法所构建的环境地图精度高。