在vins初始化中的陀螺仪偏置标定原理与代码部分

最新推荐文章于 2023-07-02 18:36:44 发布

秃头队长

最新推荐文章于 2023-07-02 18:36:44 发布

阅读量722

点赞数

CC 4.0 BY-SA版权

分类专栏： SLAM 文章标签： vins 标定陀螺仪

原文链接：https://www.dazhuanlan.com/ming1016/topics/1512451

SLAM 专栏收录该内容

55 篇文章

订阅专栏

该博客探讨了如何利用视觉SLAM（Simultaneous Localization And Mapping）和IMU（惯性测量单元）预积分的数据，通过优化代价函数来校正陀螺仪的偏置。文章详细介绍了数学公式和算法实现，包括从连续两帧图像的旋转矩阵和预积分旋转量出发，建立约束方程，最终通过LDLT分解求解偏置变化量，并在滑窗内累加得到准确的偏置值。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

对于窗口中连续两帧图像 $b_{k}$ 和 $b_{k+1}$ ，之前已经通过视觉 sfm 得到了两帧相对于滑窗第一帧的旋转 $qc0bk和qc0bk+1\mathbf{q}_{c_{0} b_{k}}和\mathbf{q}_{c_{0} b_{k+1}}$ ，此时之前通过 IMU 预积分得到这两帧旋转的预积分 $γ^bkbk+1\hat{\gamma}_{b_k b_{k+1}}$ （先前不带 bias 的预积分）。

此时，我们可以建立约束方程，最小化代价函数，如下：

$argminδbg∑kinB∣2⌊qc0bk+1−1⨂qc0bk⨂γbkbk+1⌋xyz∣2argmin_{\delta \mathbf{b}^{g}} \sum_{k in B} \left|2 \left \lfloor \mathbf{q}_{c_{0} b_{k+1}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k}} \bigotimes {\gamma}_{b_k b_{k+1}}\right \rfloor_{x y z} \right|^{2}$
其中 $γbkbk+1{\gamma}_{b_k b_{k+1}}$ 表示的是算上陀螺仪 bias 的预积分值。
$γbkbk+1≈γ^bkbk+1⨂[112Jbgγδbg]\gamma_{b_k b_{k+1}} \approx \hat{\gamma}_{b_k b_{k+1}} \bigotimes \left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{g}}^{\gamma} \delta b_{g}}\end{array}\right]$
具体方法
在上述问题的求解时，最理想的情况如下（其中等式右边的四元数代表的是旋转不变的四元数）：

$qc0bk+1−1⨂qc0bk⨂γbkbk+1=[1000]\mathbf{q}_{c_{0} b_{k+1}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k}} \bigotimes {\gamma}_{b_k b_{k+1}} = \left[\begin{array}{l}{1} \\ {0}\\ {0}\\ {0}\end{array}\right]$
对上式进行变换可以得到

$γbkbk+1=qc0bk−1⨂qc0bk+1⨂[1000]{\gamma}_{b_k b_{k+1}} = \mathbf{q}_{c_{0} b_{k}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k+1}} \bigotimes \left[\begin{array}{l}{1} \\ {0}\\ {0}\\ {0}\end{array}\right]$
将 $δbg\delta b_{g}$ 带入上式可以得到

$γ^bkbk+1⨂[112Jbgγδbg]=qc0bk−1⨂qc0bk+1⨂[10]\hat{\gamma}_{b_k b_{k+1}} \bigotimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{g}}^{\gamma} \delta b_{g}}\end{array}\right] = \mathbf{q}_{c_{0} b_{k}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k+1}} \bigotimes \left[\begin{array}{l}{1} \\ {0}\end{array}\right]$
只考虑上式虚部，我们可以得到 (其中 $v e c$ 代表取虚部)

$Jbgγδbg=2(γ^bkbk+1−1⨂qc0bk−1⨂qc0bk+1)vecJ_{b_{g}}^{\gamma} \delta b_{g}=2\left({\hat{\gamma}_{b_k b_{k+1}}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k+1}}\right)_{v e c}$
对于上式，我们想要做的是求解出 $δbg\delta b_{g}$ 的大小，此时我们可以使用如下办法：

等式两侧同时乘以 $JbgγTJ_{b_{g}}^{\gamma T}$ (在使用 cholesky 分解求解矩阵，获取使目标函数达到最小的解时，需要将系数矩阵变为正定)，即得到
$JbgγTJbgγδbg=2JbgγT(γ^bkbk+1−1⨂qc0bk−1⨂qc0bk+1)vecJ_{b_{g}}^{\gamma T} J_{b_{g}}^{\gamma} \delta b_{g}=2 J_{b_{g}}^{\gamma T} \left({\hat{\gamma}_{b_k b_{k+1}}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k}}^{-1} \bigotimes \mathbf{q}_{c_{0} b_{k+1}}\right)_{v e c}$
使用 LDLT 分解即可求得 $δbg\delta b_{g}$ 注意这个地方得到的只是 Bias 的变化量，需要在滑窗内累加得到Bias 的准确值。

/**
 * @brief   陀螺仪偏置校正
 * @optional    根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
 *              主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
 *              注意得到了新的Bias后对应的预积分需要repropagate
 * @param[in]   all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
 * @param[out]  Bgs 陀螺仪偏置
 * @return      void
*/
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    //遍历所有图像帧
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        //下一帧
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();

        //两帧之间的旋转变换 
        //R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 转换为四元数 q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        //tmp_A = J_j_bw 雅可比 ，之前的预积分是没有算上陀螺仪bias的
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        //tmp_b = 2 * (r^bk_bk+1)^-1 * (q^c0_bk)^-1 * (q^c0_bk+1)
        //      = 2 * (r^bk_bk+1)^-1 * q_ij
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        //tmp_A * delta_bg = tmp_b
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    //LDLT方法
    delta_bg = A.ldlt().solve(b);
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    //假设窗口中bgs值都相同
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    //标定成功bias后需要重新进行预积分
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}