激光SLAM学习笔记2——slam中PVQ增量的残差理解

在SLAM（特别是基于因子图的SLAM系统如LIO-SAM）中，PVQ增量的残差是指状态变量（位置P、速度V、姿态Q）的预测值与观测值之间的差异。这是优化问题的核心概念，用于衡量当前估计与传感器测量之间的一致性。

PVQ增量残差的详细解释：

1. PVQ的含义：

   • P (Position)：位置，3D空间中的坐标(x,y,z)
   • V (Velocity)：速度，3D速度向量(vx,vy,vz)
   • Q (Quaternion)：姿态，用四元数表示的旋转

2. 增量(Δ)：
   指两个连续时刻之间的状态变化量。例如：

   • ΔP = P_t - P_{t-1}
   • ΔV = V_t - V_{t-1}
   • ΔQ = Q_t ⊗ Q_{t-1}^{-1} (四元数乘法)

3. 残差(residual)：
   残差 = 预测值 - 观测值
   在SLAM中通常表示为：

   r = h(x) - z
   

   其中：

   • h(x)：从状态变量x预测的测量值
   • z：实际的传感器测量值

在LIO-SAM中的具体应用：

1. IMU预积分残差：

   // 在imuPreintegration.cpp中的IMU因子
   gtsam::ImuFactor(X(key-1), V(key-1), X(key), V(key), B(key-1), preint_imu)
   

   这个因子计算的是：

   • 基于IMU测量预测的PVQ增量
   • 与通过状态变量计算的PVQ增量之间的差异

2. 残差计算过程：

   // 预测状态（使用IMU预积分）
   gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
   
   // 优化后得到的状态
   gtsam::Pose3 optimizedPose = result.at<gtsam::Pose3>(X(key));
   gtsam::Vector3 optimizedVel = result.at<gtsam::Vector3>(V(key));
   
   // 残差 = 预测状态 - 优化状态
   gtsam::Vector6 poseResidual = gtsam::Pose3::Logmap(propState_.pose().between(optimizedPose));
   gtsam::Vector3 velResidual = optimizedVel - propState_.v();
   

3. 物理意义：

   • 位置残差：反映IMU积分的位置漂移
   • 速度残差：反映IMU加速度计测量的累积误差
   • 姿态残差：反映陀螺仪积分的旋转漂移

4. 在优化中的作用：
   残差的平方（马氏距离）构成目标函数：

   cost(x) = r^T Σ^{-1} r
   

   其中Σ是协方差矩阵，优化器通过最小化这个代价函数来调整状态估计。

为什么需要残差？

1. 传感器融合：通过残差将不同传感器（如IMU和激光雷达）的测量统一到同一优化框架
2. 误差修正：IMU积分会随时间漂移，残差帮助修正这些误差
3. 不确定性建模：通过协方差矩阵Σ可以合理加权不同传感器的可靠性

在LIO-SAM中，PVQ增量残差是紧耦合激光-IMU融合的关键桥梁，它使得高频的IMU测量可以与低频但精确的激光雷达测量共同优化，实现高精度的状态估计。