VINS-Mono融合轮式编码器和GPS（一）：预积分

开篇

最近在学习多传感器融合技术，本着重复造轮子的精神，做了一个VINS-Mono融合轮式编码器和GPS数据的小项目，轮式编码器数据在融合时做了紧耦合，主要参考了论文[1]；GPS数据做了松耦合，主要是借用了VINS-Fusion的方式，当然也可以用紧耦合的方式（挖个坑，后续再加），项目地址VINS-GPS-Wheel，附视频。项目仍在进行中，欢迎交流学习。
在此记录一下主要的几个方面（水几篇博客）：
VINS-Mono融合轮式编码器和GPS（一）：预积分
VINS-Mono融合轮式编码器和GPS（二）：鲁棒初始化
VINS-Mono融合轮式编码器和GPS（三）：后端优化
VINS-Mono融合轮式编码器和GPS（四）：融合GPS

理论

VIO系统本身具有4自由度的不可观性，但是有文献[2]表明，对于汽车的行驶方式，例如恒速运动、圆周运动，会造成尺度无法精确估计，融合轮式编码器可以提高VIO系统的精确性。在自动驾驶的室外场景下，回环比较困难，GNSS数据可以提供一个全局的定位，需要加进去。

预积分公式推导

不同资料记法可能不太一样，推公式之前先说一下这里使用的记法，${\left(\cdot \right)}^w$ 、${\left(\cdot \right)}^{b_k}$、${\left(\cdot \right)}^{c_k}$、${\left(\cdot \right)}^{o_k}$分别表示物理量的世界坐标系、IMU坐标系、相机坐标系和轮式编码器坐标系在图像第k帧的表示。$R_o^b$和$q_o^b$表示向量从$o$坐标系到$b$坐标系下的旋转变换，$p_o^b$表示$b$坐标系下$o$坐标系原点的坐标。

单独写轮式编码器的预积分太单薄，有的地方会和IMU预积分公式放到一起，不过IMU相关的公式说明就不给出了。

1. 预积分形式

$${\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}R({\hat{\gamma }}_i^{b_k})(\hat{a}-b_{a_i}){\delta t}^2$$
$${\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+R({\hat{\gamma }}_i^{b_k})(\hat{a}-b_{a_i})\delta t$$
$${\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}(\hat{\omega }-b_{{\omega }_i})\delta t\end{array}\right]$$
$${\hat{\eta }}_{i+1}^{b_k}={\hat{\eta }}_i^{b_k}+R({\hat{\gamma }}_i^{b_k})({\hat{R}}_o^b{\hat{v}}_i)\delta t$$
$\eta$表示轮式编码器预积分量，$R_o^b$为轮式编码器的外参旋转矩阵，${\hat{v}}_i$为轮式编码器的线速度数据，当然这里采用的不是编码器的原始数据，需要自己算出线速度。注意，这里的$i$表示IMU时刻，虽然作者没有说明，本项目在代码实现过程中还是做了一下时间戳对齐。

2. 中值积分

$${\hat{\eta }}_{i+1}^{b_k}={\hat{\eta }}_i^{b_k}+\frac{1}{2}\left[R({\hat{\gamma }}_i^{b_k})({\hat{R}}_o^b{\hat{v}}_i)+R({\hat{\gamma }}_{i+1}^{b_k})({\hat{R}}_o^b{\hat{v}}_{i+1})\right]\delta t$$

3. 连续形式下的预积分误差、协方差和Jacobian

