【VIO】第1讲 IMU 传感器

1.旋转运动学

速度合成公式：
$$v^i=v_{ib}^i+w_{ib}^i\times r^i+R_{ib}{v}^b$$
加速度合成公式：
$$a^i=\underset{欧拉力}{\underbrace{{\dot{w}}_{ib}^i\times r^i}}+\underset{离心力}{\underbrace{w_{ib}^i\times (w_{ib}^i\times r^i)}}+\underset{科氏力}{\underbrace{2w_{ib}^i\times v^i}}+R_{ib}{a}^b$$

2.IMU 测量模型及运动模型

​ 1）加速度计

​ 2）陀螺仪

3.IMU 误差模型

​ 1）确定性误差

​ 零偏（Bias）：IMU传感器在完全静止状态下存在的非零输出值

​ 尺度因子（Scale）： IMU传感器自身尺度不准引起的测量误差

​ 不垂直度（Nonorothogonality）：IMU传感器中由于各轴安装不垂直引起的误差

​ 2）随机误差

​ 高斯白噪声：均值为 0 ，方差为 $\sigma$ 的独立高斯过程
$$n(t)\sim (0,\sigma )$$
​ bias随机游走

​ 3）高斯白噪声的离散积分

​ 离散状态下的高斯白噪声 $n(t)$ 服从如下分布：
$$n_d(k)\sim N(0,\frac{{\sigma }^2}{\Delta t})$$
​ 离散方差 ${\sigma }_d$ ：
$${\sigma }_d=\frac{\sigma }{\sqrt{\Delta t}}$$
​ 高斯白噪声的连续时间到离散时间之间差 $\frac{1}{\sqrt{\Delta t}}$， 其中 $\Delta t$ 为传感器的采样时间

​ 4）零偏的离散积分

​ 已知：
$$\dot{b(t)}=n(t)={\sigma }_{b}w(t)$$
​ 所以可以得到离散状态下零偏符合分布：
$$b_d(k)\sim N(b_d(k-1),\Delta t{\sigma }_b^2)$$

​ bias 随机游走的噪声方差从连续时间到连续时间之间需要乘以 $\sqrt{\Delta t}$

4.IMU 随机误差标定

​ Allan 方差标定

5.加速度计与陀螺仪误差模型

（1）加速度计

​ 导航系G为东北天，$g^G=(0,0,-9.81{)}^T$

​ 理论测量值：
$$a_m^B=R_{BG}(a^G-g^G)$$
​ 考虑高斯白噪声，bias，以及尺度因子
$$a_m^B=S_a{R}_{BG}(a^G-g^G)+n_a+b_a$$
（2）陀螺仪

​ 考虑高斯白噪声，bias，以及尺度因子
$$w_m^B=S_g{w}^B+n_g+b_g$$

6.VIO中的IMU模型

忽略scale，只考虑白噪声和bias随机游走：
$${\tilde{w}}^b=w^b+b^g+n^g$$

$${\tilde{a}}^b=q_{bw}(a^w+g^w)+b^a+n^a$$

上标g表示陀螺仪，a表示加速度计，w表示世界坐标系，b表示IMU机体坐标系。IMU真实值 $w,a$， 测量值 $\tilde{w}$, $\tilde{a}$

7.运动模型的离散积分

（1）欧拉法
$$a=q_{w{b}_k}(a^{b_k}-b_k^a)-g^w$$

$$w=w^{b_k}-b_k^g$$

（2）中值法
$$a=\frac{1}{2}[q_{w{b}_k}(a^{b_k}-b_k^a)-g^w+q_{w{b}_{k+1}}(a^{b_{k+1}}-b_k^a)-g^w]$$

$$w=\frac{1}{2}[(w^{b_k}-b_k^g)+(w^{b_{k+1}}-b_k^g)]$$

// imu 动力学模型 欧拉积分

Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;

// 中值积分
Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;
Qwb = Qwb * dq;
Eigen::Vector3d acc_w1 = Qwb * (imupose.imu_acc) + gw;
acc_w = 0.5 * (acc_w + acc_w1);

Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15