基于卡尔曼滤波的捷联惯性/卫星组合导航基本原理

1、《惯性导航》（第三版）秦永元编著，科学出版社。个人认为该书是惯性导航领域工作和研究的必备经典之作。该书理论严谨且内容全面，主要包括四个部分：惯性器件原理、惯性稳定平台、平台式惯性导航系统、捷联惯导系统，是一本不可多得的经典理论教材，值得十分深入地学习研究。

2、《先进滤波方法及其在导航中的应用》宁晓琳、宫晓琳、李建利编著，国防工业出版社。该书是北京航空航天大学仪器科学与光电工程学院，精密仪器与导航控制系研究生推荐课程“卡尔曼滤波与组合导航”的教材。该书简明而不失全面性地介绍了卡尔曼滤波系列滤波算法，另外介绍了几大导航技术及卡尔曼滤波在其中的应用：惯性导航初始对准技术、卫星导航技术、天文导航技术、捷联惯性/卫星组合导航技术、惯性/天文/卫星组合导航技术。

3、《卡尔曼滤波与组合导航原理》（第三版）秦永元、张洪钺、汪叔华编著，西北工业大学出版社。该书个人认为是前两本参考书的进阶，以其需要较为完备的惯性导航、理论力学、自动控制理论基础才能较好地理解部分内容。个人认为该书理论严谨，不过难度较大，但亦是导航专业领域的一本十分重要的参考书。

3、适读人群与所需基础

按照本人的创作惯例，为了照顾不同层次的读者朋友的阅读体验，本文将对一些重要的基本概念进行较为详细的解释、推导和说明，熟悉的朋友们可以选择性跳过。

本文适合：包括但不限于精密仪器与机械、测试计量技术与仪器、测控技术与仪器、导航制导与控制、探测制导与控制技术、自动化、控制科学与工程、电子信息工程、航空航天工程专业，或从事飞行器控制与导航领域研究的研究生、本科生及从业人员阅读参考。

为了更好地理解本文内容，可能需要对：理论力学、自动控制原理（状态空间分析）、惯性器件原理、惯性导航原理、线性系统理论、卡尔曼滤波、数值分析、线性代数等有一定了解。

4、声明

为了维护更好的学术风气，在此敬告各位读者朋友：请务必不要在没有声明原作出处的情况下，照抄、挪用、转载本文中的内容，用于包括但不限于学术论文、毕业论文等任何公开的学术场合，违者必究。感谢广大读者朋友们的理解和支持！

如果需要本文答疑或其他合作交流，欢迎在评论区留言或与本人私信；如果本文对您的工作有帮助，感谢您能点赞收藏~

二、绪论

1、捷联惯性导航系统概述

与平台式惯性导航系统相对应，将惯性器件（陀螺仪和加速度计）直接固连在运载体上，采用机载计算机根据惯性器件的输出同步解算运载体的导航参数的惯性导航系统，称之为捷联式惯导系统（Strapdown Inertial Navigation System，SINS）。其中，陀螺仪用于测量运载体的角运动信息（角速度、姿态角度变化等），加速度计（业内简称为加计）用于测量运载体的线运动信息。机载计算机根据惯性器件的测量信息，解算出运载体的位置、速度、姿态、航向等导航信息。

由于现在机载计算机的算力性能已经比较发达，以捷联惯导系统具有提供导航参数全面（位置、速度、姿态）、信号输出频率较高、结构简单、体积小、重量轻、成本低、维护简单、可靠性高的特点，具有比平台式惯性导航系统更加广泛的应用。

捷联式惯性导航系统在具备上述优点之外，具有一些局限性，主要体现在由于捷联解算过程中有较多的积分计算，使得导航参数的误差极容易随时间积累。

2、卫星导航系统概述

卫星导航系统，或者全球卫星导航系统（Global Navigation Satellite System，GNSS），是一种能在地球表面或近地空间的任何地点为全球陆、海、空、天各类军民载体提供全天候、高精度的位置、速度和时间信息的空基无线电导航定位系统。其本质思想是将传统的无线电导航台放置于人造卫星上，采用多卫星、中高轨道、测距体制，通过多颗卫星同时测距，实现对载体的位置和速度等信息的高精度确定。

目前，全球卫星导航系统国际委员会公布的四大卫星导航系统包括：美国的全球定位系统（Global Positioning System，GPS）、中国的“北斗”卫星导航系统（BeiDou(Compass) Navigation Satellite System，BDS）、俄罗斯的“格洛纳斯”系统（GLONASS）和欧洲的“伽利略”系统（Galileo）。

关于卫星导航系统的进一步介绍，读者朋友可以自行获取，以其不是本文的重点，故不再赘述。为了便于理解GNSS在组合导航中的意义，在这里简要概括一下GNSS自身的优点及缺点，其优点是可以全天候、高精度的提供载体位置、速度信息，而缺点在于其信号输出频率比较低，难以输出载体姿态信息，且易受干扰。

