为什么角速度可以只积分一帧,而加速度不能只靠"本帧加速度"求平移去畸变
你说得对:加速度也完全可以只积分当前一帧 LiDAR 扫描的 0.1 秒左右,这样误差不会像连续积分几十秒那样无限变大。但问题不在于"积分时间短不短",而在于:**陀螺仪积分能直接得到本帧内的相对转角;加速度积分却不能只靠本帧加速度直接得到本帧内相对位移。**因为位移不仅由加速度决定,还由扫描开始时已经存在的速度决定。
对于旋转去畸变,假设当前一帧扫描从 tst_sts 开始,某个点在 tit_iti 时刻被采到。系统真正需要的是该点相对于扫描开始时刻转了多少,而不是机器人从开机以来的世界绝对朝向:
ΔR(t_i) = R(t_s)ᵀ · R(t_i)
其中 R(ts)R(t_s)R(ts) 是扫描开始时刻的姿态,R(ti)R(t_i)R(ti) 是点采样时刻的姿态,ΔR(ti)ΔR(t_i)ΔR(ti) 就是本帧内相对旋转。因为扫描开始时可直接定义为"相对旋转为零",即:
ΔR(t_s) = I
所以,只要把扫描开始到当前点时刻之间的陀螺仪角速度积分起来,就能得到该点相对于扫描起点的旋转量。LIO-SAM 的 imuDeskewInfo() 正是这样做的:每处理一帧点云,先将本帧累计旋转置零,再按 IMU 时间间隔累积角速度。源码中的核心逻辑如下:
imuRotX[0] = 0;
imuRotY[0] = 0;
imuRotZ[0] = 0;
imuRotX[k] = imuRotX[k-1] + angular_x * dt;
imuRotY[k] = imuRotY[k-1] + angular_y * dt;
imuRotZ[k] = imuRotZ[k-1] + angular_z * dt;
这里的 imuRotX/Y/Z 并不是从程序启动到现在累计的全球角度,而是当前扫描周期内的相对累计转角。比如 10 Hz LiDAR 一帧约 0.1 秒,某个点在扫描开始后 0.06 秒采到,代码就通过插值得到该点在这 0.06 秒内相对扫描起点发生的旋转,再把点旋回扫描开始时刻。即使陀螺仪有 bias,0.1 秒内的角度误差通常比长时间积分小得多;而且 LiDAR 后端优化会持续修正陀螺仪 bias,避免它长期变大。
但是平移完全不同。某点在扫描开始后 Δt\Delta tΔt 时刻的相对位移,近似写成:
Δp(t)
=
v_start · Δt
+
1/2 · a_world · Δt²
其中:
v_start:扫描开始时刻的速度
a_world:去零偏、补偿重力并旋转到世界坐标系后的加速度
Δt:点相对扫描开始时刻的采样时间
真正关键的是第一项:
v_start · Δt
它表示机器人即使没有加速、只是匀速前进,也会在扫描期间产生位移。假设小车以 1 m/s1\,m/s1m/s 匀速前进,扫描周期是 0.1 s0.1\,s0.1s,则这一帧内真实位移为:
Δp = 1 × 0.1 = 0.1 m
但由于它是匀速运动:
a_world ≈ 0
若你只从本帧加速度开始积分,并默认扫描开始速度为零,那么会得到:
Δp ≈ 0
这显然错了,因为小车实际已经前进了 10 cm。也就是说,**加速度只能直接告诉你"速度变化了多少",不能单独告诉你"已经以多快的速度移动了多少距离"。**想通过短时间加速度积分得到这一帧的平移,必须同时已知扫描开始时的 vstartv_{start}vstart。
更严格地说,扫描开始到点采样时刻的位移不是简单双积分,而是:
p(t_i)
=
p(t_s)
+
v(t_s) · Δt
+
∫₀^Δt (Δt - τ)
[
R(τ) · (a_m(τ) - b_a) + g
] dτ
其中:
p(t_s):扫描开始时的位置
v(t_s):扫描开始时的速度
a_m:IMU 测到的原始加速度
b_a:加速度计零偏
R(τ):该时刻 IMU 姿态,用于将机体系加速度转到世界系
g:重力项
这个公式说明,平移去畸变至少依赖五类东西:扫描开始位置、扫描开始速度、扫描期间姿态、加速度 bias、重力方向。任何一个量不准,最后都会影响平移。尤其是姿态只要略有误差,重力 ggg 就可能被错误投影到水平面,形成虚假的水平加速度;这个假加速度再经过积分,就会被放大成错误位移。
这就是为什么 LIO-SAM 需要 imuPreintegration.cpp 持续维护完整惯导状态,而不是在 imageProjection.cpp 中临时从零积分加速度。imuPreintegration 维护的不是单独位置,而是一整组状态:
状态 =
姿态 R
+
位置 p
+
速度 v
+
陀螺仪 bias
+
加速度计 bias
每次 mapOptimization 通过 LiDAR scan-to-map 匹配得到更可靠的 LiDAR 位姿后,LIO-SAM 会把这个校正状态和新的 bias 作为 IMU 传播起点;随后用后续 IMU 数据预测当前时刻的 R,p,vR,p,vR,p,v,并发布到 odomTopic + "_incremental"。因此,odomQueue 中的位姿其实是:
上一轮 LiDAR 校正状态
+
后续 IMU 积分传播
=
当前时刻高频预测位姿
而不是另一套独立传感器提供的位置。imuPreintegration.cpp 中确实维护 prevStateOdom、prevBiasOdom,并通过 IMU 预积分和 predict() 持续更新预测状态,再发布增量里程计。
标准 LIO-SAM 的 imageProjection.cpp 已经会从 odomQueue 取当前扫描开始时刻的预测位置、姿态,写入:
cloudInfo.initialGuessX;
cloudInfo.initialGuessY;
cloudInfo.initialGuessZ;
cloudInfo.initialGuessRoll;
cloudInfo.initialGuessPitch;
cloudInfo.initialGuessYaw;
它们主要作为后续 mapOptimization 的 scan-to-map 匹配初值。代码还会计算扫描起点和终点之间的相对平移 odomIncreX/Y/Z。
不过,标准 LIO-SAM 默认没有真正启用逐点平移去畸变。源码中 findPosition() 先把位置置零:
*posXCur = 0;
*posYCur = 0;
*posZCur = 0;
随后按时间比例插值 odomIncreX/Y/Z 的代码被注释掉了。也就是说,原版默认主要使用 IMU 进行旋转去畸变 ;odomQueue 的完整预测位姿主要用于给后端 scan-to-map 匹配提供一个较好的初始位置和姿态。
因此可以最终记成下面这段话:
陀螺仪只积分当前一帧,也能得到本帧内相对转角,
因为扫描开始时可以直接把相对旋转设为零;
而加速度即使只积分当前一帧,也不能只靠加速度得到相对位移,
因为匀速运动时加速度为零,但机器人仍在持续移动。
平移去畸变必须知道扫描开始时的速度、姿态、位置和 bias,
所以需要 imuPreintegration 维护完整惯导状态。
LIO-SAM 用 LiDAR 优化不断校正这个状态,
再以 IMU 高频传播得到 odomQueue 中的当前预测位姿。
标准源码默认主要做旋转去畸变,
平移去畸变代码虽然预留了,但默认没有开启。
