SLAM学习日志(二) 2D(单线)激光雷达运动畸变校正(2 原理篇)

1. 背景

2D激光雷达运动畸变校正的介绍见上一篇：SLAM学习日志(一) 2D(单线)激光雷达运动畸变校正(1 介绍篇)
具体代码实现见本人的github链接： 2d_lidar_undistortion
emm这篇博文其实是我毕业设计的一部分。我的毕业设计内容主要是从头实现一个2D激光SLAM的框架，由于机器人在运动过程中，旋转的运动畸变对地图质量产生较大影响，就很想先通过IMU数据来实现激光雷达数据的畸变校正。当时查阅网络发现相关的具体资料不是很多，就自己花了几天时间研究并实现了出来。想法主要参考了深蓝学院的激光SLAM课程。
考虑到机器人运动速度较快时，平移带来的畸变同样不可忽视，因此，我也实现了订阅ROS里程计数据进行旋转平移校正的内容，后续也会补充进GitHub。

2. 激光雷达运动畸变校正原理

本论文激光雷达运动畸变校正的原理是：首先，在计算机接收激光雷达数据的同时，以较高频率接收IMU的姿态角数据，并将其保存在循环队列中。然后，根据激光雷达数据的时间戳以及激光雷达的采样间隔，求得每个激光点在采样时的精确时间。最后，在IMU队列中查找在时间上相邻的两帧IMU数据。假设机器人在相邻IMU数据帧间为匀速运动，通过球面线性插值的方法估计每个激光点在测量时刻的姿态，最终依次将同一激光数据帧的所有激光点变换至同一时刻的机器人姿态下。

2.1 球面线性插值公式

上述的球面线性插值（Spherical linear interpolation），是四元数的一种线性插值运算，主要用于在两个表示旋转的四元数之间平滑差值，常以 $\operatorname{Slerp}\left(q_{0}, q_{1}, \mu\right)$ 表示，其中 $\mu \in[0,1]$ 。
球面线性插值的概念最早由Ken Shoemake提出，被广泛用于计算机图形学。其分别给出了在几何形式下与在单位四元数形式下的球面线性插值公式：

几何形式：
$\cos (\Omega)=q_{0} \cdot q_{1}$
$\operatorname{Slerp}\left(q_{0}, q_{1}, \mu\right)=\frac{q_{0} \sin ((1-\mu) \Omega)+q_{1} \sin (\mu \Omega)}{\sin (\Omega)} \tag{1}$
单位四元数形式：
$\operatorname{Slerp}\left(q_{0}, q_{1}, \mu\right)=q_{0}\left(q_{0}^{-1} q_{1}\right)^{\mu}\tag{2}$

由于四元数的幂运算较为复杂，会耗费大量计算资源，所以在实际应用时，通常使用公式2进行球面线性插值的计算。

2.2 畸变校正计算流程

SLAM学习日志(二) 2D(单线)激光雷达运动畸变校正(2 原理篇)
假设当前对最新收到的激光雷达的第n个雷达扫描点进行畸变校正。如上图所示，激光雷达数据帧开始时刻为 $t_{\text {lidar}}$ ,激光雷达采样周期为 $\Delta t$ ，则该扫描点的采样时刻为 $t_{n}=t_{\text {lidar}}+(n-1) \Delta t$ 。经查询得到该扫描点相邻两帧IMU数据的采样时刻与姿态分别为： $t_{m}$ 、 $q_{m}$ 与 $t_{m-1}$ 、 $q_{m-1}$ 则计算可得：
$\mu=\frac{t_{n}-t_{m}}{t_{m-1}-t_{m}}\tag{3}$
根据公式1，可以得到： $q_{n}=\operatorname{Slerp}\left(q_{m}, q_{m-1}, \mu\right)\tag{4}$
已知该雷达扫描点相对于激光雷达坐标系的空间坐标 $p_{n}=\left[x_{n}, y_{n}, z_{n}\right]^{\mathrm{T}}$ ，并将四元数 $q_{0}$ 、 $q_{n}$ 分别转换至 $\mathbf{R}_{0}$ ， $\mathbf{R}_{n}$ ,最后，通过坐标变换公式可以计算得到该扫描点校正后的坐标：
$p_{n}^{\prime}=\mathbf{R}_{0}^{-1} \mathbf{R}_{n} p_{n}\tag{5}$

3. 参考文献

Shoemake K. Animating rotation with quaternion curves[J]. ACM Siggraph Computer Graphics, 1985, 19(3): 245-254.