3、组合导航系统概述

目前，导航技术主要有惯性导航、卫星导航、天文导航等。各种导航系统各有优缺点，且均有随机误差，任何一种导航系统在单独使用时都难以满足导航性能的要求。组合导航技术应运而生，即将两种或两种以上的导航系统组合起来，对同一导航信息作测量并解算以形成量测量，进而从这些量测量中计算出各导航系统的误差并校正，达到提升导航系统整体性能的目的。

组合导航技术是提高导航系统整体性能的有效途径，具有如下优点。

1、互补、超越。组合导航系统融合了各导航子系统的导航信息，相互取长补短，超越了单个子系统的性能和精度，同时提高了系统的环境适应性。

2、冗余、可靠。同一导航信息可通过多个导航子系统测量，获得冗余的测量信息，增强了系统的冗余度，提高了系统的可靠性。

3、降低成本。组合导航技术在保证导航系统精度的同时，可降低导航子系统对器件的要求，尤其是对惯性器件的要求，从而降低了组合导航系统的成本。

本文拟协同捷联惯性导航系统（SINS）与卫星导航系统（GNSS）进行组合导航。

其中，捷联式惯性导航系统（SINS）是将敏感元件（陀螺仪和加速度计）直接固连在载体上的惯性导航系统，可以不依赖任何外界信息，完全依靠自身的惯性敏感元件进行导航参数测量。因此，其不易受外界环境干扰的影响，无信号丢失等问题，具有很好的隐蔽性，是一种完全自主式、全天候且输出频率高的导航系统。但是其具有固有的缺点，即导航误差随时间而积累，即长期稳定性差。

卫星导航系统（GNSS）每一次导航定位具有独立性，误差不随时间积累，而且定位精度和测速精度较高，但卫星导航系统输出的信息不连续（数据输出频率低）、易受干扰且难以直接输出载体的姿态信息。

综上所述，SINS和GNSS在使用时各有优缺点，具有很强的互补性，拟将二者结合进行组合导航，以实现优势互补，有望显著提高导航系统的综合性能。目前，SINS/GNSS组合导航系统已广泛应用于航空、航天、航海等领域，是一种较为理想的组合导航系统。

概括而言，将SINS和GNSS进行组合导航的目的是利用GNSS的无累积误差的位置和速度信息来修正SINS中陀螺漂移、加速度计偏置和初始失准角等引起的SINS捷联解算结果的误差，从而获得高精度、高输出频率的完整导航信息（包括位置、速度、姿态）。

4、卡尔曼滤波概述

卡尔曼滤波（Kalman Filter，KF）是一种递推的线性最小方差估计，根据系统的状态空间描述（状态方程、量测方程）在时域内设计滤波器，可用于多维随机系统。卡尔曼滤波采用状态方程描述被估计量的动态变化规律，被估计量的动态统计信息由激励白噪声的统计信息和动力学方程确定。由于激励白噪声是平稳过程，动力学方程已知，因此被估计量既可以是平稳的，也可以非平稳。目前，卡尔曼滤波理论作为一种最重要的最优估计理论已经被广泛应用于各种领域。

卡尔曼滤波要求待分析系统为线性系统，即系统的状态空间描述为线性；另外要求系统噪声为高斯白噪声。对于非线性系统，可以采用扩展卡尔曼滤波（EKF）、Unscented卡尔曼滤波（UKF）等方法。

卡尔曼滤波的基本方程如下：

关于卡尔曼滤波的其他内容还有很多，感兴趣的朋友可以自行了解。

5、基于KF的SINS/GNSS组合导航的实现途径

基于卡尔曼滤波的SINS/GNSS进行组合导航，有着不同的实现途径及方法分类。

（1）直接滤波法与间接滤波法

根据状态变量选取的不同，分为直接法滤波和间接法滤波。在运用卡尔曼滤波进行SINS /GNSS组合导航前，首先需要建立系统的状态空间描述，即状态方程和量测方程。在直接法中，直接以各导航系统的导航参数作为状态，利用卡尔曼滤波直接估计载体的导航参数；在间接法中，以各导航系统的导航误差作为状态，以各导航系统输出的导航信息之差作为系统量测量，由卡尔曼滤波得到导航参数误差的估计。

在直接法滤波中，状态变量的量值大、变化快，如加速度、位置和姿态角等，另外在实际中其状态方程往往呈非线性。而间接法滤波采用导航误差作为状态变量，与导航参数本身相比，其量值小得多，且变化缓慢；采用线性状态方程即可较为准确地描述导航误差的传递规律，因此状态变量的估计精度很容易得到保证。

在实际工程应用中，普遍采用间接法滤波。在本文中，亦采用间接法滤波。

（2）输出校正与反馈校正

在滤波器得到状态估计后，按校正导航参数的方法不同，分为输出校正和反馈校正两种方法。在输出校正法中，直接用状态估计值校正SINS的捷联解算输出值以获得更加准确的导航参数；在反馈校正法中，将状态估计值反馈到子系统（SINS/GNSS）的解算流程中，周期地修正子系统导航参数，保证子系统不再产生累积误差。

