IMU数学模型详细推导

1|0旋转运动学

质量块在body坐标系下的坐标为：

body坐标系为imu坐标系惯性系为世界坐标系
只考虑旋转旋转到惯性系下：

对时间求导如下：

其中

对 $R$ 的求导有如下推导：

其中 $[w^b]_\times=w^{\wedge}$
$[w^b]_\times r =w^b\times r$
对坐标求二阶导得出：

其中 $\dot{R}_{ib}w^b =R_{ib}[w^b]_\times w^b =R_{ib}w^b\times w^b =0$ 自身和自身的叉乘为零

2|0imu测量模型

imu中包含两种测量模型：加速度计和陀螺仪
加速度计用来测量加速度的大小陀螺仪用来测量角速度的大小

2|1加速度计

这里用弹簧模型理解实际中会使用电容器

加速度的坐标系通常为东北天坐标系：

g = (0, 0, - 9.81)^{T}

$g = (0, 0, -9.81)^T$

静止时加速度计测得加速度为0 重力加速度为g
自由落体时加速度计测得加速度为g 重力加速度为0
加速度计不需要考虑科氏力的影响因为在某一时刻科氏力会等于0 达到平衡状态

2|2陀螺仪

测量 a 和 v 可以求出 w
但是会受到科氏力的影响所以会采用音叉陀螺将科氏力抵消掉

3|0误差模型

IMU误差分为确定性误差和随机误差

3|1确定性误差

确定性误差主要分为：bias和scale
bias即经常说的零偏：理论上没有外力作用时 IMU的传感器输出应为0 但实际上会出现一个偏置bias
scale可以看成实际数据与传感器输出数据之间的一个比值
以及还有xyz轴不重合的误差

常使用六面法标定加速度计和陀螺仪不过多介绍

3|2随机误差（重点）

随机误差分为高斯白噪声和bias随机游走

1|0连续时间上的随机误差

1|0高斯白噪声：

狄拉克函数：当 $t_1 = t_2$ 时为1 其他时刻均为0 说明了每段时间的独立性

1|0bias随机游走：

我们认为它的导数为高斯白噪声 $w$ 为方差为1均值为0的白噪声

1|0离散时间上的随机误差

一个单轴角速度受到高斯白噪声和bias的影响：

当传感器采集信号时认为采样时间段内信息为常数
我们无法确定随机误差的积分是否为常数所以单独对这两种随机误差进行分别积分：

1|0高斯白噪声：

高斯白噪声的方差（这里为什么要平方？因为这就是方差的定义详见《矩阵分析与应用》）

我们认为：

其中：

可以得出结论：对比高斯白噪声的连续时间和离散时间的 $n_d[k]$ ，高斯白噪声的连续时间和离散时间相差一个 $\frac{1}{\sqrt{\Delta t}} \quad$ 其中 $\Delta t$ 为传感器的采集时间（1/HZ）

1|0bias随机游走：

bias积分部分：

协方差为：

上面都不重要
结果：

其中：

得出结论：同样这里对比的是 $b_d[k]$ bias随机游走的噪声方差从连续时间到离散时间需要乘以一个 $\sqrt{\Delta t}$
实际操作时我们会使用kalibr_allan对imu进行标定算出高斯白噪声误差和bias随机游走误差（即 $\sigma$ ）
使用ROS 生成imu数据包再使用kalibr_allan将数据包转换为mat文件再输入进kalibr_allan

4|0IMU模型

其中b为bias随机游走误差 n为高斯白噪声
$w^b$ 为理想值 $\tilde{w}^b$ 为测量值 a同理

5|0运动模型的离散积分

5|1欧拉法

其中

对应到代码：

//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half =  imupose.imu_gyro * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();
        
/// 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;

5|2中值法

其中

对应到代码：

//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half =  (imupose.imu_gyro + imudata[i - 1].imu_gyro)/2.0 * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();
        
/// 中值积分
Eigen::Vector3d acc_w = (Qwb * dq * imupose.imu_acc + gw + Qwb * imudata[i - 1].imu_acc + gw)/2.0;
Qwb = Qwb * dq;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;

__EOF__

本文作者：铃灵狗的水墨书香
本文链接：https://www.cnblogs.com/linglingdog/p/15936721.html
关于博主：评论和私信会在第一时间回复。或者直接私信我。
版权声明：本博客所有文章除特别声明外，均采用 BY-NC-SA 许可协议。转载请注明出处！
声援博主：如果您觉得文章对您有帮助，可以点击文章右下角【推荐】一下。您的鼓励是博主的最大动力！