0 INS/GNSS组合导航概述

        所谓INS/GNSS组合导航，就是利用INS和GNSS两者信息进行融合，从而综合利用两者的优点，实现优势互补，求解实现pose的解算，得到PVA（位置、速度、姿态）的准确结果。
         惯性导航系统INS是一种全自主的导航系统，可以输出超过200Hz的高频信号，并且具有较高的短期测量精度。除了提供位置与速度之外还可以提供姿态信息。但由于算法内部存在积分，惯性传感器的误差会不断累积，使得长期导航误差无限制增长。
       与INS相反，GNSS（全球导航卫星系统），主要有美国GPS 、俄罗斯GLONASS 、欧空局Galileo和中国BDS。GNSS具有良好的长期精度，导航误差大致为几米，设备成本低于100美元。但是，它短期精度与输出频率较低。一个常规的GNSSS接收机通常无法提供姿态信息，除非采用一些额外的硬件或软件。此外，全球导航卫星系统需要依靠至少3颗卫星（4颗）的信号，而卫星信号通常会受到高层建筑、树木、隧道、大气以及多路径效应的干扰。
从上述特点来看，INS与GNSS具有较好的互补特性，将二者集成可以得到比单一导航系统稳定性更好、精度更高的导航方案。INS/GNSS的组合导航系统可以输出高频率的导航参数信息（位置、速度、姿态），并且在长、短期的导航过程中均能具备较高精度。采用基于卡尔曼滤波的最优估计方法，对GNSSS和INS定位导航信息进行融合，可以得到可靠的导航解。GNSS能够防止惯性数据漂移，INS能在GNSS信号中断时提供位置、速度、姿态信息。

特别声明：由于早期的GNSS全球导航卫星系统以美国为主，当前其应用普及程度也是世界主流，导致现有的教科书中泛指的GNSS信号都是直接使用的GPS（全球定位系统），GPS本身不是特指美国，但美国命名的采用这个名字，为表述准确，2016年以后出版物多数已改成GNSS写法。GNSS直译为全球导航卫星系统，亦称为全球卫星导航系统，只是翻译的习惯差异。

下文中所有图表展示的GPS和表达式中出现的GPS并不特指美国GPS，在国内也可能是北斗导航GNSS信号，图表和公式来自转载或文献应用，尊重原始版权，不作修改，准确的写法应该是全部改成GNSS字样。

下文内容全部翻译于

Aboelmagd Noureldin, Tashfeen B. Karamat, Jacques Georgy - Fundamentals of Inertial Navigation, Satellite-based Positioning and Their Integration-Springer (2013)

国内翻译版 《惯性导航、卫星定位及其组合的基本原理 》黄卫权，赵琳 译.

典型的INS/GNSS组合如图所示（以GPS为例）：
 

滤波器可以采用传统的kalman滤波器、其衍生的LKF、EKF，或者其他滤波算法（粒子滤波）。
滤波器通过比较惯导与GNSS的输出，估计出惯导系统中位置，速度和姿态的误差，该误差对惯性输出进行校正。

1 误差反馈

基于状态误差卡尔曼滤波的INS/GNSS组合导航系统，可采用两种误差反馈机制：开环和闭环。

1.1 开环INS/GNSS架构

在开环结构中，位置校正、速度校正和姿态校正均在INS外部完成，其中估计误差在每次迭代时从INS解中减去。在这一过程中，误差与状态校正不会反馈给INS系统。
 

 开环结构的优点：
在kalman滤波出现问题时，还可以通过原始的INS导航系统继续支持导航工作。
但是，由于惯性漂移没有进行补偿，惯导误差随时间不断增大，使得线性假设失效。
因此开环的结构更容易导致kalman滤波器性能下降。

1.2 闭环INS/GNSS架构

在闭环结构中，KF估计的误差通过反馈的形式对INS进行校正。在误差估计反馈后，KF的位置、速度和姿态估计被重置为零。
由下图以看出，kalman滤波估计的加速度计误差与陀螺仪误差在每个周期内都通过反馈以矫正IMU测量，继而作用于系统模型的计算。

2 组合导航的类型

为了最大限度地发挥GNSS和INS的互补特性，提出了各种INS/GNSS集成架构。
可分为：松耦合、紧耦合和深耦合。

2.1 松耦合 的INS/GNSS组合导航

