IMU 和轮式里程计联合的雷达畸变校正算法

刘佳豪，罗天放，王桐

1. 哈尔滨工程大学 信息与通信工程学院，黑龙江 哈尔滨 150001

2. 哈尔滨工程大学 先进船舶通信与信息技术工业和信息化部重点实验室，黑龙江 哈尔滨 150001

诸多商用机器人为了控制硬件成本，采用数据帧率较低的2D 激光雷达，导致机器人在快速移动时雷达数据将产生较大运动畸变[1]。迭代最近点(iterative closest point, ICP)算法[2]为经典的畸变校正算法，在两帧数据之间进行扫描匹配，但未考虑机器人的运动因素，虽然通过不断迭代能够收敛，但仍存在误差[3]；融合主成分分析算法(principal component analysis, PCA)的ICP 算法作为ICP 算法的改进，利用有效点对中欧式距离较大的点对完成精配准，但在点云较稠密时配准误差较大[4]；轮式里程计辅助法[5]在理想状态下精度较高，但在速度过快时驱动轮打滑的概率增加，所导致的角度误差难以处理[6]，致使高速状态下激光雷达角度误差不能够被准确校正；有学者提出轮式里程计与ICP 融合算法[7-8]，改进了轮式里程计位置线性假设的缺陷，将其转为位置误差的线性假设[9]，算法能够达到更高的畸变校正上限但无法处理轮式里程计打滑的问题。为解决轮式里程计辅助法缺陷，本文利用惯性测量单元(inertial measurement unit, IMU)传感器[10]在快速移动时仍然能够提供高频率准确航向角信息的特点，弥补轮式里程计的机械缺陷，提出了IMU 和轮式里程计融合的激光雷达运动畸变校正的方法。

1 激光雷达运动畸变产生原理

雷达运动畸变产生的主要原因是：激光雷达扫描数据并非瞬间完成却将其理想化为瞬间完成进行数据封装，忽略了激光雷达在单个数据周期内的运动[11]。设机器人处于封闭圆形区域，机器人向前做速度为v的匀速运动，雷达数据帧率f，其理想化数据应为图1 所示。

图1 理想的激光雷达数据

实际上，激光雷达在第1 束与最后1 束激光的时间间隔内随机器人运动，激光的发射点不断向前平移，实际的激光数据扫描情况为图2 所示，一帧中越靠后的激光束发射点位移越大，误差越大。

图2 实际的激光扫描情况

最后数据封装时，雷达被理想化为静止，所有激光发射点被统一到第1 束激光发射点处，最终得到畸变的雷达数据，如图3 所示。误差顺着扫描方向逐渐增加。当前帧数据最大畸变量ΔLmax约为一帧数据周期内机器人所移动的距离，在v=0.5 m/s、f=5 Hz 条件下有：

图3 畸变的激光雷达数据

可见数据畸变较大，不可忽略。上述论述的是机器人平移过程的位移畸变产生原理，具有一帧中越靠后的激光束发射点位移越大误差越大的特点。旋转产生的角度畸变原理与之类似，并且同样具有一帧中激光束越靠后角度误差越大的特点。

2 算法主要思想

低成本2D 激光雷达数据帧率低至5 Hz，轮式里程计数据帧率一般在100 Hz 以上，而IMU 数据帧率最高，可达200~1 000 Hz。雷达、轮式里程计、IMU 数据帧率对比如图4 所示。通过多传感器融合补全雷达在单数据周期内的运动信息[12]，从而校正由于运动带来的点云畸变。

图4 雷达、轮式里程计、IMU 数据帧率对比

激光雷达在一个数据周期内的移动使得每一束激光对应的雷达坐标系位置均不相同。算法通过轮式里程计数据获取激光雷达坐标系在一个数据周期内的运动情况，据此来初步修正每束激光数据。针对轮式里程计在机器人快速移动时容易打滑造成角度测量误差较大的问题，通过读取IMU 数据来获取一个数据周期内机器人准确的旋转运动情况，对角度误差实现更加精准的校准[13]。

校正过程涉及到多次坐标系转换，以下对相关坐标系进行说明：

1）里程计坐标系Odom[14]：{O}(OOXOYOZO)，全局唯一，数据来源于电机编码器。