导数形式：$\delta {\dot{z}}_t^{b_k}=F_t\delta z_t^{b_k}+G_t{n}_t$
根据导数定义，可写出$\delta {\dot{z}}_{t+\delta t}^{b_k}=(I+F_t\delta t)\delta z_t^{b_k}+(G_t\delta t)n_t$，进而写出展开形式（为了和代码一致，按照$pqv$而不是$pvq$的形式）
$$\left[\begin{array}{c}\delta {\hat{\alpha }}_{t+\delta t}^{b_k}\\ \delta {\hat{\theta }}_{t+\delta t}^{b_k}\\ \delta {\hat{\beta }}_{t+\delta t}^{b_k}\\ \delta {\hat{\eta }}_{t+\delta t}^{b_k}\\ \delta {\hat{b}}_{a_{t+\delta t}}\\ \delta {\hat{b}}_{{\omega }_{t+\delta t}}\end{array}\right]=\left[\begin{array}{cccccc}I& 0& I\delta t& 0& 0& 0\\ 0& I-{({\hat{\omega }}_t-b_{{\omega }_t})}^{\wedge }\delta t& 0& 0& 0& -I\delta t\\ 0& -R_t^{b_k}{({\hat{a}}_t-b_{a_t})}^{\wedge }\delta t& I& 0& -R_t^{b_k}\delta t& 0\\ 0& -R_t^{b_k}{({\hat{R}}_b^o{\hat{v}}_t)}^{\wedge }\delta & 0& I& 0& 0\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I\end{array}\right]\left[\begin{array}{c}\delta {\hat{\alpha }}_t^{b_k}\\ \delta {\hat{\theta }}_t^{b_k}\\ \delta {\hat{\beta }}_t^{b_k}\\ \delta {\hat{\eta }}_t^{b_k}\\ \delta {\hat{b}}_{a_t}\\ \delta {\hat{b}}_{{\omega }_t}\end{array}\right]+\left[\begin{array}{ccccc}0& 0& 0& 0& 0\\ 0& -I\delta t& 0& 0& 0\\ -R_t^{b_k}\delta t& 0& 0& 0& 0\\ 0& 0& -R_t^{b_k}{R}_o^b\delta t& 0& 0\\ 0& 0& 0& I\delta t& 0\\ 0& 0& 0& 0& I\delta t\end{array}\right]\left[\begin{array}{c}{n}_a\\ n_{\omega }\\ n_v\\ n_{b_a}\\ n_{b_{\omega }}\end{array}\right]$$

4. 离散形式下的预积分误差、协方差和Jacobian（和代码一致）

