VIO学习（2）——IMU传感器

IMU即是惯性测量单元，可以将其看作是加速度计（acc）与陀螺仪（gyro)的组合，它会直接输出该传感器当前的加速度( $\mathbf{a}$)与角速度( $\omega$)，根据这些信息，可以用于估计该传感器的位移、速度等等信息。

IMU传感器的模型

IMU的模型如下：

$$\begin{array}{rl}& {\tilde{\omega }}^b={\omega }^b+{\mathbf{b}}_g^b+{\mathbf{n}}_g^b\\ & {\tilde{\mathbf{a}}}^b={\mathbf{q}}_{bw}({\mathbf{a}}^w+{\mathbf{g}}^w)+{\mathbf{b}}_a^b+{\mathbf{n}}_a^b\end{array}$$

各个符号的解释如下：

1. $w$ $\to$ 世界坐标系world；
2. $b$ $\to$ IMU的body坐标系；
3. ${\tilde{\omega }}^b$ $\to$ 在IMU的body坐标系下，IMU输出的角速度的测量值；
4. ${\omega }^b$ $\to$ 在IMU的body坐标系下，IMU的角速度的真值；
5. ${\tilde{\mathbf{a}}}^b$ $\to$ 在IMU的body坐标系下，IMU输出的加速度的测量值；
6. ${\mathbf{a}}^b$ $\to$ 在IMU的body坐标系下，IMU输出的加速度的真值；
7. ${\mathbf{b}}_g^b$与${\mathbf{b}}_a^b$分别表示在IMU的body坐标系下，加速度计（acc）与陀螺仪（gyro）的偏置值，这个值是可以标定出来的，这是一个干扰值；
8. ${\mathbf{n}}_g^b$与${\mathbf{n}}_a^b$分别表示在IMU的body坐标系下，加速度计（acc）与陀螺仪（gyro）的测量值中含有的高斯白噪声，这个值是标定不出来的。
9. ${\mathbf{q}}_{bw}$表示世界坐标系world相对于IMU的body坐标系的姿态，此处是一个单位四元数（其实也可以用旋转矩阵），${\mathbf{q}}_{bw}({\mathbf{a}}^w+{\mathbf{g}}^w)$表示把在world坐标系描述的向量转换到body坐标系下来描述。

IMU的P(osition),V(elocity),Q(uatenion)对时间的导数可以写成：
$$\begin{array}{rl}& {\dot{\mathbf{p}}}_{w{b}_t}={\mathbf{v}}_t^w\\ & {\dot{\mathbf{v}}}_t^w={\mathbf{a}}_t^w\\ & {\dot{\mathbf{q}}}_{w{b}_t}={\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right]\end{array}$$

其中$b_t$表示t时刻IMU的body坐标系。

IMU的运动模型

若已知第i时刻IMU的P、V、Q值，可以通过对iMU的测量值进行积分，就可以得到第j时刻的PVQ值，如下：

$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t+{\iint }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t^2\\ & {\mathbf{v}}_j^w={\mathbf{v}}_i^w+{\int }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t\\ & {\mathbf{q}}_{w{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$

上面是一个连续时间上的积分，在写成程序时，需要将上面的式子进行离散化，离散的方式有两种，一种是欧拉法，另外一种是中值法， 中值法的精度比欧拉法的高。

欧拉法

使用欧拉法，即两个相邻时刻k到k+1的位姿是用第k时刻的测量值$\mathbf{a}$、$\omega$来计算。

$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_{k+1}}={\mathbf{p}}_{w{b}_k}+{\mathbf{v}}_k^w\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2\\ & {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+\mathbf{a}\Delta t\\ & {\mathbf{q}}_{w{b}_{k+1}}={\mathbf{q}}_{w{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\end{array}$$

其中
$$\begin{array}{l}\mathbf{a}={\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_a^{b_k}\right)-{\mathbf{g}}^w\\ \omega ={\mathbf{\omega }}^{b_k}-{\mathbf{b}}_g^{b_k}\end{array}$$

中值法

使用中值法，即两个相邻时刻k到k+1的位姿是用第k时刻与第k+1时刻的测量值$\mathbf{a}$、$\omega$的平均值来计算。

$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_{k+1}}={\mathbf{p}}_{w{b}_k}+{\mathbf{v}}_k^w\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2\\ & {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+\mathbf{a}\Delta t\\ & {\mathbf{q}}_{w{b}_{k+1}}={\mathbf{q}}_{w{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\end{array}$$

其中

$$\begin{array}{rl}& \mathbf{a}=\frac{1}{2}\left[{\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_a^{b_k}\right)-{\mathbf{g}}^w+{\mathbf{q}}_{w{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_a^{b_k}\right)-{\mathbf{g}}^w\right]\\ & \omega =\frac{1}{2}\left[\left({\omega }^{b_k}-{\mathbf{b}}_g^{b_k}\right)+\left({\omega }^{b_{k+1}}-{\mathbf{b}}_g^{b_k}\right)\right]\end{array}$$