VINS中陀螺仪零偏的估计

VINS中关于陀螺仪零偏的初始化估计

对于窗口中得连续两帧 $b_k$ 和 $b_{k+1}$ ，已经从视觉SFM中得到了旋转 $q_{b_k}^{c_0}$ 和 $q_{b_{k+1}}^{c_0}$ ，从IMU预积分中得到了相邻帧旋转 ${\hat{\gamma }}_{b_{k+1}}^{b_k}$ 。 根据约束方程，联立所有相邻帧，最小化代价函数（论文式(7)）：
$$\underset{\delta b_w}{\min }\sum _{k\in \mathcal{B}}{\Vert q_{b_{k+1}}^{c_0}{}^{-1}\otimes q_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}\Vert }^2$$
其中对陀螺仪偏置求IMU预积分项线性化，有：
$${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }\delta b_w\end{array}\right]$$
在具体实现的时候，因为上述约束方程为：

$$q_{b_{k+1}}^{c_0}{}^{-1}\otimes q_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}=\left[\begin{array}{l}1\\ 0\end{array}\right]$$
有：

$${\gamma }_{b_{k+1}}^{b_k}=q_{b_k}^{c_0-1}\otimes q_{b_{k+1}}^{c_0}\otimes \left[\begin{array}{l}1\\ 0\end{array}\right]$$
代入 $\delta b_w$ 得 :

$${\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }\delta b_w\end{array}\right]=q_{b_k}^{c0-1}\otimes q_{b_{k+1}}^{c0}\otimes \left[\begin{array}{l}1\\ 0\end{array}\right]$$

只考虑虚部，有 :

$$J_{b_w}^{\gamma }\delta b_w=2{\left({\hat{\gamma }}_{b_{k+1}}^{b_k}{}^{-1}\otimes q_{b_k}^{c_0-1}\otimes q_{b_{k+1}}^{c_0}\right)}_{vec}$$

两侧乘以 ${{\mathbf{J}}_{{\mathbf{b}}_{\mathbf{w}}}^{\gamma }}^T$ ，用LDLT分解求得 $\delta b_w$ 。
在代码中， ${\mathrm{q}}_{\mathrm{i},\mathrm{j}}$ 即 $q_{b_{k+1}}^{b_k}=q_{b_k}^{c_0-1}\otimes q_{b_{k+1}}^{c_0}$
${\mathrm{tmp}}_{-}\mathrm{A}$ 即 $J_{b_w}^{\gamma }$

${\mathrm{tmp}}_{-}\mathrm{B}$ 即
$$2{\left({\hat{r}}_{b_{k+1}}^{b_k}{}^{-1}\otimes q_{b_{k+1}}^{b_k}\right)}_{vec}=2{\left({\hat{r}}_{b_{k+1}}^{b_k}{}^{-1}\otimes q_{b_k}^{c_0-1}\otimes q_{b_{k+1}}^{c_0}\right)}_{vec}$$
根据上面的代价函数构造 $\mathrm{Ax}=\mathrm{B}$ 即
$$J_{b_w}^{\gamma T}{J}_{b_w}^{\gamma }\delta b_w=2J_{b_w}^{\gamma T}{\left({\hat{r}}_{b_{k+1}}^{b_k}{}^{-1}\otimes q_{b_k}^{c_0-1}\otimes q_{b_{k+1}}^{c_0}\right)}_{\text{vec }}$$
然后采用LDLT分解求得 $\delta b_w$ 。

• VINS中的代码

/**
 * @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
        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());
 
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;
    // 得到了新的Bias后对应的预积分需要repropagate
    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]);
    }
}

之所以 A +=tmp_A.transpose() * tmp_A，其实就是 $A^{T}Ax=A^{T}b$。在求解 $Ax=b$ 的最小二乘解时，两边同乘以A矩阵的转置得到的 $A^{T}A$ 一定是可逆的。

• TIPS

  这里为什么是可以连加的呢，直接构造该正定方程呢，简单来说就是VINS中认为滑窗中陀螺仪的Bags都是一样的。所以把所有的方程写在一起就构成了同一个变量的连加形式。

Reference

• https://zhuanlan.zhihu.com/p/465689538

• https://zhuanlan.zhihu.com/p/158621734

• https://blog.csdn.net/m0_37874102/article/details/114834500