这里直接给出离散形式下的预积分误差公式
$$\left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \delta {\theta }_{k+1}\\ \delta {\beta }_{k+1}\\ \delta {\eta }_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{{\omega }_{k+1}}\end{array}\right]=\left[\begin{array}{cccccc}I& f_{01}& \delta t& 0& f_{04}& f_{05}\\ 0& f_{11}& 0& 0& 0& -\delta t\\ 0& f_{21}& I& 0& f_{24}& f_{25}\\ 0& f_{31}& 0& I& 0& f_{35}\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I\end{array}\right]+\left[\begin{array}{cccccccc}{v}_{00}& v_{01}& 0& v_{03}& v_{04}& 0& 0& 0\\ 0& \frac{-\delta t}{2}& 0& 0& \frac{-\delta t}{2}& 0& 0& 0\\ \frac{-R_k\delta t}{2}& v_{21}& 0& \frac{-R_{k+1}\delta t}{2}& v_{24}& 0& 0& 0\\ 0& v_{31}& v_{32}& 0& v_{34}& v_{35}& 0& 0\\ 0& 0& 0& 0& 0& 0& \delta t& 0\\ 0& 0& 0& 0& 0& 0& 0& \delta t\end{array}\right]\left[\begin{array}{c}{n}_{a_k}\\ n_{{\omega }_k}\\ n_{v_k}\\ n_{a_{k+1}}\\ n_{{\omega }_{k+1}}\\ n_{v_{k+1}}\\ n_{b_a}\\ n_{b_{\omega }}\end{array}\right]$$
IMU相关推导过程省略，只给出轮式编码器的。用true表示有噪声真实测量值，nominal表示无噪声的理论值
$$\begin{array}{rl}& \delta \dot{\eta }={\dot{\eta }}_{true}-{\dot{\eta }}_{nominal}\\ & \begin{array}{rl}{\dot{\eta }}_{true}& ={R_t^{b_k}}_{true}{R}_o^b{{\hat{v}}_t}_{true}\\ & =R_t^{b_k}exp(\delta {\theta }^{\wedge })R_o^b({\hat{v}}_t-n_e)\\ & =R_t^{b_k}(I+\delta {\theta }^{\wedge })R_o^b({\hat{v}}_t-n_e)\\ & =R_t^{b_k}\left[R_o^b{\hat{v}}_t-R_o^b{n}_e-\delta {\theta }^{\wedge }{R}_o^b({\hat{v}}_t-n_e)\right]\\ & =R_t^{b_k}\left[R_o^b{\hat{v}}_t-R_o^b{n}_e-[R_o^b({\hat{v}}_t-n_e){]}^{\wedge }\delta \theta \right]\end{array}\\ & {\dot{\eta }}_{nominal}=R_t^{b_k}{R}_o^b{\hat{v}}_t\end{array}$$
$$\delta \dot{\eta }={\dot{\eta }}_{true}-{\dot{\eta }}_{nominal}=-R_t^{b_k}{R}_o^b{n}_e-R_t^{b_k}[R_o^b({\hat{v}}_t-n_e){]}^{\wedge }\delta \theta$$
$$\begin{array}{rl}\delta \dot{\eta }=& -\frac{1}{2}{R}_k^{b_k}{R}_o^b{n_e}_k-\frac{1}{2}{R}_{k+1}^{b_{k+1}}{R}_o^b{n_e}_{k+1}-\frac{1}{2}{R}_k^{b_k}[R_o^b({\hat{v}}_k-{n_e}_k){]}^{\wedge }\delta {\theta }_k-\frac{1}{2}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}-{n_e}_{k+1}){]}^{\wedge }\delta {\theta }_{k+1}\\ =& -\frac{1}{2}{R}_k^{b_k}{R}_o^b{n_e}_k-\frac{1}{2}{R}_{k+1}^{b_{k+1}}{R}_o^b{n_e}_{k+1}-\frac{1}{2}{R}_k^{b_k}[R_o^b({\hat{v}}_k-{n_e}_k){]}^{\wedge }\delta {\theta }_k-\frac{1}{2}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}-{n_e}_{k+1}){]}^{\wedge }\left\{\left[I-{\left(\frac{{\hat{\omega }}_k+{\hat{\omega }}_{k+1}}{2}-b_{{\omega }_k}\right)}^{\wedge }\delta t\right]\delta {\theta }_k-\frac{{n_{\omega }}_k+{n_{\omega }}_{k+1}}{2}\delta t-\delta t\delta b_{{\omega }_k}\right\}\\ =& \left\{-\frac{1}{2}{R}_k^{b_k}[R_o^b({\hat{v}}_k-{n_e}_k){]}^{\wedge }-\frac{1}{2}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}-{n_e}_{k+1}){]}^{\wedge }\left[I-{\left(\frac{{\hat{\omega }}_k+{\hat{\omega }}_{k+1}}{2}-b_{{\omega }_k}\right)}^{\wedge }\delta t\right]\right\}\delta {\theta }_k\\ & +\frac{1}{4}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}){]}^{\wedge }{n_{\omega }}_k\delta t+\frac{1}{4}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}){]}^{\wedge }{n_{\omega }}_{k+1}\delta t\\ & +\frac{1}{2}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}){]}^{\wedge }\delta b_{{\omega }_k}\delta t-\frac{1}{2}{R}_k^{b_k}{R}_o^b{n_e}_k-\frac{1}{2}{R}_{k+1}^{b_{k+1}}{R}_o^b{n_e}_{k+1}\end{array}$$
$$\begin{array}{rl}\delta {\eta }_{k+1}& =\delta {\eta }_k+\delta \dot{\eta }\delta t\\ & =f_{31}\delta {\theta }_k+f_{35}\delta b_{{\omega }_k}+v_{31}{n_{\omega }}_k+v_{32}{n_e}_k+v_{34}{n_{\omega }}_{k+1}+v_{35}{n_e}_{k+1}\end{array}$$
其中：
$$\begin{array}{rl}& f_{31}=-\frac{1}{2}{R}_k^{b_k}[R_o^b({\hat{v}}_k-{n_e}_k){]}^{\wedge }-\frac{1}{2}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}-{n_e}_{k+1}){]}^{\wedge }\left[I-{\left(\frac{{\hat{\omega }}_k+{\hat{\omega }}_{k+1}}{2}-b_{{\omega }_k}\right)}^{\wedge }\delta t\right]\delta t\\ & f_{35}=\frac{1}{2}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}){]}^{\wedge }\delta t^2\\ & v_{31}=v_{34}=\frac{1}{4}{R}_{k+1}^{b_{k+1}}[R_o^b({\hat{v}}_{k+1}){]}^{\wedge }\delta t^2\\ & v_{32}=-\frac{1}{2}{R}_k^{b_k}{R}_o^b\delta t\\ & v_{35}=-\frac{1}{2}{R}_{k+1}^{b_k}{R}_o^b\delta t\end{array}$$
将预积分误差简写成
$$\delta z_{k+1}=F\delta z_k+VQ$$
则Jacobian的迭代公式为$J_{k+1}=F{J}_k$，协方差的迭代公式为$P_{k+1}=F{P}_k{F}^T+VQ{V}^T$
$$Q=\left[\begin{array}{cccccccc}{\sigma }_a^2& 0& 0& 0& 0& 0& 0& 0\\ 0& {\sigma }_w^2& 0& 0& 0& 0& 0& 0\\ 0& 0& {\sigma }_e^2& 0& 0& 0& 0& 0\\ 0& 0& 0& {\sigma }_a^2& 0& 0& 0& 0\\ 0& 0& 0& 0& {\sigma }_w^2& 0& 0& 0\\ 0& 0& 0& 0& 0& {\sigma }_e^2& 0& 0\\ 0& 0& 0& 0& 0& 0& {\sigma }_{b_a}^2& 0\\ 0& 0& 0& 0& 0& 0& 0& {\sigma }_{b_w}^2\end{array}\right]$$

