android 惯导 路径绘制 惯导系统导航方程

惯性导航公式浅推

• 引言
• 惯性导航状态方程

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

引言

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

惯性导航状态方程

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

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

首先给出姿态矩阵对时间的微分方程：
$\dot R_b^n = R_b^n\Omega_{nb}^b \tag{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=\begin {bmatrix} 0 &-\omega_z &\omega_y \\ \omega_z &0 &-\omega_x \\ -\omega_y &\omega_x &0 \end {bmatrix}$
常用坐标系及其相互关系：
$\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 \tag{1-2}$
其中$\omega_{ie}^b$为陀螺仪的原始输出值。注意在低速运动载体中最后一项$\omega_{el}^b$可以忽略；而在低精度陀螺仪中后两项$\omega_{ie}^b$和$\omega_{el}^b$都可以忽略，因其精度不足以感知这两个小量。
下面解微分方程（1-1）:
公式（1-1）相当于如下形式的微分方程：$\dot y = yx$。解这个微分方程：
$\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_ke^{\int xdt}$
因此，姿态矩阵的更新方程为：
$R_{b,k+1}^n = R_{b,k}^n*e^{\int \Omega_{nb,k}^bdt} \tag{1-3}$
上式对$e$指数展开，写成离散递归的形式：
$R_{b,k+1}^n = R_{b,k}^n*\sum_{n=0}^{n=\infty}\frac{(\Omega_{nb,k}^b\Delta t)^n}{n!} \tag{1-4}$
令$S = \Omega_{nb,k}^b\Delta t$,则：
$R_{b,k+1}^n = R_{b,k}^n*\sum_{n=0}^{n=\infty}\frac{(S)^n}{n!}\tag{1-5}$
令$\vec {\theta} = \omega_{nb,k}^b*\Delta t=[\theta_x,\theta_y,\theta_z]^T$，则：
$S = \begin{bmatrix} 0 &-\theta_z &\theta_y \\ \theta_z &0 &-\theta_x \\ -\theta_y &\theta_x &0 \end{bmatrix} \tag{1-6}$
对于$S$有如下性质：
$S^3 = -|\vec{\theta}|^2S;S^4 = -|\vec{\theta}|^2S^2;S^5 = |\vec{\theta}|^4S;S^6 = |\vec{\theta}|^2S^2$
其中：$|\vec{\theta}|^2 = \theta_x^2+\theta_y^2+\theta_z^2$。
把式（1-5）展开：
$R_{b,k+1}^n = R_{b,k}^n*[I+\frac{S}{1!}+\frac{S^2}{2!}+\frac{S^3}{3!}+\frac{S^4}{4!}+\frac{S^5}{5!}+...]\tag{1-7}$
根据$S$的性质，可以将（1-7）式改写为：
$R_{b,k+1}^n = R_{b,k}^n*[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-...]\tag{1-8}$
合并同类项：
$R_{b,k+1}^n = R_{b,k}^n*[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]\tag{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*(I+\frac{\sin|\vec\theta|}{|\vec\theta|}S+\frac{1-\cos|\vec\theta|}{|\vec\theta|^2}S^2)\tag{1-10}$
公式（1-10）为著名的Rodrigues公式。有了陀螺仪的测量数据和采样时间$\Delta t$，利用上式就可以进行姿态的更新。值得注意的是，当角速度的测量值非常小的时候，公式（1-10）近似为：
$R_{b,k+1}^n = R_{b,k}^n*(I+S)\tag{1-10.1}$

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