2）一帧雷达数据中第i束激光对应的激光雷达坐标系Radari：{Ri}(ORXRYRZR)，每一束激光都拥有自己对应的{Ri}，该坐标系原点即为激光雷达发射点，激光雷达运动时对于不同i，{Ri}并不相同，即如果一帧雷达数据包含360 个激光点，则该数据周期内有360 个不同{Ri}。

3）基准坐标系Base：{B}(OBXBYBZB)，一帧雷达数据中第0 束激光对应雷达坐标系，{B}={R0}，所有激光点都需要校正到该坐标系进行数据封装。

首先借助里程计坐标系{O}为中间媒介，将所有激光各自对应的激光雷达坐标系{Ri}转换到该帧雷达数据的基准坐标系{B}下，完成轮式里程计辅助的初步校正；然后再通过IMU 数据获取激光雷达坐标系{Ri}与基准坐标系{B}之间精确的旋转变换关系，对初步校正后的各激光束再次进行精度更高的角度校正；最后以基准坐标系{B}来封装数据。

通常不带有磁力校准的IMU 在测量航向角时会存在逐渐增大的累计误差[15-16]，但算法中计算的是IMU 雷达扫描周期内的旋转增量，误差不会累积。算法的主要流程如图5 所示。

图5 算法流程

3 轮式里程计初步校正

3.1 激光束i 对应{Ri}与{O}的变换关系

如图6 所示，设激光点P在{Ri}中的坐标为(xr,yr)，在{O}中的坐标为(xo,yo)，则存在该激光点对应的{Ri}到{O}变换关系，该变换关系由旋转矩阵RR-O和平移向量TR-O组成。

图6 {Ri}和{O}变换关系

旋转矩阵RR-O变换为四元数QR-O便于插值：

设激光雷达当前数据帧的激光总数为N，激光束之间的间隔时间为Δt。由于不一定能够找到与激光束发出时刻完全对应的轮式里程计数据，为了实现第i束激光对应的激光雷达坐标系{Ri}在轮式里程计坐标系{O}当中的准确匹配，如图7 所示，需要对雷达数据帧进行分段，针对分段始末对应的{Ri}与{O}变换关系进行线性插值。

图7 激光雷达点云分段

每个分段中激光束个数为k，则第j段中的第1 束激光在雷达数据帧中编号为j×k，发出时刻为tstart，对应{Ri}在{O}中的变换关系由旋转矩阵RR-O[j×k]和平移向量TR-O[j×k]共2 部分组成，二者可通过轮式里程计数据获取。同理，最后1 束激光在数据帧中编号为(j+1)×k-1，发出时刻为tend，旋转矩阵RR-O[(j+1)×k-1]和平移向量TR-O[(j+1)×k-1]。

第i个点P的发出时间为ti，分段号为j，则有：

根据式(2)，将RR-O[j×k]转换为四元数QR-O[j×k]，RR-O[(j+1)×k-1]转换为四元数QR-O[(j+1)×k-1]进行插值，插值原理见式(3)、(4)以及图8、9。

四元数球面线性插值为

向量线性插值为

机器人在Vx=0.4 m/s、Az=0.45 rad/s 条件下，2D 激光雷达数据帧按每10 束激光进行分段处理，其中第7 段激光四元数QR-O[7×10]到QR-O[8×10]球面线性插值与向量TR-O[7×10]到TR-O[8×10]线性插值情况如图9 所示。

图9 四元数与向量插值

3.2 激光坐标系{Ri}转{O}

激光点需要由对应激光雷达坐标系{Ri}变换到轮式里程计坐标系{O}。假设第i个激光点P在雷达极坐标系中角度为angles[i]，距离为ranges[i]；转换为{R}中的坐标(xr,yr)，此时{Ri}与{O}变换关系由四元数QR-O[i]和平移向量TR-O[i]表示，有：

结合式(2)得到图6 中{Ri}与{O}夹角α：

结合式(1)将该激光点在{Ri}的坐标(xr,yr)转换为在轮式里程计坐标系{O}中的坐标(xo,yo)：

3.3 激光坐标系{O}转{B}

如图10 所示，设激光点P在{B}中的坐标为(xb,yb)，则存在该激光点对应的轮式里程计坐标系{O}与基准坐标系{B}变换关系。