公式纯手打，若发现有错误，请指出

实践

公式可能看起来多一点，其实是和IMU预积分的公式类似的，对应的代码比较简洁。

配合代码查看

对应代码在预积分文件中 integration_base.h

void midPointIntegration(double _dt,
                            const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, const Eigen::Vector3d &_enc_v_0,
                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1, const Eigen::Vector3d &_enc_v_1,
                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Vector3d &delta_v,
                            const Eigen::Vector3d &delta_eta, const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                            Eigen::Vector3d &result_delta_eta, Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg,
                            bool update_jacobian)
    {
        // 中值法
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        Vector3d un_enc_0 = delta_q * RIO * _enc_v_0; // encoder
        Vector3d un_enc_1 = result_delta_q * RIO * _enc_v_1; // encoder
        result_delta_eta = delta_eta + 0.5 * (un_enc_0 + un_enc_1) * _dt; // encoder
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;
        

        if(update_jacobian)
        {
            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
            Vector3d a_0_x = _acc_0 - linearized_ba;
            Vector3d a_1_x = _acc_1 - linearized_ba;
            Vector3d e_0_x = RIO * _enc_v_0;
            Vector3d e_1_x = RIO * _enc_v_1; 
            Matrix3d R_w_x, R_a_0_x, R_a_1_x, R_e_0_x, R_e_1_x;

            R_w_x<<0, -w_x(2), w_x(1),
                w_x(2), 0, -w_x(0),
                -w_x(1), w_x(0), 0;
            R_a_0_x<<0, -a_0_x(2), a_0_x(1),
                a_0_x(2), 0, -a_0_x(0),
                -a_0_x(1), a_0_x(0), 0;
            R_a_1_x<<0, -a_1_x(2), a_1_x(1),
                a_1_x(2), 0, -a_1_x(0),
                -a_1_x(1), a_1_x(0), 0;
            R_e_0_x << 0, -e_0_x(2), e_0_x(1),
                    e_0_x(2), 0, -e_0_x(0),
                    -e_0_x(1), e_0_x(0), 0;
            R_e_1_x << 0, -e_1_x(2), e_1_x(1),
                    e_1_x(2), 0, -e_1_x(0),
                    -e_1_x(1), e_1_x(0), 0;

            MatrixXd F = MatrixXd::Zero(18, 18);
            F.block<3, 3>(0, 0) = Matrix3d::Identity();
            F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
            F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(0, 12) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
            F.block<3, 3>(0, 15) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
            F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
            F.block<3, 3>(3, 15) = -1.0 * MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
            F.block<3, 3>(6, 6) = Matrix3d::Identity();
            F.block<3, 3>(6, 12) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
            F.block<3, 3>(6, 15) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;

                F.block<3, 3>(9, 3) = -0.5 * delta_q.toRotationMatrix() * R_e_0_x * _dt + 
                                   -0.5 * result_delta_q.toRotationMatrix() * R_e_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
                F.block<3, 3>(9, 9) = Matrix3d::Identity();
                F.block<3, 3>(9, 15) = 0.5 * result_delta_q.toRotationMatrix() * R_e_1_x * _dt * _dt;


            F.block<3, 3>(12, 12) = Matrix3d::Identity();
            F.block<3, 3>(15, 15) = Matrix3d::Identity();

            MatrixXd V = MatrixXd::Zero(18, 24);
            V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
            V.block<3, 3>(0, 9) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 12) =  V.block<3, 3>(0, 3);
            V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(3, 12) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
            V.block<3, 3>(6, 9) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 12) =  V.block<3, 3>(6, 3);

                V.block<3, 3>(9, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_e_1_x * _dt * _dt; // 相差-
                V.block<3, 3>(9, 6) = 0.5 * delta_q.toRotationMatrix() * RIO * _dt;
                V.block<3, 3>(9, 12) = V.block<3, 3>(9, 3);
                V.block<3, 3>(9, 15) = 0.5 * result_delta_q.toRotationMatrix() * RIO * _dt;

            V.block<3, 3>(12, 18) = MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(15, 21) = MatrixXd::Identity(3,3) * _dt;

            // step_jacobian_enc = F;
            // step_V_enc = V;

            jacobian_enc = F * jacobian_enc;
            covariance_enc = F * covariance_enc * F.transpose() + V * noise_enc * V.transpose();
        }
    }

参考

1. J. Liu, W. Gao and Z. Hu, “Visual-Inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration.”
2. K. J. Wu, C. X. Guo, G. Georgiou and S. I. Roumeliotis, “VINS on wheels.”
3. 崔华坤：VINS 论文推导及代码解析