在松耦合的结构中，GNSS与INS均独立工作并各自提供导航参数的结果。为了提高导航精度，通常将GNSS的位置与速度输入到滤波器中，同时，INS的位置、速度、姿态也作为滤波器的输入，滤波器通过比较二者的差值，建立误差模型以估计INS的误差。利用这些误差对惯导结果进行修正，得到速度、位置、姿态的组合导航结果。
松耦合的组合结构易于实现，并且比较稳定。当它为开环时，可以提供三个独立的导航结果(原始INS、原始GNSS和组合结果)，当它为闭环时可以提供两个独立的导航结果（原始GNSS、组合结果）。
一个主要的缺点，当卫星数量低于最低数量时，GNSS会暂时失效。并且GNSS KF的输出是时间相关的，那么KF对于测量噪声不相关的假设就会受到影响，从而影响系统性能。
 

2.2 紧耦合 的INS/GNSS组合导航

在紧耦合的导航系统中，GNSS的伪距以及伪距速率的测量 将与INS预测的相应值 进行做差，并将差值反馈给kalman滤波器，用来估计惯导系统的误差。惯导系统的输出经过误差的校正之后，得到组合导航的解。紧耦合的系统结构如图所示。

 紧耦合的组合方式消除了松耦合方法中因卡尔曼滤波级联而产生的测量关联问题。除此之外，在卫星信号消失的情况下，紧耦合的组合模式依然能够提供GNSS信号更新。
由于紧耦合方法涉及对原始GNSS数据的处理，因此在结构上相对于松耦合来说更加复杂。
另一个局限在于，紧耦合结构无法得到独立的GNSS导航结果。
在INS/GNSS组合导航使用相同硬件条件下，紧耦合的解在精度与鲁棒性上均优于松耦合的结果。
 

2.3 深度耦合的 INS/GNSS组合导航

 深度耦合结构与紧耦合、松耦合主要存在两个区别：
1.GNSS接收机采用了回环校正的结构
2.INS的信息作为GNSS接收机的一个组成部分
因此，在深耦合的结构中，INS与GNSS将不再是独立的系统。
该方案通常需要接入内部GNSS硬件，实现起来比较复杂。
深组合的优点是：这种方式降低了跟踪频率，提高了抗干扰能力。使得导航系统可以在较低的信噪比下工作，并且在卫星信号拒止的情况下GNSS依然能够得到导航解。

3 INS方程的动态误差模型

在INS/GNSS组合导航中，需要一个最优估计器来进行数据融合与估计。根据不同系统的需求，估计器的选择也有所不同。传统上，常用kalman滤波器进行惯性传感器的融合，因此，在后续主要针对这一组合进行研究。
在导航坐标系的选择上，根据第(五)讲的内容，通常选用local-level坐标系（当地坐标系）来实现导航解算。第(六)讲分析了导航初始化误差、惯性传感器误差、算法误差等对惯性导航系统误差的影响。因此，此处要使用误差模型来分析和估计与INS相关的各种误差源，总结误差模型，以便于对组合导航的不同结构进行分析。

在第6讲曾分析过，L系的状态误差矢量方程中，包含了沿大地曲面的误差（纬度误差、精度误差、海拔误差）、沿地球系的速度误差（东向速度误差Ve、北向速度误差Vn、天向速度误差Vu）、以及三个姿态角的误差（俯仰pitch、横滚roll、航向yaw）、同时还包括加速度计的bias和陀螺仪的drift。
因此，完整的误差状态矢量表述为：
 

 

 在L系下）上式中，各部分的误差线性化模型为：

位置误差：
速度误差：

 姿态误差：

加速度误差：

 

 角速度误差：

4 松耦合的INS/GNSS组合导航模型

4.1 系统模型

连续时间卡尔曼滤波的系统模型：

其中，G为噪声分布矢量，包含了与状态矢量相关的方差：

F为状态转换矩阵，包含了INS误差模型的各个部分【也就是之前的线性化模型】，可以总结为以下形式：

状态矢量为 位置、速度、姿态、加速度计、陀螺仪的误差分量：

 因此，INS/GNSS 松耦合组合导航的系统模型 可以写成：

 将上式进行展开，可以较为清楚地看到系统状态是如何通过动态矩阵进行耦合的：

 同理，离散时间的系统模型为：

 

4.2 测量模型

离散的KF测量模型表示为：

 其中，等式右侧第二项代表零均值的测量噪声，协方差为Rk。
由于KF的状态矢量包含INS中的误差，因此对应的测量矢量由INS预测的速度、位置与GNSS测量的位置、速度之差组成：

 注意：如果感觉GNSS的速度不准确，也可以只把位置当作观测值！

