组合导航算法（一）之捷联惯导更新及组合模式

捷联惯导基本算法

惯性导航技术于20世纪50年代最初开始投入使用，可分为物理平台与模拟平台。物理平台就是平台式惯性导航系统（PINS）。模拟平台又称捷联式惯性导航系统（SINS），它以计算机为平台，随着MEMS与微处理器技术的飞速发展也得到了迅速推广，由于SINS结构简单、体积小的特点迅速受到普遍的关注。

1 姿态更新

姿态更新微分方程：

式中，×表示对某一矢量求反对称矩阵，wb表示载体坐标系b系相对于惯性坐标系i系的角速度在b系下的投影，wn角速度矢量的标注方法同上。
由上文介绍的四元数与姿态变换的关系可得：

式中所有矢量数据均使用四元数形式来表示。
wn可由与wig与wgn相加来表示：

上式中，wig与wgn分别由下式表示为：

式中，L表示g系下的纬度，N、W、U表示n系下的北向、西向与天向，v
表示n系下三个轴向上的速度，R分别为沿地球子午圈与卯酉圈的主曲率半径。

2 速度更新

速度更新微分方程为：

式中，a为加速度计的观测值，g为重力加速度在n系下的分量，wgnv为地球表面的载体运动产生的向心加速度；wigv表示载体与地球发生运动而产生的哥氏加速度；即：

普通民用等级的惯性传感器并不能对地球的自转有所反映，因此使用时需忽略地球自转速度与哥氏加速度对系统产生的影响，则对式（2.33）简化，速度解算微分方程改写如下：

3 位置更新

位置更新微分方程为：

式中，C为位置矩阵，一般采用西北天坐标系时，h表示当地海拔高度，经度与纬度L之间关系可用矩阵形式表示如下：

则经纬度可表示为：

组合导航基本技术

1 SINS/GNSS松组合模型

SINS与GNSS的松组合导航，根本上就是惯性器件与GNSS的姿态、速度及位置信息融合处理的一种组合导航方式。在SINS/GNSS松组合中， GNSS的输出用来为SINS的输出数据进行校正，且二者关系相对独立。松组合的基本工作原理如图所示：使用GNSS接收机与惯性元件测得的速度、位置信息分别作差，该差值即为观测值，之后通过组合滤波器得到惯性传感器的误差估计值，并将其反馈给SINS从而实现输出的矫正。

由上述介绍及原理图展示可以看出，松组合模式的导航系统可以对两系统进行互补，大幅度地提高了导航定位的输出精度，并且其组成结构较为简单，实现起来也较为容易。然而松组合模式的输出几乎不存在冗余信息，这就意味着难以及时对系统异常数据进行监测，并且接收机通过自身滤波输出速度及位置信息，与滤波器形成串联关系，这种情况容易影响观测噪声的相关性，即观测噪声不再属于白噪声。如果此类情况发生可能会导致滤波器输出的稳定性下降。

实际使用时

松组合模式的组合导航系统解算步骤如图所示。

√系统开始启动时，SINS输出初始姿态、位置及速度信息。
姿态信息的确定通过九轴传感器的融合互补补偿得到。使用加速度计输出对陀螺仪解算的横滚角与俯仰角进行校正，使用磁力计输出对陀螺仪解算的航向角进行校正，最终输出载体姿态。而速度及位置信息在初始时刻使用GNSS输出信息进行对准，当载体运动速度提高到某规定值后，直接使用GNSS输出的速度、位置信息作为速度及位置的初始值，即：

√将九轴传感器输出时间与GNSS输出时间进行比较，如果九轴传感器输出时间大则初始对准完成，反之，进行循环更新。

√使用当前时刻的九轴传感器输出进行解算，一方面通过滤波器输出姿态信息，另一方面经过滤波器预测，求得状态观测量与状态协方差矩阵。
将当前时刻九轴传感器与GNSS输出的时间信息进行比较，当满足：

√即当两系统输出时间最为接近时，开始进行融合解算，以此时获得的GNSS信号的速度及位置数据作为滤波器的观测量进行滤波解算。
反之则更新数据对下一时刻的SINS进行解算，并与该GNSS输出进行融合。如此循环比较并输出。

√输出当前时刻的速度及位置信息，同时将系统误差状态量反馈给SINS，对下时刻更新的数据进行补偿。通过以上循环算法最终实现导航过程。

未完待续。。。
To be continue…