该变换关系由旋转矩阵RO-B和平移向量TO-B表示，设:

将RO-B转换为四元数QO-B：

QO-B转欧拉角得到图10 中{O}与{B}的夹角θ：

该激光点在{O}中的坐标(xo,yo)转换为在{B}中坐标(xb,yb)完成激光束数据的里程计初步校正：

4 IMU 激光雷达角度误差校正

通过读取IMU 数据可以得到激光束i对应激光雷达坐标系{Ri}与轮式里程计坐标系{O}之间的旋转变换关系，该关系通过四元数QIMU[i]给出：

四元数转欧拉角得到从IMU 获取到的激光雷达坐标系{Ri}与轮式里程计坐标系{O}夹角γ：

初步校正中通过轮式里程计数据已知轮式里程计坐标系{O}与基准坐标系{B}之间的夹角θ，则由式(5)可得{R}与{B}基准坐标系之间的夹角ϕ，即当前激光束从第1 束激光发出到当前激光束时刻发生的角度偏移量，angles[i]- ϕ 即为激光束i经过IMU 修正后的最终角度。至此激光数据校正完毕。

为了体现各个校正变换的效果，将机器人置于一半径1.6 m 规则圆形场地内进行实验，机器人在Vx=0.4 m/s、Az=0.45 rad/s 条件下，激光雷达帧按每10 束激光进行分段处理，数据如图11 所示。

图11 校正前后激光雷达数据曲线

未校正的雷达数据曲线在220°之后（尾部）有明显的上升趋势，这是因为一帧中越靠后的激光束发射点位移越大，激光雷达距离测量误差越大。

完成轮式里程计初步校正后的雷达数据无明显上升趋势，其距离数值在场地的半径（1.6 m）附近正常波动，但是其角度校正精度较差，雷达数据存在角度误差。

对比分析完成IMU 激光雷达角度误差校正后的雷达数据，算法利用IMU 数据完成了角度滞后误差的校正，所以数据曲线相对于完成轮式里程计初步校正后的雷达数据有一定的角度超前，且一帧中越靠后的数据角度超前量越大，符合前述的角度误差特点。

5 实验验证与结果分析

实验采用机器人操作系统(robot operating system, ROS)麦克纳姆轮全向机器人为平台，采用Rviz 插件作为雷达数据可视化工具，相关误差数据利用ros_bag 工具导出由origin 软件绘图。激光雷达采用思岚RPLIDAR A1 单线激光雷达，IMU 采用MPU6050，直流电机自带霍尔编码器。机器人、激光雷达、IMU 霍尔编码器实物及相关参数分别如图12、表1~表3 所示。

表1 思岚RPLIDAR A1 参数

表2 MPU6050 参数

表3 直流电机参数

5.1 位移误差

机器人前向速度Vx=1.15 m/s，所得激光雷达点云数据如图13 所示。

未校正雷达一帧点云数据中的前部数据误差不大，但由于RPLIDAR A1 是逆时针扫描，误差按照逆时针方向逐渐增大，在当前帧的尾部达到最大，地图左下方边缘与实际地图存在较大错位。

对比分析联合校正后的点云数据，各部分均与实际静态地图贴合。校正前后位移误差曲线如图14 所示。

图14 激光雷达位移误差曲线

联合校正后位移误差相比校正前大大减小，误差最大值由10.5 cm 减小为0.85 cm，由此可见联合校正下位移误差得到有效矫正。

5.2 角度误差

在慢速（Vx=0.4 m/s，Az=0.45 rad/s）情况下，如图15、16 所示，未校正的数据帧前部角度误差较小，中部误差略大，尾部误差最大约为5°，雷达数据与静态地图存在较明显错位。

图15 低速角度校正点云

图16 低速角度误差曲线

对比分析里程计初步校正与联合校正后的数据，两者点云均与静态地图基本重合，因为在低速情况下驱动轮不易打滑，里程计数据尚且较为准确。但是里程计校正误差曲线整体略高于联合校正误差曲线，可知相对于未校正的雷达数据，两者角度误差均得到有效校正，但联合校正精度相比里程计校正更高。

