惯性导航公式浅推

引言

之前推过惯导的更新方程，但是久而久之又会忘掉一些。今天刚看了一篇论文，又重新推了一下。这里将自己理解的推导过程做个记录，便于以后翻看，同时通过记录的方式，加深自己的印象。

惯性导航状态方程

惯性导航所用的传感器就是我们通常所说的IMU（Inetial Measurement Unit），一般包括三轴加速度计和三轴陀螺仪。陀螺仪输出的是载体实时的旋转角速度，加速度计输出的是载体实时的加速度和重力加速度的组合加速度。通过这两个量，我们可以实时计算载体的位移，速度和姿态。其中位移和速度通过加速度对时间积分得到，姿态通过角速度对时间积分得到：

角速度积分得到姿态(姿态矩阵更新方程)：

首先给出姿态矩阵对时间的微分方程：
$${\dot{R}}_b^n=R_b^n{\Omega }_{nb}^b\text{\hspace{1em}(1-1)}$$
其中$R_b^n$表示b系到n系的旋转（姿态）矩阵。${\Omega }_{nb}^b$表示b系相对于n系的旋转角速度在b系下的表示${\omega }_{nb}^b=[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$的斜对称矩阵：
$${\Omega }_{nb}^b=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
常用坐标系及其相互关系：
${\omega }_{nb}^b$一般还不是陀螺仪的输出值${\omega }_{ib}^b$，陀螺仪输出值是载体坐标系$b$系相对于惯性坐标系$i$系的旋转角速度，而我们通常用于计算的参考坐标系$n$系不是惯性坐标系$i$。在地球上常用的惯性系为地心坐标系：$z$轴为地球自转轴，$x$轴和$y$轴在赤道平面互相垂直，坐标系保持静止。$n$系有时表示地心地固(ECEF)坐标系，即通常所说的$e$系，$z$轴为地球自转轴，$x$轴和$y$轴在赤道平面互相垂直，坐标系随地球自转而转动。即ECEF系相对于$i$系，有一个固定的旋转角速度，即地球自转角速度。$n$系有时表示当地(Local）坐标系$l$系。$l$系通常有东北天（E(x)-N(y)-U(z)）系和北东地（N(x)-E(y)-D(z)）系之分。$l$系将当前所在位置的水平面作为自己x，y轴所在平面，z轴垂直于水平面，一般是指向重力方向(Down)或反方向(Up)。$l$系相对于$e$系也会有旋转速度，这个旋转速度根据$l$系在地球上的位置变化速度而定，但除了高速运动的载体（飞机，导弹）外，低速运动载体（如车载）的$l$系因其速度而产生的相对于$e$系旋转速度极小，一般忽略不计（陀螺仪精度不足以感知这个小量）。（以上都是坐标系都是右手坐标系）
如果选取$l$系为参考坐标系$n$系的话，则有：
$${\omega }_{nb}^b={\omega }_{ib}^b-{\omega }_{ie}^b-{\omega }_{el}^b\text{\hspace{1em}(1-2)}$$
其中${\omega }_{ie}^b$为陀螺仪的原始输出值。注意在低速运动载体中最后一项${\omega }_{el}^b$可以忽略；而在低精度陀螺仪中后两项${\omega }_{ie}^b$和${\omega }_{el}^b$都可以忽略，因其精度不足以感知这两个小量。
下面解微分方程（1-1）:
公式（1-1）相当于如下形式的微分方程：$\dot{y}=yx$。解这个微分方程：
$$\begin{gathered} \frac{dy}{dt}=yx \\ \frac{dy}{y}=xdt \\ \ln (y){|}_k^{k+1}=\int xdt \\ \ln (y_{k+1})-\ln (y_k)=\int xdt \\ \ln (\frac{y_{k+1}}{y_k})=\int xdt \\ \frac{y_{k+1}}{y_k}=e^{\int xdt} \\ {y}_{k+1}=y_k{e}^{\int xdt} \end{gathered}$$
因此，姿态矩阵的更新方程为：
$$R_{b,k+1}^n=R_{b,k}^n\ast e^{\int {\Omega }_{nb,k}^{b}dt}\text{\hspace{1em}(1-3)}$$
上式对$e$指数展开，写成离散递归的形式：
$$R_{b,k+1}^n=R_{b,k}^n\ast \sum _{n=0}^{n=\infty }\frac{({\Omega }_{nb,k}^b\Delta t{)}^n}{n!}\text{\hspace{1em}(1-4)}$$
令$S={\Omega }_{nb,k}^b\Delta t$,则：
$$R_{b,k+1}^n=R_{b,k}^n\ast \sum _{n=0}^{n=\infty }\frac{(S{)}^n}{n!}\text{\hspace{1em}(1-5)}$$
令$\vec{\theta }={\omega }_{nb,k}^b\ast \Delta t=[{\theta }_x,{\theta }_y,{\theta }_z{]}^T$，则：
$$S=\left[\begin{array}{ccc}0& -{\theta }_z& {\theta }_y\\ {\theta }_z& 0& -{\theta }_x\\ -{\theta }_y& {\theta }_x& 0\end{array}\right]\text{\hspace{1em}(1-6)}$$
对于$S$有如下性质：
$$S^3=-|\vec{\theta }{|}^{2}S;S^4=-|\vec{\theta }{|}^2{S}^2;S^5=|\vec{\theta }{|}^{4}S;S^6=|\vec{\theta }{|}^2{S}^2$$
其中：$|\vec{\theta }{|}^2={\theta }_x^2+{\theta }_y^2+{\theta }_z^2$。
把式（1-5）展开：
$$R_{b,k+1}^n=R_{b,k}^n\ast [I+\frac{S}{1!}+\frac{S^2}{2!}+\frac{S^3}{3!}+\frac{S^4}{4!}+\frac{S^5}{5!}+...]\text{\hspace{1em}(1-7)}$$
根据$S$的性质，可以将（1-7）式改写为：
$$R_{b,k+1}^n=R_{b,k}^n\ast [I+S+\frac{S^2}{2!}-\frac{|\vec{\theta }{|}^2}{3!}S-\frac{|\vec{\theta }{|}^2}{4!}{S}^2+\frac{|\vec{\theta }{|}^4}{5!}S+\frac{|\vec{\theta }{|}^4}{6!}{S}^2-...]\text{\hspace{1em}(1-8)}$$
合并同类项：
$$R_{b,k+1}^n=R_{b,k}^n\ast [I+(\frac{1}{2!}-\frac{1}{4!}|\vec{\theta }{|}^2+\frac{1}{6!}|\vec{\theta }{|}^4-...)S^2+(I-\frac{1}{3!}|\vec{\theta }{|}^2+\frac{1}{5!}|\vec{\theta }{|}^4-...)S]\text{\hspace{1em}(1-9)}$$
我们知道，对于正弦和余弦函数有如下性质：
$$\sin \theta =\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\frac{{\theta }^7}{7!}+...$$
$$\frac{\sin \theta }{\theta }=I-\frac{{\theta }^2}{3!}+\frac{{\theta }^4}{5!}-\frac{{\theta }^6}{7!}+...$$
$$\cos \theta =I-\frac{{\theta }^2}{2!}+\frac{{\theta }^4}{4!}-\frac{{\theta }^6}{6!}+...$$
$$\frac{1-\cos \theta }{{\theta }^2}=\frac{1}{2!}-\frac{{\theta }^2}{4!}+\frac{{\theta }^4}{6!}-...$$
根据以上性质，式（9）可以改写为：
$$R_{b,k+1}^n=R_{b,k}^n\ast (I+\frac{\sin |\vec{\theta }|}{|\vec{\theta }|}S+\frac{1-\cos |\vec{\theta }|}{|\vec{\theta }{|}^2}{S}^2)\text{\hspace{1em}(1-10)}$$
公式（1-10）为著名的Rodrigues公式。有了陀螺仪的测量数据和采样时间$\Delta t$，利用上式就可以进行姿态的更新。值得注意的是，当角速度的测量值非常小的时候，公式（1-10）近似为：
$$R_{b,k+1}^n=R_{b,k}^n\ast (I+S)\text{\hspace{1em}(1-10.1)}$$

以上就是姿态矩阵的更新方程的推导。
未完待续。。。。