激光雷达点云去畸变：IMU后向传播原理与实践

1. 项目概述

在激光雷达(LiDAR)与惯性测量单元(IMU)融合的SLAM系统中，点云去畸变是一个关键的前处理步骤。由于LiDAR在扫描过程中存在运动，采集到的点云会因传感器自身运动而产生畸变。FAST-LIVO2提出了一种基于IMU后向传播的点云去畸变方法，本文将深入解析其数学原理和实现细节。

注意：这里的"后向传播"特指从帧结束时刻向历史时刻反向补偿点云畸变的过程，与神经网络中的反向传播是完全不同的概念。

2. 核心问题与基本概念

2.1 点云畸变产生的原因

激光雷达在扫描过程中，每个点的采集时刻不同。当雷达本身在运动时，不同时刻采集的点实际上处于不同的传感器位姿下。如果不进行校正，这些点会被错误地认为是在同一时刻采集的，从而导致点云畸变。

2.2 去畸变的基本思路

传统方法通常采用前向传播的方式，从扫描开始时刻逐步补偿运动。而FAST-LIVO2采用了反向传播的思路，主要基于以下考虑：

1. 帧结束时刻的位姿通常更准确，因为它是IMU积分的最新结果
2. 反向传播可以避免累积误差的影响
3. 实现上更高效，可以与后续的配准步骤更好地衔接

2.3 关键术语定义

3. 数学推导

3.1 坐标系定义

首先明确各坐标系的定义：

1. 世界坐标系W：首帧IMU位置为原点
2. IMU坐标系I(t)：t时刻IMU本体坐标系
3. LiDAR坐标系L(t)：t时刻LiDAR本体坐标系
4. 目标坐标系L_E：帧结束时刻t_E的LiDAR坐标系

3.2 坐标变换链

我们需要构建从L(t_i)到L_E的完整变换路径。设有一个点P^L在L(t_i)坐标系下的坐标为(x,y,z)，我们需要计算它在L_E坐标系下的坐标P^L_comp。

变换步骤如下：

1. L(t_i) → I(t_i)：
   P^I(t_i) = R_L^I P^L + t_L^I
   （利用LiDAR到IMU的外参）

2. I(t_i