VINS-Mono+Fusion源码解析系列（十五）：视觉惯性对齐

最新推荐文章于 2026-03-31 08:57:45 发布

原创

最新推荐文章于 2026-03-31 08:57:45 发布 · 743 阅读

标签

#自动驾驶 #计算机视觉 #人工智能

本文详细解析了VINS-Mono+Fusion中的VisualIMUAlignment过程，包括陀螺仪零偏solveGyroscopeBias的理论推导、流程及代码实现。此外，介绍了LinearAlignment的线性方程构造，通过预积分量求解速度、加速度修正和平移尺度。整个过程涉及到自动驾驶中计算机视觉和惯性传感器融合的关键技术。

1. VisualIMUAlignment

（1）大致思路

通过solveGyroscopeBias求出陀螺仪零偏
通过LinearAlignment求出每帧的速度，加速度修正以及平移尺度s

（2）代码实现

/**
 * @brief 
 * 
 * @param[in] all_image_frame 每帧的位姿和对应的预积分量
 * @param[out] Bgs 陀螺仪零偏
 * @param[out] g 重力向量
 * @param[out] x 其他状态量
 * @return true 
 * @return false 
 */

bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, 
                        Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
   
   
    solveGyroscopeBias(all_image_frame, Bgs);  // 求零偏

    if(LinearAlignment(all_image_frame, g, x))  // 求每帧的速度，加速度修正以及尺度s
        return true;
    else 
        return false;
}

1.1 陀螺仪零偏solveGyroscopeBias

（1）理论推导 —— 根据旋转预积分量构造方程来求解陀螺仪零偏

假设 $c_0$ 帧为前面所说的枢纽帧，记它到第 $k$ 个IMU数据之间的旋转为 $q_{c_0}^{b_k}$ ，平移为 $pˉc0bk\bar{p}_{c_0}^{b_k}$ 。另外，记相机与IMU之间的旋转外参为 $q_c^b$ ，平移外参为 $p_c^b$ 。

$c_0$ 帧到 $c_k$ 帧的变换关系如下：
$T_{c_0}^{c_k} = T_{c_0}^{b_k} \cdot T_b^c \\ \left[\begin{array}{cc} R_{c_0}^{c_k} & sp_{c_0}^{c_k} \\ 0 & 1 \end{array} \right] = \left[\begin{array}{cc} R_{c_0}^{b_k} & sp_{c_0}^{b_k} \\ 0 & 1 \end{array} \right] \left[\begin{array}{cc} R_b^c & p_b^c \\ 0 & 1 \end{array} \right] \\ \Rightarrow \\ R_{c_0}^{c_k} = R_{c_0}^{b_k}R_b^c\ \ \ \ \ \ s\bar{p}_{c_0}^{b_k} = s\bar{p}_{c_0}^{c_k} - R_{c_0}^{b_k}p_b^c$
于是，枢纽帧到第 $k$ 个IMU数据之间的旋转和平移可通过下式表达：
$q_{c_0}^{b_k} = q_{c_0}^{c_k} \otimes (q_b^c)^{-1} \\ s\bar{p}_{c_0}^{b_k} = s\bar{p}_{c_0}^{c_k} - R_{c_0}^{b_k}p_b^c$
注：这里的命名是从右到左，从下到上，与SLAM14讲中的命名方式一样。

陀螺仪零偏的求解方程
$\min_{\delta b_w}\sum_{k \in \mathcal{B}}||(q_{c_0}^{b_{k+1}})^{-1} \otimes q_{c_0}^{b_k} \otimes \gamma_{b_k}^{b_{k+1}}||^2$
对上述方程的解释：前面已经求出枢纽帧 $c_0$ 到第 $k$ 帧IMU之间的旋转 $q_{c_0}^{b_k}$ ，因此也可求出枢纽帧 $c_0$ 到第 $k + 1$ 帧IMU之间的旋转 $q_{c_0}^{b_{k+1}}$ ，第 $k$ 帧IMU与第 $k + 1$ 帧IMU预积分量 $γbkbk+1\gamma_{b_k}^{b_{k+1}}$ 也可求出。

所以，理论上 $(qc0bk+1)−1⊗qc0bk⊗γbkbk+1=qbk+1bk⊗qbkbk+1=I(q_{c_0}^{b_{k+1}})^{-1} \otimes q_{c_0}^{b_k} \otimes \gamma_{b_k}^{b_{k+1}} = q_{b_{k+1}}^{b_k} \otimes q_{b_k}^{b_{k+1}} = I$ 。但由于陀螺仪零偏 $δbw\delta b_w$ 的存在，使得预积分量 $γbkbk+1\gamma_{b_k}^{b_{k+1}}$ 存在误差，为了避免重新进行预积分，需要在原有预积分量 $γ^bkbk+1\hat{\gamma}_{b_k}^{b_{k+1}}$ 的基础上进行如下补偿：
$γbkbk+1≈γ^bkbk+1⊗[112Jbwγδbw] \gamma_{b_k}^{b_{k+1}} \approx \hat{\gamma}_{b_k}^{b_{k+1}} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}J_{b_w}^{\gamma}\delta b_w \end{array} \right]$
于是，在考虑陀螺仪零偏的情况下，上述求解方程应该得到一个单位四元数，即：
$(qc0bk+1)−1⊗qc0bk⊗γ^bkbk+1⊗[112Jbwγδbw]=[1000]⇒[112Jbwγδbw]=[(qc0bk+1)−1⊗qc0bk⊗γ^bkbk+1]−1=(γ^bkbk+1)−1⊗(qc0bk)−1⊗qc0bk+1⇒12Jbwγδbw=[(γ^bkbk+1)−1⊗(qc0bk)−1⊗qc0bk+1]vec⇒Jbwγδbw=2[(γ^bkbk+1)−1⊗(qc0bk)−1⊗qc0bk+1]vec (q_{c_0}^{b_{k+1}})^{-1} \otimes q_{c_0}^{b_k} \otimes \hat{\gamma}_{b_k}^{b_{k+1}} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}J_{b_w}^{\gamma}\delta b_w \end{array} \right] = \left[\begin{array}{cc} 1 \\ 0 \\ 0 \\ 0 \end{array} \right] \\ \Rightarrow \\ \left[\begin{array}{cc} 1 \\ \frac{1}{2}J_{b_w}^{\gamma}\delta b_w \end{array} \right] = [(q_{c_0}^{b_{k+1}})^{-1} \otimes q_{c_0}^{b_k} \otimes \hat{\gamma}_{b_k}^{b_{k+1}} ]^{-1} = (\hat{\gamma}_{b_k}^{b_{k+1}})^{-1} \otimes (q_{c_0}^{b_k})^{-1} \otimes q_{c_0}^{b_{k+1}} \\ \Rightarrow \\ \frac{1}{2}J_{b_w}^{\gamma}\delta b_w = [(\hat{\gamma}_{b_k}^{b_{k+1}})^{-1} \otimes (q_{c_0}^{b_k})^{-1} \otimes q_{c_0}^{b_{k+1}}]_{vec} \\ \Rightarrow \\ J_{b_w}^{\gamma}\delta b_w = 2[(\hat{\gamma}_{b_k}^{b_{k+1}})^{-1} \otimes (q_{c_0}^{b_k})^{-1} \otimes q_{c_0}^{b_{k+1}}]_{vec}$

最低0.47元/天解锁文章