激光雷达数学模型及运动畸变去除

黄 越，郁汉琪，陆宇豪

（南京工程学院 机械工程学院，南京 211100）

激光雷达作为SLAM系统的主要传感器，影响定位准确度与地图构建效果。

1 激光雷达传感器介绍

1.1 激光雷达常用测距原理

对于市面上的主流激光雷达，主要是用于环境探测、地图构建，按技术路线可分为：三角测距激光雷达，TOF激光雷达。

TOF测距是通过测量光的飞行时间来计算距离。根据发射激光信号的不同形式，TOF测距法可分为脉冲测距和相位测距[1]。脉冲激光测距基本原理是激光器发射激光脉冲，计时器记录发射时间T1；接收器接收被反射的激光，计时器记录接收时间T2。两个时间的间隔△T即为激光的“飞行时间”。光速c是一定的，因此距离可由速度与时间计算得到。相位测距是将发射的连续的激光进行幅度调制（调制光的光强随时间做周期性变化）[2]，被反射后，激光产生相位变化，根据相位差来间接测量时间和距离（曲线测距）[3]。

三角测距根据摄像头的光斑成像位置，利用相似三角形的几何关系计算距离。双目视觉、结构光测距等，都可归纳为三角测距原理。激光器发射激光，激光照射到物体表面后发生反射；基线另一端的CCD相机接收反射信号，并记录入射光与反射光的夹角。依照光学路径，不同距离的物体会显示在CCD上的不同位置。因此，在已知激光源和CCD间距离的情况下，根据相似三角形的几何关系[4]，就可以推导出激光源与物体表面的距离。

1.2 激光雷达数学模型介绍

1）光束模型

激光雷达的光束模型需要第t时刻机器人的位姿xt和地图m，来获得第t时刻激光数据，包含K个激光束测量的距离值zt。在非结构化环境中机器人位姿微小的变化就会造成期望值巨大的改变，从而导致得分进行突变，因此光束模型很少有应用场景。

2）似然场模型

似然场模型是对图像进行高斯平滑，在任何环境中期望值对于位姿都是平滑的。似然场得分的技术不需要经过光线追踪，而是通过高斯分布计算出一个得分换成查表的形式。似然场模型从程序一开始就是在进行离线计算，所以对一帧激光来说，只需要查360次表，同时适合结构化环境和非结构化环境中[5]。

2 激光雷达运动畸变

2.1 运动畸变产生的原因

运动畸变是由于激光雷达在采集数据过程中，随着载体运动，导致一帧激光数据中的激光点不是在同一时刻采集的，即不同激光点的坐标不一致。在静止场景中，即激光雷达载体本身静止且环境中没有运动物体，那么采集到的一帧点云没有畸变。在运动场景中，即激光雷达载体运动或者环境中存在运动物体，采集到的点云数据有畸变现象。采集一帧数据起始点的雷达原点位置和采集一帧数据终止点时的雷达原点位置出现了偏移，三维环境信息出现了畸变。

由于雷达计算激光点坐标时，都是以接收到激光束时刻的雷达自身坐标系为基础的，所以载体运动过程中，每一列激光点的基准坐标系都是不一样的，但是他们在同一帧点云里，希望能统一在同一个坐标系下，所以需要知道每次采集时的坐标系相对于初始时刻坐标系的转换关系。

2.2 运动畸变的去除

点云的畸变是由于一帧数据的采集过程中，雷达载体进行运动导致，因而需要先计算采集过程中雷达的运动，然后在每帧中根据每个点的相对时间进行补偿这个运动量，包括旋转和平移的补偿。

去除激光雷达运动畸变的方法有包括ICP（Iterative Closest Point）、VICP（ Velocity Estimation Iterative Closest Point）的纯估计方法和传感器的辅助方法。ICP方法没有考虑到激光的运动畸变，以及当前激光数据错误问题；VICP法忽略了低频率激光雷达扫描对小车匀速运动的假设不成立，以及数据预处理和状态估计过程耦合的问题。针对ICP和VICP方法的缺点，主要介绍传感器的辅助方法以及提出轮式方法与匹配方法进行融合从而去除运动畸变。

3 传感器辅助方法

