IMU姿态解算，从IMU数据中计算旋转、速度、位置，IMU测量的原理

更新(2024.2.27)：提供一个代码实现，供参考学习：>>戳我跳转<<

0. 预备

a. IMU测量值解释
IMU在测量时，得到的角速度或者加速度均是相对于地心惯性系结果，并且将该结果表示到Body坐标系下，就形成了最终的IMU输出。

记作：${\omega }_{ib}^b$，表示body系相对于惯性系的角速度测量在body下的表示。

​ $a_{ib}^b$，表示body系相对于惯性系的加速度测量在body系下的表示。

b. IMU加速度计误差建模
$${\tilde{a}}_{ib}^b=a_{ib}^b+g^b+b^a+n^a$$
其中：${\tilde{a}}_{ib}^b$ 表示的是IMU最终输出的测量值；

​ $a_{ib}^b$ 表示的是当前的加速度真值，去除了各种误差；

​ $g^b$重力加速度在body系下的表示；

​ $b^a$ 加速度计的bias，它是一个恒定偏差，并且随时间缓慢变化；

​ $n^a$加速度计的随机测量噪声，建模为0均值高斯噪声。

对于上式的加速度计建模，有时候也会建模到导航系下，多数情况下重力加速度是在导航系下有比较简洁的表示$[0,0,g{]}^T$，因此如果将加速度计的测量转到导航下就可以直接在导航系中简单的处理重力加速度。因此在实际使用时，有时也可以建模为如下形式：
$${\tilde{a}}_{ib}^b=R_{bw}(a_{ib}^w+g^w)+b^a+n^a$$
c. IMU陀螺仪误差建模
$${\tilde{\omega }}_{ib}^b={\omega }_{ib}^b+b^g+n^g$$
其中：${\tilde{\omega }}_{ib}^b$表示的是IMU最终输出的角速度测量值；

​ ${\omega }_{ib}^b$表示的是当前的角速度真值，去除了各种误差；

​ $b^g$ 陀螺仪的bias，并且随时间缓慢变化，短时间内可以认为是一个恒定偏差；

​ $n^g$ 陀螺仪的随机测量噪声，建模为0均值高斯噪声。

d. 考虑地球自转时的各个角速度关系
$${\omega }_{in}^n={\omega }_{ie}^n+{\omega }_{en}^n$$
其中：${\omega }_{in}^n$ 表示导航系相对于惯性系的角速度在导航下的表示

​ ${\omega }_{ie}^n$表示地球相对于惯性系的自转角速度在导航下的表示

​ ${\omega }_{en}^n$ 表示导航系相对于地球的角速度在导航下的表示

IMU测量的角速度是相对于惯性系i，而多数情况下进行导航姿态计算时是在导航系下的，也就是说如果直接使用IMU的角速度进行解算，就会把地球自转以及导航系变化带来的角速度都会引入到系统中，在高精度的IMU中这会带来误差，对于MEMS的IMU而言，可以忽略这一点。

对于${\omega }_{ie}^n$的计算如下：
$${\omega }_{ie}^n=[0{\omega }_{ie}\cos L{\omega }_{ie}\sin L]$$
其中：${\omega }_{ie}$是地球的自转角速度，取$7.2921151467\times 10^{-5}rad/s$。参考《捷联惯导算法与组合导航原理》 P54

​ $L$是当前位置对应的地理纬度

对于${\omega }_{en}^n$的计算如下：
$${\omega }_{en}^n=[-\frac{v_N}{R_M+h}\frac{v_E}{R_N+h}\frac{v_E}{R_N+h}\tan L]$$
其中：$v_E$,$v_N$是当前时刻载体在东北天坐标系下的速度分量

$R_M,R_N$分别为子午圈主曲率半径和卯酉圈曲率半径$R_N$，计算方法如下：参考《捷联惯导算法与组合导航原理》 P46和P48
$$R_N=\frac{R_e}{\sqrt{1-e^2{\sin }^{2}L}}$$

