惯性导航解算（三）续

二、惯性导航解算的方法

前面一节主要是在讲解旋转的数学表达形式，而这一节主要讲解的是怎么通过原始的角速度和加速度得到我们想要的位置、速度、姿态，而具体点就是“角速度->姿态”、“加速度->速度”、“速度->位置”，而这三种情况就很明显是一个积分问题。

1. 姿态更新

1.1 基于旋转矩阵的姿态更新

1）连续时间处理

旋转矩阵的微分方程为
$${\dot{\mathbf{R}}}_{wb}={\mathbf{R}}_{wb}[\mathbf{\omega }{]}_{\times }$$
其中$ \boldsymbol{\omega}$ 为IMU三个轴测量得到的角速度。

根据微分方程可以很容易写出它的积分形式为（此处用到的是高数中讲的求一阶微分方程通解的方法，基础知识不展开介绍了）
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}{e}^{{\int }_{t_{k-1}}^{t_k}[\mathbf{\omega }{]}_{\times }d\tau }$$
这里给出的是由 k-1 时刻的姿态积分得到 k 时刻姿态的积分形式。仔细观察指数部分，它表示的是角速度矢量的积分，而角速度矢量的积分其实不就是 k-1 时刻到 k 时刻对应的旋转向量嘛（这里也可以这样理解，即两个旋转矩阵之间的变化可以由一个旋转向量来表示），因此，我们可以直接写成
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}{e}^{\varphi \times }$$
由于反对称矩阵的指数函数前面已经推导完毕(罗德里格斯公式)，因此上式又可以写为
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}{\mathbf{R}}_{b_{k-1}{b}_k}$$
其中
$${\mathbf{R}}_{b_{k-1}{b}_k}=I+\frac{\sin \varphi }{\varphi }(\mathbf{\varphi }\times )+\frac{1-\cos \varphi }{{\varphi }^2}(\mathbf{\varphi }\times {)}^2$$
可见，求旋转矩阵的问题，最终转化成了旋转向量的问题。咋求？接着用微分方程求呗。旋转向量的微分方程为
$$\dot{\varphi }\approx \omega +\frac{1}{2}\varphi \times \omega$$
这个方程看起来还是比较复杂的，可以也应当化简。在方程右边第二项，可以看到是两个矢量的叉乘，根据叉乘的定义，当两个矢量方向重合时，叉乘的结果为0。一般 imu 的采样频率不会低于 100hz，即 k-1 时刻到 k 时刻的时间间隔不会大于10ms，而载体一般自动驾驶车或者机器人也不会发生过于高频的运动，因此可以近似认为满足叉乘为0的条件，因此可以直接把上式简化为
$$\dot{\varphi }\approx \omega$$
它对应的积分形式就很简单了
$$\varphi \approx {\int }_{t_{k-1}}^{t_k}\mathbf{\omega }(\tau )d\tau$$
有了旋转向量的积分形式，那么该旋转矩阵的形式就了然了，直接带进去就行。

但是，到这里会遇到一个问题，我们一直讨论的都是连续时间的，即 k-1 时刻到 k 时刻之间的所有值都是连续可测的，而实际数据都是离散的，只能得到 k-1 和 k 这两个时刻的值，那么必须把上面的连续时间形式高程离散形式，才能继续走下去了。

2）离散时间处理

这就牵扯到常见的三种方法：欧拉法、中值法、龙哥库塔法，三者按复杂程度逐渐递增，按精度也是逐渐递增。我们先从简单的开始说。

a.欧拉法

欧拉法很好理解，虽然我不知道 k-1 到 k 时刻的所有值，但是如果我假设 imu 在这个时间段内是匀速旋转的，那不就解决了吗，此时，对应的旋转向量为
$$\mathbf{\varphi }={\mathbf{\omega }}_{k-1}\left(t_k-t_{k-1}\right)$$
b.中值法