传感器辅助方法包括惯性测量单元（IMU）辅助方法及轮式里程计辅助方法。惯性测量单元辅助方法是直接测量机器人的角速度和线加速度，具有较高的角速度测量精度[6]，但测量频率极高，一般在1kHz～8kHz，并且线加速度精度太差，二次积分在局部的精度依然很差。轮式里程计直接测量机器人的位移和角度，具有较高的局部角度测量精度[7]，与IMU辅助方法相比，更新速度更高，一般在 100kHz～ 200kHz。

轮式里程计方法的原理是求解当前帧激光数据中每一个激光点对应的机器人位姿，根据求解的位姿把所有激光点转换到同一坐标系下，重新封装成一帧激光数据发布出去。

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

1）求解 ：ts，te时刻的位姿ps，pe

里程计队列正好和激光数据同步，假设第i和第j个数据的时刻分布为ts，te：

在ts时刻没有对应的里程计位姿，则进行线性插值。设在l，k时刻有位姿，且l＜s＜k，则 ：

2）二次插值

在一帧激光数据之间，认为机器人做匀加速运动。机器人的位姿是关于时间t的二次函数，设：tm=(ts+te)/2，且l＜m＜k，则 ：

已知：ps，pm，pe，则可以插值一条二次曲线：

3）二次曲线的近似

用分段线性函数对二次函数进行近似，分段数大于3时近似误差可忽略不计。在ts和te时间段内一共取m个位姿{ps,ps+1,…,ps+m-2,pe}，位姿通过线性插值获取，在这m个位姿之间，进行线性插值：

设ps和ps+1之间有n个位姿{ps,ps1,…,ps(n-2),ps+1}，假设si在l和k之间，则：

4）坐标系统转换与激光数据发布

一帧激光数据K个激光点，每个激光点对应的位姿{p1,p2,…,pk}，通过上述的方法插值得到。xi为转换之前的坐标，为转换之后的坐标，则：

把转换后的坐标转换为激光数据发布出去：

4 轮式方法与匹配方法的融合

4.1 融合方法思想

本文提出了将轮式里程计方法与ICP方法进行融合，其主要思想如下：

1）用里程计方法进行矫正，去除绝大部分的运动畸变。

2）认为里程计存在误差，但是误差值是线性分布的。

3）用ICP方法进行匹配，匹配的结果作为正确值，得到里程计的误差值。

4）把误差值均摊到每一个点上，重新进行激光点位置修正。

5）再一次进行ICP迭代，直到收敛为止。

4.2 实验结果分析

4.2.1 激光雷达性能参数

试验使用的激光雷达选用思岚RPLIDAR A2型号，其性能参数见表1。

表1 思岚RPLIDAR A2性能参数Table 1 RPLIDAR A2 performance parameters

4.2.2 激光雷达去畸实验

为了验证激光雷达运动畸变去除算法对同步定位与地图构建（Simultaneous Localization And Mapping，SLAM）系统在实际环境中的影响[8]，在机器人系统ROS平台中采用两种算法构建环境地图，在可视化界面RVIZ中显示环境地图。设置智能移动底盘移动的线速度为0.5 m/s。

两种算法所建地图如图1、图2所示。

从图中可以明显看出，当移动底盘发生运动时，激光雷达所扫描的点云数据发生了畸变现象。比较图1与图2可以看出，采用轮式里程计与ICP融合的方法去除激光雷达产生运动畸变的效果更好，所建地图精度更高。通过比较两种算法的建图效果，提出的轮式里程计与ICP融合方法提高了系统构建环境地图的精度。

图1 辅助方法的建图效果Fig.1 Mapping effect of the auxiliary method

图2 轮式里程计和ICP融合方法的建图效果Fig.2 Mapping effect of wheel odometer and ICP fusion method

5 结论

本文介绍了激光雷达的测距原理以及数学模型，并根据载体运动时激光雷达采集数据过程中产生的运动畸变，详述了传感器辅助方法去除激光雷达运动畸变的原理。在此基础上，针对轮式里程计辅助方法的不足，提出了轮式里程计和ICP 融合的方法。当智能底盘在室内移动时，分别使用两种算法进行构建环境地图。实验结果表明，轮式里程计和 ICP 融合的方法所构建的地图精度更高。