当前位置：首页 > news >正文

SLAM文献之DM-VIO: Delayed Marginalization Visual-Inertial Odometry

news 来源：原创 2025/4/25 14:22:21

1. 算法概述

DM-VIO (Delayed Marginalization Visual-Inertial Odometry) 是一种基于延迟边缘化的视觉-惯性里程计算法，它结合了视觉和惯性测量单元(IMU)的数据进行位姿估计。该算法是VINS-Mono的改进版本，主要创新点在于采用了一种延迟边缘化策略，能够更好地处理非线性问题和保持系统的可观测性。

2. 系统模型

2.1 状态表示

系统的状态向量定义为：
$x_0, x_1, \dots, x_n, \lambda_0, \lambda_1, \dots, \lambda_m ]$

其中：

x_i = [p_i, q_i, v_i, b_a_i, b_g_i] 表示第i个关键帧的状态
- p_i ∈ ℝ³：位置
- q_i ∈ SO(3)：姿态（四元数表示）
- v_i ∈ ℝ³：速度
- b_a_i ∈ ℝ³：加速度计偏置
- b_g_i ∈ ℝ³：陀螺仪偏置
λ_j ∈ ℝ：第j个3D点的逆深度

2.2 IMU预积分

IMU测量模型：
$\hat{a}_t = a_t + b_a + R_w^t g^w + n_a ]$

$\hat{\omega}_t = \omega_t + b_g + n_g ]$

IMU预积分量（两个关键帧i和j之间）：
$\Delta R_{ij} = \prod_{k=i}^{j-1} \exp\left((\hat{\omega}_k - b_{g_k}) \Delta t \right) ]$
$\Delta v_{ij} = \sum_{k=i}^{j-1} \Delta R_{ik} (\hat{a}_k - b_{a_k}) \Delta t ]$
$\Delta p_{ij} = \sum_{k=i}^{j-1} \left[ \Delta v_{ik} \Delta t + 0.5 \Delta R_{ik} (\hat{a}_k - b_{a_k}) \Delta t^2 \right] ]$

其中：

$\Delta R_{ij} )$ 表示从时刻 $(i)$ 到时刻 $(j)$ 的旋转增量，通过累积旋转误差来计算。
$\Delta v_{ij} )$ 和 $Delta p_{ij} )$ 分别是从时刻 $(i)$ 到时刻 $(j)$ 的速度增量和位置增量。
$\hat{\omega}_k )$ 是时刻 $(k)$ 的角速度估计， $b_{g_k} )$ 是角速度偏差。
$\hat{a}_k )$ 是时刻 $(k)$ 的加速度估计， $b_{a_k} )$ 是加速度偏差。
$\Delta t )$ 是两个时刻之间的时间间隔。

预积分残差：
$r_{\Delta R_{ij}} = \text{Log}(\Delta R_{ij}^T R_i^T R_j) ]$
$r_{\Delta v_{ij}} = R_i^T(v_j - v_i - g \Delta t_{ij}) - \Delta v_{ij} ]$
$r_{\Delta p_{ij}} = R_i^T(p_j - p_i - v_i \Delta t_{ij} - 0.5 g \Delta t_{ij}^2) - \Delta p_{ij} ]$

其中：

$r_{\Delta R_{ij}} )$ , $r_{\Delta v_{ij}} )$ , 和 $r_{\Delta p_{ij}} )$ 分别表示旋转矩阵、速度和位置的残差。
$(\text{Log})$ 是李群的对数映射（通常用于旋转矩阵或四元数的误差表示）。
$(\Delta R_{ij})$ , $(\Delta v_{ij})$ , 和 $(\Delta p_{ij})$ 是旋转、速度和位置的增量或误差项。
$R_i )$ 和 $R_j )$ 是对应时刻 $(i)$ 和 $(j)$ 的旋转矩阵。
$v_i )$ 和 $v_j )$ 是时刻 $(i)$ 和 $(j)$ 的速度向量。
$p_i )$ 和 $p_j )$ 是时刻 $(i)$ 和 $(j)$ 的位置向量。
$\Delta t_{ij} )$ 是时刻 $(i)$ 和 $(j)$ 之间的时间间隔。
$(g)$ 是重力加速度。

2.3 视觉重投影误差

对于特征点l在帧i中的观测 $u_i^l, v_i^l)$ ，其重投影误差为：
$r_c^l = \begin{bmatrix} u_i^l - \pi(R_i(X_l - p_i)) \\ v_i^l - \pi(R_i(X_l - p_i)) \end{bmatrix}^T ]$

其中 $X_l$ 是3D点坐标，π是投影函数。

使用逆深度参数化时，3D点表示为：
$X_l = p_{C1} + \frac{1}{\lambda_l} R_{C1} \begin{bmatrix} u_l \\ v_l \\ 1 \end{bmatrix} ]$

3. 优化问题

3.1 目标函数

联合优化问题：
$\min_X \left\{ \sum \left\| r_I(x_i, x_j) \right\|_{\Sigma_I}^2 + \sum \rho \left( \left\| r_C(x_i, \lambda_l) \right\|_{\Sigma_C}^2 \right) \right\} ]$
其中ρ是Huber鲁棒核函数。

3.2 滑动窗口优化

DM-VIO采用滑动窗口优化策略，窗口内保持N个关键帧。当新帧加入时，最旧的关键帧会被边缘化。

4. 延迟边缘化策略

4.1 标准边缘化的问题

传统VIO在边缘化旧帧时：

线性化点固定，导致线性化误差
可能破坏系统的可观测性
对IMU偏置的估计有负面影响

4.2 DM-VIO的解决方案

DM-VIO采用两阶段边缘化：

初始边缘化：当关键帧被移出窗口时，只边缘化与视觉相关的状态（位姿和路标点），保留IMU相关状态（速度、偏置）在优化窗口中。
延迟边缘化：当这些IMU状态也变得过时时，再进行第二次边缘化。

4.3 数学推导

设要边缘化的状态为 $x_m$ ，保留状态为 $x_r$ 。将Hessian矩阵分块

$\begin{bmatrix} H_{mm} & H_{mr} \\ H_{rm} & H_{rr} \end{bmatrix} ]$

边缘化后的先验信息：
$H^* = H_{rr} - H_{rm} H_{mm}^{-1} H_{mr} ]$
$b^* = b_r - H_{rm} H_{mm}^{-1} b_m ]$

在DM-VIO中，第一次边缘化：
$x_m = [p_i, q_i, \lambda_j], \quad x_r = [v_i, b_{a_i}, b_{g_i}, \dots] ]$

第二次边缘化：
$x_m = [v_i, b_{a_i}, b_{g_i}], \quad x_r = [\text{其他状态}] ]$

其中：

$(H)$ 是Hessian矩阵，分为四个子块 $H_{mm}, H_{mr}, H_{rm}, H_{rr} )$ 。
$H^* )$ 和 $b^* )$ 是边缘化后的先验信息。
$x_m )$ 是边缘化的状态， $x_r )$ 是保留的状态。
在DM-VIO中，第一次边缘化涉及将位置和方向 $p_i, q_i, \lambda_j )$ 从状态中移除，保留速度、偏差等其他信息。第二次边缘化则是将速度和偏差 (v_i, b_a_i, b_g_i )从状态中移除。