欧拉法固然简单，但是匀速的假设还是有些太强了，实际使用中会发现精度不够，于是就有了中值法。中值法假设 imu 是匀加速运动的，这就比匀速更高级了一些，此时离散时间形式为
$$\mathbf{\varphi }=\frac{{\mathbf{\omega }}_{k-1}+{\mathbf{\omega }}_k}{2}\left(t_k-t_{k-1}\right)$$
c.龙哥库塔法

中值法假设的匀加速运动，在imu运动更加剧烈的情况下仍然不够，那就继续把模型复杂化呗，龙哥库塔法就是这种，把运动模型搞成一个高阶曲线。由于这部分略显复杂，而且实际使用中，中值法多数情况下已经够用，因此在此处就不对这种方法做过多介绍了。

1.2 基于四元数的姿态更新

1）连续时间处理

四元数的微分方程为
$${\dot{\mathbf{q}}}_{wb}={\mathbf{q}}_{wb}\otimes \frac{1}{2}\left[\begin{array}{l}0\\ \mathbf{\omega }\end{array}\right]$$
其矩阵形式为(可回顾四元数乘法那一小节的推导)
$${\dot{\mathbf{q}}}_{wb}=\frac{1}{2}{\left[\begin{array}{l}0\\ \mathbf{\omega }\end{array}\right]}_R{\mathbf{q}}_{wb}$$
同样按照微分方程的解法，得到其积分形式为
$${\dot{\mathbf{q}}}_{wb}={\mathrm{e}}^{\frac{1}{2}\mathbf{\Theta }}{\mathbf{q}}_{wb}$$
其中
$$\mathbf{\Theta }=\left[\begin{array}{cccc}0& -{\varphi }_x& -{\varphi }_y& -{\varphi }_z\\ {\varphi }_x& 0& {\varphi }_z& -{\varphi }_y\\ {\varphi }_y& -{\varphi }_z& 0& {\varphi }_x\\ {\varphi }_z& {\varphi }_y& -{\varphi }_x& 0\end{array}\right]$$
为了求解该方程，要对指数函数进行泰勒展开，同样包含高次幂，展开方法与前面旋转向量指数形式展开的方法类似，可以自行推导，此处直接给出结论
$${\mathrm{e}}^{\frac{1}{2}\Theta (T)}=I\cos \frac{\varphi }{2}+\frac{\Theta }{\varphi }\sin \frac{\varphi }{2}$$
因此有
$${\mathbf{q}}_{w{b}_k}=\left[I\cos \frac{\varphi }{2}+\frac{\mathbf{\Theta }}{\varphi }\sin \frac{\varphi }{2}\right]{\mathbf{q}}_{w{b}_{k-1}}$$
由于
$${\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ \frac{\varphi }{\varphi }\sin \frac{\varphi }{2}\end{array}\right]}_R=\left[I\cos \frac{\varphi }{2}+\frac{\Theta }{\varphi }\sin \frac{\varphi }{2}\right]$$
另
$${\mathbf{q}}_{b_{k-1}{b}_k}=\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ \frac{\varphi }{\varphi }\sin \frac{\varphi }{2}\end{array}\right]$$
则
$${\mathbf{q}}_{w{b}_k}={\mathbf{q}}_{w{b}_{k-1}}\otimes {\mathbf{q}}_{b_{k-1}{b}_k}$$

2) 离散时间处理

此处也是同样用中值法算出旋转矢量即可得到 ${\mathbf{q}}_{b_{k-1}{b}_k}$ ，从而得到更新后的四元数 ${\mathbf{q}}_{w{b}_k}$ ，与前述方法相同，不再重复。

1.3 总结

1. 通过旋转矩阵去更新姿态需要去求一个增量选装矩阵${\mathbf{R}}_{b_{k-1}{b}_k}$，而${\mathbf{R}}_{b_{k-1}{b}_k}$则需要去求旋转向量，如果器件直接输出的是角增量（角速度在时间上的积分）那就皆大欢喜，如果是输出的角速度则需要通过中值积分求得选装向量。通过四元数更新也是类似的。

