捷联惯导基本算法

惯性导航技术于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…

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

028捷联惯导更新算法备忘
1.姿态更新对于nnn及bbb系,假定有如下四元数转换关系: 那么: rn(k+1)=Cb(k+1)n(k+1)rb(k+1)=Cn(k)n(k+1)Cn(k)b(k)Cb(k)n(k)Cb ...
基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1doc.doc...
基于 Matlab 的捷联惯导算法设计及仿真1doc 基于 Matlab 的捷联惯导算法设计及仿真1 严恭敏西北工业大学航海学院,西安 (710072) E-mail:yangongmin@163. ...
捷联惯导基础知识解析之五（低成本姿态航向参考系统）
陀螺仪精度0.1°/s:加速度计精度5mg:主要指零偏重复性! 一.简化的惯导算法 1.姿态更新陀螺仪输出直接进行积分,得到角增量: 2.速度更新: 在导航系下,完整的速度微分方程,如下: 其中,在 ...
捷联惯导算法与组合导航原理学习——四元数和姿态阵转换（二）
四元数和姿态阵转换学习资料参考: [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8. Quaternion.h #pragma once #in ...
捷联惯导算法与组合导航原理学习——等效旋转矢量和姿态阵转换（一）
等效旋转矢量和姿态阵转换学习资料参考: [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8. EquRotationVec.h #pragma o ...
捷联惯导基础知识解析之四（粗/精对准和GPS/IMU和GPS/里程计组合导航）
初始对准(粗.精对准)/组合导航一.捷联惯导粗对准目的:寻找.确定参考导航坐标系:结果表现形式:得到姿态矩阵(进而可以求出欧拉角.四元数等) 前提:在导航坐标系(比如:东北天)下的重力矢量.地球旋 ...
捷联惯导总结--初始对准，位置标定，INS姿态更新，GPS/INS组合
惯导及组合导航回顾 2018.09.16 今天和17系的同学一起把惯导的流程捋了一遍,为了加深自己的记忆,这里在前面把心得大致列出来. 我们这里只考虑捷联式惯导及松组合首先拿到惯性传感器(加速度计 ...
捷联惯导算法（二）位置更新算法的理解
前言文中算法公式摘自<捷联惯导算法与组合导航原理>(严恭敏.翁浚编著).<惯性导航>(秦永元编著),其他理解仅代表个人观点.本文是对位置更新算法,按照自己学习的思路整理得 ...
捷联惯导算法（三）姿态角和姿态矩阵
前言文中算法公式摘自<捷联惯导算法与组合导航原理>(严恭敏.翁浚编著).<惯性导航>(秦永元编著),其他理解仅代表个人观点.本文是对姿态角和姿态矩阵之间转化的理解. 一. ...

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