如果能够准确地建立系统状态方程和量测方程，则输出校正法和反馈校正法估计效果一样。反之，由于输出校正不能修正子系统内部的导航误差，长时间工作时滤波容易发散；而反馈校正由于能够修正子系统内部的导航误差，所以长时间工作时也能保证滤波的稳定性。

在本文中，采用输出校正法。

（3）浅组合和深组合

按照SINS与GNSS组合深度的不同，可以将SINS/GNSS组合导航系统的组合模式分为浅组合（松耦合）、深组合（紧耦合）和超深组合（超紧耦合）三类。

浅组合是一种仅利用GNSS辅助修正SINS的误差，SINS和GNSS可独立工作的组合模式。该组合模式主要特点是工作比较简单，便于工程实现，使导航系统有一定的余度。

深组合是一种SINS和GNSS彼此辅助的组合模式。在该模式中，GNSS可以辅助修正SINS的导航误差，同时SINS也可以辅助修正GNSS的导航误差，从而实现二者导航精度的共同提高。深组合的基本模式为伪距、伪距率的组合。

深组合的具体内容及超深组合，这里不再赘述，感兴趣的读者可以翻阅本文提供的三本参考书。本文采用浅组合实现组合导航，相关方法见后文。

（4）建模方法

对于SINS/GNSS组合导航系统，建立准确的数学模型是进行卡尔曼滤波进行导航解算的基础，数学模型的准确性直接影响状态及参数的估计精度和估计时间。而惯性导航系统的误差模型是SINS/GNSS组合导航系统数学模型的核心，由一组描述系统误差传播特性的微分方程表示。

三、基于KF的SINS/GNSS组合导航原理

1、捷联惯性导航系统的基本原理

（1）初始对准

在惯性导航系统理论中，习惯用四元数描述刚体的转动。

采用四元数进行旋转运动描述的原因是，在一些机载计算机系统中，由于单位时间内cpu或mpu的算力有限，若以角度信息作为姿态解算主体，需要引入大量的三角函数运算。众所周知，计算机系统的三角函数运算以若干阶泰勒展开作为替代，当展开的阶数过多时，容易对cpu或mpu带来负担；当展开的阶数过少时，容易与真实值偏差较大。而采用四元数对旋转运动进行描述时，由于四元数的数学特性，即其运动微分方程的解具有简洁的表达形式，采用毕卡数值解法时，能在避免三角函数的计算的前提下，得到很好的导航参数近似值。后文将对这一基于四元数的姿态解算过程，进行进一步介绍。

（2）地理参数计算

捷联惯导解算过程是一个递推式迭代解算过程。迭代过程中，需要首先对载体当前所处地点的地理参数进行计算，如下。

（3）姿态更新

捷联惯导的姿态更新过程，基于四元数的数学特性。为简便起见，这里不作推导直接给出捷联惯性解算中四元数Q的微分方程。

由于捷联陀螺的输出一般情况下是采样时间间隔内的角增量，为了避免噪声的微分放大，应直接用角增量来确定四元数。由角增量计算四元数常用毕卡求解法，通常分为定时采样增量法和定量采样增量法两种方法。一般采用定时采样增量法，本例亦是如此。

在定时采样增量法中，角增量对应的采样时间间隔是相同的。对于前文中给出的关于四元数Q的齐次线性方程，由线性系统理论，有

（4）速度更新

在捷联惯导解算过程中，载体的速度方程为

（5）位置更新

至此，捷联惯性解算方法已经完全明确。在后续组合导航与卡尔曼滤波过程中，SINS仍然按照如是的方法进行导航参数迭代解算，只不过在滤波过程中，以状态的最优估计值校正SINS的导航参数。

2、卡尔曼滤波与组合导航过程

（1）状态变量选取

（2）状态方程

（3）量测方程

（4）滤波方程

（5）输出校正

（6）参数设置

其中，滤波参数设置根据所采用的SINS/GNSS的惯性器件与输出信号的特性决定。

至此，我们便完成了基于卡尔曼滤波（Kalman Filter）的捷联惯性导航系统（SINS）/卫星导航系统（GNSS）组合导航的全部理论基础。根据上述理论，可以利用MATLAB进行仿真实验验证，也可编写C++实现飞行控制与导航算法。

四、结语

1、关于本领域

惯性导航系统（Inertial Navigation System，INS）及其相关领域，在学术上是力学与现代控制理论的延伸，同时也是理论性、技术性、应用性都非常显著的一个研究领域。正如同其在航空航天工程、军事战略武器等领域的应用之广泛一样，与其相关的学科专业、工作岗位也众多。惯性导航技术的研究意义更是不言而喻，上至国防安全保障下至人民生活质量，皆有重要应用。故惯性导航是一个重要的研究领域，十分值得深入研究。