【VIO】第1讲 IMU 传感器

VIO

花舞の街道

798人浏览 · 2022-09-30 16:14:55

花舞の街道 · 2022-09-30 16:14:55 发布

1.旋转运动学

速度合成公式：
$v^i = v_{ib}^i + w_{ib}^i \times r^i +R_{ib}v^b$
加速度合成公式：
$a^i = \underbrace{\dot{w}_{ib}^i \times r^i}_{欧拉力} + \underbrace{w_{ib}^i \times (w_{ib}^i \times r^i)}_{离心力} + \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$ 的独立高斯过程
$\sim (0, \sigma)$
bias随机游走

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

离散状态下的高斯白噪声 $n (t)$ 服从如下分布：
$n_d(k)\sim N(0, \frac{\sigma^2}{\Delta t})$
离散方差 $σd\sigma_d$ ：
$\sigma_d = \frac{\sigma}{\sqrt{\Delta t}}$
高斯白噪声的连续时间到离散时间之间差 $1Δt\frac{1}{\sqrt{\Delta t}}$ ，其中 $Δ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 随机游走的噪声方差从连续时间到连续时间之间需要乘以 $Δt\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_aR_{BG}(a^G-g^G) + n_a + b_a$
（2）陀螺仪

考虑高斯白噪声，bias，以及尺度因子
$w_m^B = S_gw^B + n_g+b_g$

6.VIO中的IMU模型

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

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

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

7.运动模型的离散积分

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

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

（2）中值法
$\frac{1}{2} [q_{wb_k}(a^{b_k} - b^a_k ) - g^w + q_{wb_{k+1}}(a^{b_{k+1}} - b^a_k ) - g^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;