在高速（Vx=0.85 m/s，Az=1.2 rad/s）情况下，如图17、18 所示，未校正的雷达数据存在最大为15°的角度误差，而轮式里程计校正的数据结果其数据帧尾部也存在10°的角度误差，因为高速状态下驱动轮多次不可避免地细微滑动，轮式里程计并不能够提供准确的机器人旋转数据，且该误差将持续累积，导致雷达数据与地图错位。

图17 高速角度校正点云

图18 高速角度误差曲线

对比分析IMU 里程计联合校正的数据，其点云始终与静态地图吻合，角度误差曲线不超过2°。由于IMU 不存在轮式里程计驱动轮打滑的问题，机器人角速度较大时依然能够准确提供姿态信息，将角度误差进行良好校正。所以在高速状态下，角度误差校正实际表现远优于轮式里程计校正。

综上所述，低速状态下轮式里程计校正和联合校正均具备较好的角度校正效果，但是高速状态下联合校正克服了轮式里程计的机械性缺陷，具备更好的角度校正性能。

5.3 地图构建质量

低速（Vx=0.4 m/s，Az=0.45 rad/s）情况下的机器人地图构建结果如图19 所示。轮式里程计辅助法与IMU 轮式里程计联合校正下的地图构建结果均较为理想，没有发生明显的地图错位，说明在机器人速度较低的条件下，驱动轮不容易发生打滑现象，2 种方法均能够很好地校正激光雷达运动畸变。

图19 低速建图对比

高速（Vx=0.85 m/s，Az=1.2 rad/s）情况下的机器人地图构建结果如图20 所示。地图可大致分为A、B、C 共3 大区域，其中C 为地图主体区域。机器人由A 经过咽喉要道1 后进入C 区域，再经过咽喉要道2 进入B 区域完成探索。

图20 高速建图对比

轮式里程计辅助法中，机器人从A 进入C 时在1 处急转弯时，由于高速行驶下驱动轮打滑，根据轮式里程计数据进行矫正的激光雷达数据发生较大角度误差，使刚进入C 区域时获取到的C 区域地形信息随之附带相应的角度错误，导致A 区域构图结果相对于地图的主体区域C 区域存在明显的顺时针角度错位。同样，从C 区域经由咽喉要道2 进入B 区域时也发生了类似情况，导致B 区域地图相对于C 区域也存在逆时针方向的角度错位。

IMU 轮式里程计联合校正算法中，由于激光雷达数据的角度信息最终是由IMU 进行校正，雷达数据的角度误差校正不受轮式里程计打滑的影响，所以即使是在高速状态下依然能够实现较好的建图质量，不会产生地图不同区域之间的角度错位。少数墙体因为机器人速度太快、采集激光束过少，造成墙体构建较薄、障碍物点稀疏，该问题可通过反复探索来解决。

综上所述，机器人在构建地图时通过狭窄咽喉通道进入新区域，由于进入新区域后难以再扫描到旧区域的环境，新区域与旧区域之间的连接区域的信息大部分在该通道中获得（如图20 中的1、2 处），如果此时机器人驱动轮打滑会使得里程计数据错误从而使得激光雷达运动畸变反而加剧，2 个区域的连接处产生角度错位，得到错误的地图。

而IMU 不存在该机械性缺陷，即使在高速情况下依然能够准确反映激光雷达在自身一个数据周期内的旋转情况，从而消除在此间隔内由于激光雷达本身的运动造成的激光雷达运动畸变。

6 结论

针对低帧率2D 激光雷达的运动畸变问题，本文分析了雷达运动畸变的产生原因，阐述了轮式里程计辅助校正的不足，针对其在快速移动时容易打滑的问题，提出了IMU 和轮式里程计融合激光雷达运动畸变校正方法。

算法通过轮式里程计获取校正位移误差的运动信息，通过IMU 获取校正角度误差的运动信息，利用线性插值完成2 个传感器与雷达数据的时间戳匹配。轮式机器人实机验证表明，该算法相比于传统里程计辅助法具备更高的角度误差校正精度，能够有效克服轮式里程计在机器人快速移动时打滑的机械缺陷，消除此问题所带来的角度误差，大大提升机器人地图构建的质量，极大改善了低帧率2D 激光雷达的动态性能。