$$R_M=\frac{R_e(1-e^2)}{(1-e^2{\sin }^{2}L{)}^{\frac{3}{2}}}$$

​ $L,h$分别是当前位置对应的地理纬度和高度

注意：上面介绍了IMU的误差建模，但是后续的推导均未考虑误差

1. 姿态更新

参考《捷联惯导算法与组合导航原理》 P79

a.惯性系下的IMU姿态解算

$${\dot{C}}_b^i=C_b^i\ast ({\omega }_{ib}^b\times )$$
上式是IMU的姿态的微分方程。如果要在惯性系下对上式进行解微分方程，即可得到IMU姿态计算结果。结果如下：

$$C_{b(m)}^i=C_{b(m-1)}^i{C}_{b(m)}^{b(m-1)}$$
其中：$C_{b(m)}^i$ 表示m时刻，b系到i的变换矩阵

​ $C_{b(m)}^{b(m-1)}$ 表示由b系在m时刻到m-1时刻的变化矩阵，它就是有IMU的角速度测量计算而来。

需要格外的关注的是$C_{b(m)}^{b(m-1)}$的计算，它是IMU姿态计算的关键，如果m-1时刻到m是IMU符合定轴转动，则由下式严格成立：

$$C_{b(m)}^{b(m-1)}=e^{{\int }_{t_{m-1}}^{t_m}{\omega }_{ib}^b(t)dt}$$
注意：上式是在定轴假设下才严格成立的，所谓定轴转动就是角速度的方向不发生变化，但是很显然这个假设在实际情况下不严格成立，但是幸运的是目前的IMU都是高频率的IMU它一次测量内很接近定轴转动。对于$e^{{\int }_{t_{m-1}}{t}_m{\omega }_{ib}^b(t)dt}$的计算有多种方式，有欧拉积分，中值积分，矢量二子样算法，单子样+前一周期等算法。参考《捷联惯导算法与组合导航原理》 P28~P30。尽管解算方法很多，但是对于消费级的MEMS IMU使用中值积分就足够使用。

b.导航系下的IMU姿态解算

上面的过程是围绕着参考系为i系进行的，多数情况下我们在使用时都是用的导航系，下面就以东北线(ENU)导航系对IMU姿态解算进行推导介绍。导航下旋转矩阵对时间的微分方程如下：

$${\dot{C}}_b^n=C_b^n\ast ({\omega }_{nb}^b\times )$$
当考虑上地球转速以及导航系变化的角速度时，${\omega }_{nb}^b={\omega }_{ib}^b-{\omega }_{in}^b$，上式可以进一步列写为：

$${\dot{C}}_b^n=C_b^n\ast ({\omega }_{nb}^b\times )=C_b^n[({\omega }_{ib}^b-{\omega }_{in}^b)\times ]=C_b^n({\omega }_{ib}^b\times )-({\omega }_{in}^n\times )C_b^n$$
对上式进行求微分方程，并进行离散化就可以得到导航系下的姿态更新方法。但是求解比较麻烦，可以采用矩阵链式法则进行求解：

$$C_{b(m)}^{n(m)}=C_{n(m-1)}^{n(m)}{C}_i^{n(m-1)}{C}_{b(m-1)}^i{C}_{b(m)}^{b(m-1)}=C_{n(m-1)}^{n(m)}{C}_{b(m-1)}^{n(m-1)}{C}_{b(m)}^{b(m-1)}$$
其中：$C_{n(m-1)}^{n(m)}$ 表示m-1时刻到m时刻导航系发生的姿态变化，也相当于在m时刻对应的导航系下表示m-1时刻导航系的位姿，计算方法是使用${\omega }_{in}^n={\omega }_{ie}^n+{\omega }_{en}^n$.

​ $C_{b(m-1)}^{n(m-1)}$ 表示m-1时刻IMU相对于导航系的姿态

​ $C_{b(m)}^{b(m-1)}$ 表示m时刻到m-1时刻IMU发生的位姿变化，它来自于IMU角速度测量的积分。