Hk是tk时刻的测量矩阵，描述了在无噪声情况下通过状态变量的线性组合得到测量值Zk：

那么，完整的松耦合测量模型可写为:

 

将其展开得到：

 此外，还有两个重要的协方差矩阵Rk、Pk
Rk包含测量状态对角线上的方差，定义为：

 

 预测状态的协方差Pk同样是一个对角矩阵，由对角线上的方差组成：

 其中，每一个对角元素的方差项也是3x3的对角矩阵，分别与位置，速度，姿态，陀螺仪偏差和加速度计偏差有关。

4.3 INS/GNSS松耦合总框图

5 紧耦合的INS/GNSS组合导航模型

 此处讨论紧耦合的组合导航模型。
与第4部分类似，首先讨论从L系下的INS动态误差和测量模型；
接着给出GNSS误差和测量误差模型；
最后通过kalman滤波实现综合系统模型与测量模型。

5.1 系统模型

INS part
连续时间kalman滤波 INS系统模型（与松耦合相同）：

FI：动态协方差矩阵
G：噪声分布矩阵
wI：白噪声

 GNSS part
kalman滤波GNSS系统模型：

状态矢量中包含：GNSS接收机时钟误差和漂移，通过随机游走建模：

动态协方差矩阵为

 噪声分布矢量：

各分量分别为：时钟偏差的白噪声标准差，时钟漂移的白噪声标准差; 因此，完整的系统模型可以表示为：  

 结合INS与GNSS的系统模型

将 INS part 与 GNSS part 的方程代入，得到上式的展开式：

 

 离散时间的系统模型：

 

5.2 测量模型

kalman滤波的测量模型在离散时间上的表达式为：

对于紧耦合的系统来说，可用的观测数据为GNSS的伪距以及伪距速率等测量值，因此，测量矢量即为INS估计值与GNSS测量值之差：

对于M个卫星来说，方程又可写为：

 

 伪距测量值【具体内容在第三章】

GNSS接收机获得第m个卫星的信号，可以通过以下模型来表示：

利用卫星导航信息可以计算出卫星的钟差和电离层误差，对流层误差也可以通过建立适当的模型进行计算。因此，在对GNSS误差进行校正之后，可以将校正后的伪距写成：

等式右侧第三项为各种剩余误差。

第m个卫星到GNSS接收机的实际物理距离为：

 

 则，伪距方程可重写为：

 

 其中：

 校正后的接收机位置定义为：

 其中：

 由于伪距测量方程是非线性的，因此在进行kalman滤波之前需要进行线性化（在xINS附近泰勒展开），对于函数 F(x,y,z)在线性化点（xi,yi.zi）的展开式为：

 将伪距方程在当前最优估计值（xINS, yINS, zINS）处线性化后，得到：

 由INS的输出定义的伪距测量为：

 因此，可以得到二者的差值：

 

 

 

 所以，伪距测量误差可归结为：

 对于M个可被观测的卫星，测量误差方程可表示为：

 

 对于ECEF大地坐标中的位置，需要使用以下关系将其转换成ECEF直角坐标：

 为了在kalman滤波中使用，上述方程组必须通过泰勒级数线性化：

 

 将上式代入到测量误差方程中，

 

得到最终的伪距测量模型：

 

伪距速率测量

 

 卫星和接收器运动产生的多普勒频移是二者相对速度在连线上的投影，与发射频率成正比，与光速成反比

 

 其中：

 根据多普勒测量，伪距速率可以通过下式进行计算：

而实际的伪距速率为：

 建立伪距测量的模型：

令接收机时间漂移： 

 则

得到的测量模型是关于deta（Vx）、deta（Vy）、deta（Vz）的函数，因此需要将上式转化为状态误差的形式。
由INS测量得到的伪距速率为：
在这里插入图片描述
其中，Vx,INS、Vy,INS、Vz,INS是INS在e坐标系下估计的接收机速率。
将（8.66）与（8.67）式做差，得到：

 

 (8.70)中

 将（8.70）写成状态空间的形式：

对于M个可被观测的卫星，伪距速率的测量误差方程可表示为：

 

 速度在L系与e系之间的关系可通过下式表示：

 代入R矩阵，得到：

 因此，伪距速率测量模型最终可写为：

 

5.3 总体测量模型

将5.2提供的伪距误差和伪距速率误差的测量模型结合起来，可以得到整体测量模型为：

 

紧耦合集成实现的框图如图所示：