2. 角增量和旋转向量的区别

   ​ 如果是在匀速模型下，也就是前后两个时刻的旋转方向没有发生改变，那么认为角增量和旋转向量是相等的，如果是在线性或在高阶情况下，则不相等。

2. 速度更新

相比来讲，速度更新就显得很简单，速度的微分方程为
$$\dot{\mathbf{v}}={\mathbf{R}}_{wb}\mathbf{a}-\mathbf{g}$$
其中$\mathbf{a}=\left[\begin{array}{lll}{a}_x& a_y& a_z\end{array}\right]$ 为测量加速度， $\mathbf{g}=\left[\begin{array}{lll}0& 0& g_0\end{array}\right]$为重力加速度。

该微分方程的通解形式为
$$\Delta \mathbf{v}=\left({\mathbf{R}}_{wb}\mathbf{a}-\mathbf{g}\right)\Delta t$$
对应的基于中值法的速度更新形式为
$${\mathbf{v}}_k={\mathbf{v}}_{k-1}+\left(\frac{{\mathbf{R}}_{w{b}_k}{\mathbf{a}}_k+{\mathbf{R}}_{w{b}_k}{\mathbf{a}}_{k-1}}{2}-\mathbf{g}\right)\left(t_k-t_{k-1}\right)$$

3. 位置更新

位置更新就更加简单，微分形式为
$$\dot{\mathbf{p}}=\mathbf{v}$$
其通解形式为
$$\Delta \mathbf{p}=\mathbf{v}\Delta t$$
基于中值法的离散形式为
$${\mathbf{p}}_k={\mathbf{p}}_{k-1}+\frac{{\mathbf{v}}_k+{\mathbf{v}}_{k-1}}{2}\left(t_k-t_{k-1}\right)$$
另外，如果把速度的通解代入，则位置的通解形式也可以写为
$$\Delta \mathbf{p}=\mathbf{v}\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2$$
相应的基于中值法的离散形式为
$$\begin{array}{r}{\mathbf{p}}_k={\mathbf{p}}_{k-1}+{\mathbf{v}}_{k-1}\left(t_k-t_{k-1}\right)+\frac{1}{2}\left(\frac{{\mathbf{R}}_{w{b}_k}{\mathbf{a}}_k+{\mathbf{R}}_{w{b}_{k-1}}{\mathbf{a}}_{k-1}}{2}-\mathbf{g}\right){\left(t_k-t_{k-1}\right)}^2\end{array}$$

4. 补充知识

4.1 导航系与惯性系

在实际导航模型中,不动系(i系)是地心惯性系,而我们要的导航结果是相对于导航系(n系)的,这两
个坐标系之间有相对旋转,他们之间的旋转角速度可以表示为
$${\mathbf{\omega }}_{in}^n={\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n$$
其中${\mathbf{\omega }}_{ie}^n$表示地球自转引起的角速度;${\mathbf{\omega }}_{en}^n$表示载体在地球表面运动时,绕地球旋转形成的角速度。其表达式如下:
$$\begin{gathered} {\mathbf{\omega }}_{ie}^n={\left[\begin{array}{ccc}0& {\mathbf{\omega }}_{ie}^n\cos L& {\mathbf{\omega }}_{ie}^n\sin L\end{array}\right]}^T \\ {\mathbf{\omega }}_{en}^n={\left[\begin{array}{ccc}-\frac{v_N}{r_M+h}& \frac{v_E}{r_N+h}& \frac{v_E}{r_N+h}\tan L\end{array}\right]}^T \end{gathered}$$
后者的理解需要借助地球模型,此处不引入地球模型的推导,其推导过程同样可参考《捷联惯导算法与组合导航
原理》(严恭敏等编著)。
原文链接：多传感器融合定位理论基础（五）：惯性导航解算