c. 导航系简化版姿态计算

$$C_{b(m)}^{n(m)}=C_{n(m-1)}^{n(m)}{C}_{b(m-1)}^{n(m-1)}{C}_{b(m)}^{b(m-1)}$$
在MEMS IMU中上式可以忽略导航系的运动$C_{n(m-1)}^{n(m)}$，因为消费级普通IMU无法敏感到这一项，所以可以忽略。于是可以得到简化版的姿态解算：

$$C_{b(m)}^{n(m)}=C_{b(m-1)}^{n(m-1)}{C}_{b(m)}^{b(m-1)}$$
其中，$C_{b(m)}^{b(m-1)}=e^{{\int }_{t_{m-1}}^{t_m}{\omega }_{ib}^b(t)dt}$，对于它的求解与前述思路一样，可以采用欧拉积分，中值积分等求解${\int }_{t_{m-1}}^{t_m}{\omega }_{ib}^b(t)dt$，然后使用罗德里格斯公式完成指数映射。

2. 速度更新

参考《捷联惯导算法与组合导航原理》 P81

a. 精确速度微分建模

$${\dot{v}}_{en}^n=C_b^n{f}_{sf}^b-(2w_{ie}^n+w_{en}^n)\times v_{en}^n+g^n$$
其中：$f_{sf}^b$ 为加速度计测量的比力

​ $-(2w_{ie}^n+w_{en}^n)\times v_{en}^n$ 为有害加速度

​ $g^n$ 为重力加速度

以上便是对导航系下载体速度的精确微分建模，对其积分即可得到速度的解算，具体过程比较复杂，参考《捷联惯导算法与组合导航原理》 P82。

在IMU加速度误差的建模中使用的是加速度一词，这里使用的是比力，实际上可以认为它们是同样的意义。

b. 简化版速度微分建模

在MEMS IMU中，有害加速度这一项可以忽略，并忽略一些类似划桨误差等，可以得到简化版的速度更新方程：

$${\dot{v}}_{en}^n=C_b^n{f}_{sf}^b+g^n$$
上式求积分之后改写成递推的形式为：

$$v_m^{n(m)}=v_{m-1}^{n(m-1)}+\Delta v_{sf(m)}^n$$
其中：$v_{m-1}^{n(m-1)}$为m-1时刻载体速度在导航下的表示。

$$\Delta v_{sf(m)}^n={\int }_{m-1}^m(C_b^n{f}_{sf}^b+g^n)dt$$
上式的求解可以采用欧拉积分，中值积分等完成。

3. 位置更新

对于位置的更新，在参考《捷联惯导算法与组合导航原理》 P87 中采用的是经纬高来表示位置。而实际使用中多以导航系原点来表示位置，因此此处不总结介绍经纬高的方式。将直接在导航系下进行解算。

$${\dot{p}}^{n(m)}=v^{n(m)}$$
对上式进行欧拉积分，即可得到递推方程：

$$p^{n(m)}=p^{n(m-1)}+{\int }_{m-1}^m{v}^n(t)dt$$
同样的方式，可以采用欧拉积分，中值积分等对${\int }_{m-1}^m{v}^n(t)dt$进行求解。

例如，使用中值积分，并假设m-1到m时刻速度为线性变化，则有如下式：

$$p^{n(m)}=p^{n(m-1)}+v^{n(m-1)}\ast \Delta t+\frac{1}{2}(f_{sf}^{n(m-1)}+g^n)\Delta t^2$$
注意：IMU的姿态解算过程比较复杂，在建模时可以做的非常精确，这会导致模型非常复杂，实际当中使用时，普通的MEMS IMU无需做复杂的建模，不需要考虑地球转速，导航系的运动，速度划桨误差等等。当我们有了IMU初始时刻的姿态、速度、位置之后，就可以使用上述的递推方式计算出后续的姿态变化。但是对于低精度的IMU这个递推的过程维持不了多久就会发散。