分类号 V249.32 密 级 公开
学校代号 10462 学 号 ************
硕士学位论文
捷联惯导系统四元数导航
滤波算法研究
学位申请人 ： 导师姓名及职称 ： 专学
业科
名门
称 ： 类 ：
张铎 丁国强 副教授
电气工程 工科 2017.6
论文提交日期 ：
A Dissertation Submitted to
Zhengzhou University of Light Industry for the Academic Degree of Master of Engineering Science (Economics、Arts)
Research on Navigation Algorithm of
Strapdown Inertial Navigation System Based on
Quaternion
Candidate:Zhang D Supervisor:Ding G Q Major: Electrical Engineering
School of
Zhengzhou University of Light Industry
Zhengzhou 450002, P.R.China
June 2017
郑州轻工业学院
摘要
捷联惯性导航系统(Strapdown Inertial Navigation System, SINS)是一种具有高精度、强可靠性的数学平台惯导系统，能够为运载体提供高精度的运动参数信息，而不依赖于任何外部信息，被广泛应用于航空、航天、航海、导弹等诸多领域。由于惯导系统的强非线性和不确定性等因素的存在，如何提高捷联惯性导航系统的导航精度成为国内外研究的热点问题。本文从提高导航系统精度的角度出发，结合四元数模型、高阶矩匹配、Gaussian过程建模和Stirling插值方法，针对捷联惯导系统的姿态误差模型及非线性滤波算法进行了深入系统的研究。
针对四元数模型在导航解算中存在的局限性，利用对偶四元数能够将刚体的平移和转动统一描述的特性，构建了对偶四元数姿态转换模型和乘性对偶四元数误差方程。通过飞行器的姿态估计仿真验证，表明该算法在处理带有乘性误差量的姿态估计系统中的有效性。
针对导航系统模型的强非线性和统计特性传递偏差的问题，利用高阶矩匹配方法精确匹配系统状态参数的预测采样点集及其权值概率分布平均偏态和峰值，提出了高阶矩匹配无迹Kalman滤波算法(High order Moment Matching Unscented Kalman Filter， HoMM-UKF)，通过舰载SINS四元数姿态估计仿真验证，可知该方法具有较高的精度和数值稳定性；接着，针对组合导航系统的非线性计算复杂性的问题，利用Gaussian过程建模方法，构建Gaussian过程非参数化模型，提出了基于Gaussian过程容积Kalman滤波算法(Gaussian Process Cubature Kalman filter, GPs-CKF)和增强型GPs-CKF算法，并对其进行仿真验证，相对于传统CKF算法，增强型GPs-CKF算法和GPs-CKF算法计算效率具有明显优势。
针对Krein空间中的扩展Kalman滤波算法(Extended Kalman Filter, EKF)的线性化面临的Jacobin矩阵计算问题，结合Stirling中心插值方法，提出了基于Stirling插值逼近的Krein空间EKF算法。通过速度匹配算法建立舰载体捷联惯性导航系统的传递对准模型，以主子惯导系统的速度误差量和姿态误差量作为观测数据，利用所提出的算法进行导航滤波，实现了该惯导系统传递对准的快速高精度估计。
关键词：捷联惯性导航；四元数；高阶矩匹配方法；Gaussian过程；Stirling插值
I
Abstract
The strapdown inertial navigation system (SINS) is a mathematical platform inertial navigation system with high precision and high reliability, which can provide high precision motion parameter information without any external information. It is widely used in many fields such as aviation, spaceflight, navigation, missile and so on. Because of the strong nonlinearity and uncertainty of INS, how to improve the navigation accuracy of strapdown inertial navigation system becomes a hot topic in the world. In this paper, from the point of improving the precision of navigation system, this paper makes a deep research on the establishment of the attitude error model and nonlinear filtering algorithm based on quaternion model, high order moment matching, Gauss process and Stirling interpolation method.
Aiming at the limitation of the quaternion model in the navigation solution, the paper constructed the attitude transformation model of dual quaternion and multiplicative error equation of dual quaternion by utilizing its characteristic to combine the rotation calculation and the translation calculation altogether. By comparing the simulation results through Matlab simulation, it is proved that the multiplicative dual quaternion has better effect in the system with multiplicative error.
Aiming at the navigation system’s problem of strong nonlinearity and statistical characteristics of transfer error, the high order moment matching method was utilized to calculate the average skewness value and peak value of the predicted sampling points set, and their weights in the view of the probability distribution, and the high order moment matching unscented Kalman filter (HoMM-UKF) is raised. The simulation results show that the HoMM-UKF algorithm is feasible and accurate in strapdown inertial navigation system. Then, aiming at the problem of nonlinear computational complexity of integrated navigation system, by using Gauss process modeling method, the paper has constructed non-parametric GPs model, and proposed the Gauss-process cubature Kalman filter (GPs-CKF) algorithm and the enhanced GPs-CKF algorithm. The simulation results show that compared with the traditional CKF algorithm, the enhanced GPs-CKF algorithm and GPs-CKF algorithm have obvious
I
advantages on computational efficiency
In the light of the Krein space extended Kalman filter (EKF) of the linearization problem of the Jacobin matrix computational complexity. The Stirling interpolation Krein space EKF algorithm is proposed. Based on the velocity matching transfer alignment model, the velocity and attitude errors of the main inertial navigation system and the sub inertial navigation system are taken as the observation, and the system state and observation model are established. The extended Krein space Kalman optimal filtering algorithm is used to estimate the nonlinear filter, which can realize the fast and high precision calculation of the transfer alignment.
Key words: SINS; Quaternion; High order moment matching; Gaussian process; Stirling interpolation
II
目录
第一章 绪论 ............................................................ 1
1.1课题背景及研究目的 ................................................ 1 1.2捷联惯导系统国内外研究现状 ........................................ 3
1.2.1姿态更新算法研究现状 ......................................... 3 1.2.2初始对准算法研究现状 ......................................... 4 1.3本课题研究的主要内容 .............................................. 5 1.3.1课题研究的主要内容 ........................................... 5 1.3.2课题的主要创新点 ............................................. 6
第二章 捷联惯性导航系统原理 .......................................... 8
2.1捷联惯性导航系统原理 .............................................. 8 2.2常用坐标系及姿态描述方法介绍 ...................................... 9 2.2.1坐标系的定义 ................................................. 9 2.2.2坐标系间的变换 ............................................... 9 2.2.3载体姿态描述方法 ............................................ 10 2.3四元数导航解算方法 ............................................... 11 2.4 对偶四元数导航解算方法 ........................................... 14 2.4.1对偶四元数 .................................................. 14 2.4.2对偶四元数导航算法 .......................................... 15 2.4.3乘性误差对偶四元数 .......................................... 20 2.4.4仿真分析 .................................................... 22 2.5 本章小结 ......................................................... 25
第三章 非线性滤波算法 ................................................ 26
3.1 Bayesian最优滤波理论与Gaussian假设 ............................. 26 3.1.1 基于Bayesian范式的状态估计 ................................ 28 3.1.2 Bayesian估计范式的Gaussian假设 ............................ 29 3.2 无迹Kalman滤波算法 .............................................. 30 3.3 HoMM-UKF滤波算法 ................................................ 32 3.3.1 高阶矩匹配算法 ............................................. 32 3.3.2 高阶UKF估计算法 ........................................... 33 3.3.3 仿真分析 ................................................... 36 3.4 GPs-CKF滤波算法 ................................................. 37
3.4.1 GPs模型 .................................................... 38 3.4.2增强型GPs模型 .............................................. 39 3.4.3 GPs-CKF算法 ................................................ 40
I
3.4.3 仿真分析 ................................................... 42 3.5 本章小结 ......................................................... 44
第四章 传递对准的Krein空间EKF算法 ............................... 46
4.1速度匹配传递对准模型 ............................................. 46 4.2 Stirling插值原理 ................................................ 48 4.3非线性系统Krein空间EKF算法 ..................................... 49 4.4 仿真分析 ......................................................... 52 4.5 结论 .............................................................
第五章 总结与展望 .................................................... 56
总结 ................................................................. 56 展望 ................................................................. 56
致谢 ................................................................... 58 参考文献 ............................................................... 59 附录1 硕士期间发表论文目录 ......................................... 附录2 硕士期间获得科成果 ........................................... 65
II
第一章 绪论
第一章 绪论
1.1课题背景及研究目的
惯性导航系统 (Inertial Navigation System, INS)是一类具有高精度、强可靠性以的导航系统[1]，能够为各种运动体及运载体提供高精度的运动参数信息，而不依赖于任何外部信息[2-3]，特点是短时精度高、可连续提供全部运动参数、可以有效避免外部干扰、稳定性高、可以实现全天候运行而不受气候条件的影响，目前在军用飞机、舰艇、航天工程、导弹及诸多民用领域得到了很多应用[4]。
早期的惯性导航系统采用平台式的结构[5]，由实体的三轴框架作为物理平台，通过机电控制实时地补偿陀螺仪输出角来模拟导航坐标系，这种平台式惯导系统，由于精度要求不高、陀螺仪动态范围较小[6-7]，因而对系统计算量的要求也不大，对于当时的技术水平来说是非常适用的。然而随着计算机性能的提升以及制造工艺的进步，采用物理平台的导航系统体积、重量大，可靠性差的缺陷逐步暴露出来，再者因为平台惯导系统具有造价高昂、不易维护等方面的局限，在大多数领域已逐渐被捷联惯性导航系统所取代[8]。
SINS是在20世纪60年代初期被研制出来的一种新型惯导系统[9]，它不需要物理平台作为框架，而是通过实时的姿态解算建立起导航坐标系，以虚拟的数学平台代替物理平台，省去了复杂的机械结构，可以实现更小的体积，降低生产和维护成本[10]。SINS的惯性测量单元（Inertial Measurement Unit, IMU）由三个加速度传感器与三个角速度传感器组合而成。SINS的工作原理简要解释为：将惯性测量装置固连在载体上，装置内集成的加速度计用来记录载体坐标系内三个轴方向上的加速度，角速度传感器记录载体角运动信息，经过数模转换将传感器接收到的模拟量转换为数字信号送给CPU进行处理，CPU实时计算出载体坐标系与导航坐标系间的转移矩阵，将载体坐标系内的角运动和线运动信息解算到导航坐标系，从而获得载体的俯仰角、横滚角与航向角以及东北天三个方向的速度、加速度与位移[11-13]。
由于捷联惯导系统具有自主性、隐蔽性、抗干扰性等性能优势，因此非常适用于具有较高运动安全性和机动性的领域[14]。惯导系统在最初设计完成以后被优先运用在军用飞机、导弹、舰船、潜艇等武器系统，用于对武器设备的定向定位，航路导航以及姿态稳定等[15]，如图1-1所示为辽宁号航空母舰；此外，惯导系统也常被应用于航天飞行与空间探索，为太空中各种飞船卫星空间站等远离地面控制的设备提供自主定位和姿态稳
1
郑州轻工业学院硕士学位论文
定的功能[16-17]，如图1-2为进行模拟交会对接的神舟飞船；最后，还可以通过使用多套导航系统为大型客机和高铁等民用载体提供冗余和备份[18-19]，保障广大民众的出行安全，如图1-3为搭载惯导系统的国产支线客机。
图1-1 辽宁号航空母舰
图1-2 神舟十一号飞船与天宫二号空间站交会对接示意图
图1-3 搭载捷联惯导系统的国产支线客机
2
第一章 绪论
对于捷联惯性导航技术的研究和发展主要分为两个方面：惯导系统本身硬件技术水平的不断提高和新型导航算法的提出与改进。早期的惯导系统采用机械陀螺与模拟电路为主要实现形式，而随着计算机技术的发展，微型计算机数据处理能力不断提升。与此同时，一些采用先进测量原理的惯性器件不断涌现，从最初的机械式陀螺仪发展到光纤陀螺、激光陀螺以及微机械陀螺，可以根据不同技术要求和应用背景提供多种选择方案。对于新型导航算法方面的理论与实践研究也有了长足的进展，通过对导航系统精确建模，采用高精度的导航解算方法，对误差进行估计和补偿，从而实现提高惯导系统性能和导航精度的目的[20]。
本课题的目的就是对捷联惯性导航系统模型和滤波算法展开研究，对现有的四元数姿态转换方法进行改进，通过设计新型滤波算法，改善SINS算法的计算精度。
1.2捷联惯导系统国内外研究现状
1.2.1姿态更新算法研究现状
SINS系统的核心技术在于初始对准、建立惯导误差模型、姿态更新算法、滤波算法等。本课题重点研究捷联惯导系统四元数姿态更新算法和新型滤波算法。
姿态更新算法是SINS的关键技术之一[21]，对SINS的导航精度有很大影响[22]。四元数的概念在19世纪60年代被爱尔兰籍数学家Hamilton引入到数学计算中，而大约一个世纪后才被应用到姿态解算中[23]。四元数作为姿态解算模型，因为计算量较小、结构简洁以及便于实现高精度计算等优点被大量应用在工程建模领域。但是四元数姿态更新算法在递推计算的过程中往往难以避免会产生不可交换的过程噪声[24]，尤其是当载体处在高动态运动的状态下，对转动矢量直接积分而引入的不可交换误差迅速增大，以至于无法被忽略[25]，因此必须采取措施对误差项进行估计和补偿来减小不可交换误差。1971 年，Bortz提出了利用等效旋转矢量为基础建立姿态更新方程，并根据方向余弦矩阵和旋转矢量之间的解析关系，推导出旋转矢量法对应的微分方程，进而得出旋转矢量的微分等于载体旋转的角速度与非交换误差项之和的结论，从而将旋转的不可交换性所带来的姿态误差分离出来，该方法在理论上为不可交换误差提供了解决的途径，一定程度上弥补了四元数姿态更新算法的缺陷，在此之后学者们的研究目标主要关注于等效旋转矢量如何求解的问题。文献[26]分析了典型圆锥环境的运动形式时的基于四元数姿态更新算法的圆锥误差; 1983年，Miller提出了圆锥运动情形下的三子样等效旋转矢量算法，文献[27]对基于四元数姿态更新算法的四阶一龙格库塔法和Miller提出的三子样旋转矢量
3
郑州轻工业学院硕士学位论文
法进行对比研究，验证了等效旋转矢量法可以一定程度上提升高动态环境下的导航精度。在此之后，J.GLee和J.Yoon又在Miller所提出三子样算法的基础上推导出四子样算法[28]，得出了在同样精度的高频圆锥运动中，四子样圆锥补偿算法比三子样算法所用计算时间更短的结论。Ignagni在此基础上提出理论上在纯圆锥运动条件下实现最优精度和最小计算量的假设[29]，即只有在纯圆锥运动条件下，算法中的每个传感器数据的乘积必定有最简单的形式和最优的系数，并使圆锥补偿误差为最小。1999年，GP.chan在其发表的文章中详细地总结了旋转矢量算法圆锥补偿最优系数的公式化表达，从而可以计算出每个子样的圆锥算法的最优系数[30]。
在外国专家对SINS姿态更新方法所做相关研究的基础上，内地高校和研究机构的专家学者也在该领域进行了不少的跟踪研究，并取得了一些新进展。例如北京理工大学、国防科技大学等高校的专家教授，在姿态更新算法的多子样优化设计与划船效应补偿算法的优化算法等领域做了大量的研究工作。国内专家对于姿态更新算法领域的研究重点着眼于等效旋转矢量的优化设计方面，譬如秦永元在三子样算法的基础上，提出了针对旋转矢量的优化方法[31]。截至目前，我国在姿态更新算法的验证方面仍然局限于以仿真作为主要验证手段，有些参数设置过于理想化，与实际应用还有相当距离。
国内专家学者对惯性导航系统领域的研究与国外同行相比起步比较晚，直到70年代才受到国内学者的重视，在国内工业基础薄弱与国外严密技术封锁的情况之下，系统地开展惯性导航系统的研制工作[32]。后来经过863、973等一系列国家主导项目的扶持以及国内众多学者与科研工作者数几十年如一日的不懈努力，国内在惯性导航技术领域己初步具备了的设计、研发和生产能力，特别是中高精度的平台式惯导系统已经可以自主研制和生产，但是在捷联惯导领域的一些高精度传感器和处理芯片还依赖进口。 1.2.2初始对准算法研究现状
初始对准是影响惯导系统精度的关键技术之一[33]。如何在IMU器件进行导航结算之前对系统的初始姿态、位置等信息进行快速准确地标定，是研究初始对准算法的目的所在。SINS的初始对准过程包括位置、速度及姿态对准，其中位置和速度对准主要通过卫星定位和测速仪器等外部原件，相对而言，姿态对准只能依靠运载体内部的IMU器件获得，而且捷联惯导系统在每次开机之前都要进行姿态对准，并且要求能够快速精确地完成[34]。初始对准按照对准的阶段来划分，可以分为两个阶段：粗对准，通过水平对准和罗经对准对平台进行姿态粗调整，迅速地把系统按照一定的精度范围进行对准，对对准精度要求较低，但是对准速度要快，以便为后续精对准做好准备；精对准，在粗
4
第一章 绪论
对准的基础上开展，用时一般为3~10min，要求精度更高。按照运载体基座的运动状态划分，又可以分为静基座的初始对准与动基座的初始对准[35]。根据对准的信息来源划分，又可以分为自主对准与非自主对准：自主对准只依靠惯导系统提供的信息，而不依赖外部信息，具有自主性强、精度较低的特点；非自主对准则是利用主子惯导系统或组合导航系统等方式，把外部参考坐标系引入组合导航系统，确保对准后的平台始终对准导航坐标系。
美国在20世纪60年代末期，就开展了初始对准的研究，Bar-Itzhack， Goshen-Meskin等人对惯性导航初始对准问题进行了系统深入的研究[37]。早期平台式惯导系统大多是在静基座上采用自主式对准方式[38]，如Lee提出的静基座多位置对准方法，后来随着初始对准技术的不断发展，在80年代，Bar-Itzhack和Porat对于动基座的传递对准中的载体加速度对对准效果的影响做了深入分析。90年代，Goshen-Meskin 和Bar-Itzhack从现代控制理论的角度出发，利用分段定常系统(Piece-wise Constant System, PWCS)代替时变系统，进行可观性研究，为基于动基座初始对准的可观性判定提供了依据。Baziw和Leondes 提出了基于动基座的传递对准技术，其原理是将Kalman 滤波算法应用在基于主子工作方式的惯性导航系统初始对准，利用主惯导（Main Inertial Navigation System, MINS）与子惯导（Sub Inertial Navigation System, SubINS）间的速度及位置差作为量测，利用Kalman算法进行滤波从而实现SINS初始对准。根据Baziw与Leondes 所提出的动基座传递对准技术，Chung与Park两位学者对动基座的多位置传递对准的惯导系统误差模型建立方法与初始对准方法进行了了深入的研究。
国内的专家也对初始对准技术进行了了大量的研究工作[39]。例如北京航空航天大学的教授对一些常用于SINS初始对准的可观性分析的策略做了总结，并由此提出了利用自适应方法开展弹性载体的传递对准算法，可以实现较高的精度，并且能显著地抑制由噪声信息不准确导致的滤波发散。目前，国内对于初始对准的许多最新研究成果已经应用在国产航母舰载机、导弹及车载武器系统的初始对准中，取得了良好的实践效果。
1.3本课题研究的主要内容
1.3.1课题研究的主要内容
本课题以提高捷联惯导系统精度为目的，对捷联惯导系统的姿态更新算法、新型滤波算法与传递对准算法展开深入研究，论文主要内容安排如下：
第一章跟踪SINS领域最新的研究进展，对课题的研究目的和相关背景进行了详细
5
郑州轻工业学院硕士学位论文
阐述，并结合国内外研究成果对SINS姿态更新算法与初始对准算法的发展情况做了总结，分析讨论了SINS的几项关键技术与主要研究方向，最后对文章的研究内容和主要创新点做了介绍。
第二章对惯导系统的基本原理和知识点进行了说明，对常用的几个坐标系的定义以与坐标系间的转换关系进行了介绍；对几种常见的载体姿态描述方法进行介绍，并简要地阐述了这些方法的实现原理；提出了对偶四元数模型与运算法则并推导了对偶四元数导航解算方法的运算框架；然后建立基于对偶四元数的SINS乘性误差模型并做了仿真分析，证明了乘性误差对偶四元数模型的可行性。
第三章对Bayesian最优估计理论的发展历程和推导过程进行了分析和介绍，推导出UT变换的UKF滤波算法；并在此基础上提出了利用高阶矩匹配算法算计算系统状态参数的预测采样点集及其权值的概率分布平均偏态和峰值，达到能够使其精确逼近状态参数最优估计计算目的；接着，利用Gaussian过程建模方法，构建GPs非参数化模型算模型，提出了GPs-CKF算法和增强型GPs-CKF算法，并进行仿真验证。
第四章提出了基于Stirling插值逼近的扩展Krein空间EKF算法，利用Stirling中心插值方法，获得非线性系统函数的一阶近似线性化处理表达式，把非线性系统转移到线性Krein空间估计计算框架中，从而获得一种新型的Krein空间EKF方法。通过速度匹配传递对准导航模型，以主惯导系统和子惯导系统的速度误差和姿态误差作为观测量，建立系统状态和观测模型。利用扩展Krein空间EKF算法开展非线性滤波估计，实现舰载体捷联惯性导航系统传递对准的快速高精度估计。
第五章对全篇的研究做了总结，对尚未完成的问题提出了自己的想法，为下一步的研究和工作奠定基础。 1.3.2课题的主要创新点
本文从提高SINS精度出发，以四元数、对偶四元数、乘性误差对偶四元数为基础建立导航模型，利用新型滤波算法系统状态和量测的实时更新，并对误差量进行估计和补偿，结合主子惯导系统传递对准的应用背景，展开深入研究。论文主要创新点如下：
(1) 本课题应用对偶四元数来统一刻画刚体运动中的位移和转动问题，利用推力速度方程、引力速度方程与位置方程构建算法框架，从而简化算法结构，提高运算效率。在构建对偶四元数姿态转换模型的基础上，以对偶四元数运算法则为基础研究了SINS的误差特性，推导出具有乘性误差的对偶四元数状态模型。
(2) 根据UKF算法在强噪声条件下采样点预测值偏离Gaussian分布的情况，利用
6
第一章 绪论
高阶矩匹配算法逼近状态参数的最优估计值，推导出HoMM-UKF算法。
(3) 利用Gaussian过程建模方法重构非线性系统计算模型，它利用传统Bayesian最优滤波框架下的平方根容积卡尔曼算法的迭代递推过程中的估计数据设计GPs非参数化模型，从而构建了新型的GPs-CKF算法和增强型GPs-CKF算法。
(4) 针对舰载体捷联惯性导航系统快速高精度传递对准需要，设计一种基于速度匹配传递对准模型的扩展Krein空间EKF算法。出于降低非线性系统扩展Krein空间EKF算法中Taylor级数的Jacobin矩阵计算复杂性的实际需要，本文提出了一种基于Stirling多项式插值逼近的线性化方法，获得一种无微分操作的非线性系统Krein空间扩展Kalman最优滤波算法。
7
郑州轻工业学院硕士学位论文
第二章 捷联惯性导航系统原理
本章对SINS的工作原理和基本知识点做了简要介绍，阐述了常用的几种参考坐标系及其相互转换关系；然后对欧拉角、三角函数法、四元数、等效矢量等常用的姿态描述策略进行介绍和说明，并以四元数法为例对SINS姿态转换方法进行了推导运算，以此为基础研究了对偶四元数惯导模型的坐标转换与误差建模方法，并进行了仿真测试，为捷联惯性导航系统滤波算法的建立提供了模型与理论基础。
2.1捷联惯性导航系统原理
V加速度计速度增量变换速度计算 计算姿态解算 计算位置计算L,λ,hΦ,θ ,γ姿态阵解算误 差 补 偿四元数计算等效转动矢量计算+陀螺仪++
图2-1 SINS原理图
SINS系统原理如图 2-1所示，图中b表示载体坐标系，n表示导航坐标系。SINS不需要物理平台，所有传感器与测量元件通过IMU固连在在体内部，对输入的载体角运动与线运动信息，利用载体内部的CPU快速计算出载体坐标系与导航坐标系间的转移矩阵，进一步将载体坐标系内的姿态、速度以及位置等信息解算到导航坐标系[40]。
SINS的实质是通过陀螺仪获得载体相对预惯性系的姿态角速度，再经由坐标系转换获得载体相对于地理系的相对角速度，通过等效转动矢量计算将刚体角运动信息转换为三维坐标系内的矢量表达，经过四元数，姿态阵解算等步骤得到载体姿态信息，将加速度计测算出的速度值解算到地理坐标系，获得载体速度，经过积分获得载体位置信息。实际计算过程中，可以将坐标系的转换视为刚体的旋转，根据刚体旋转规律，后文将对捷联惯导系统常用坐标系及其相互转换公式进行推理。
8
第二章 捷联惯性导航系统原理
2.2常用坐标系及姿态描述方法介绍
2.2.1坐标系的定义
对SINS中用到的坐标系定义如下：
(1) 推力速度坐标系T，和载体坐标系b相平行，从地心指向坐标原点的矢量表示推力速度VT；
(2) 引力速度坐标系G，和地球坐标系e相平行，从地心指向坐标原点的矢量等于引力速度VG；
(3) 位置坐标系U，和地球坐标系e相平行，从地心到坐标原点的矢量等于载体对于地心的位置矢量r；
(4) 惯性坐标系I，原点为地心，不跟随地球自转，t=0时刻与地球坐标系平行； (5) 地球坐标系e，原点为地球中心，z轴指向地球极轴，x轴通过零子午线，y轴指向东经90°方向；
(6) 地理坐标系g，原点为载体质心，x轴沿卯酉圈指向东，y轴沿子午圈方向指向北，z轴竖直向上；
(7) 载体坐标系b，固连在载体上，以载体质心为原点； (8) 导航坐标系n，实际应用中常用来指代地理坐标系。 2.2.2坐标系间的变换
令刚体围绕固定轴心做一次旋转的运动称为基本旋转，如果将空间坐标系类比为刚体，那么两个坐标系间的角位置关系可以通过多次基本旋转来表示，则坐标系间的转换矩阵可以通过刚体的基本旋转矩阵的连乘得到[41]。以图2-2中坐标系为例，将载体在空间内的姿态转换类比为载体坐标系b到导航坐标系n绕俯仰轴、横滚轴以及航向轴经过3次基本旋转变换得到：
绕−Zn轴绕X1轴绕Y2轴→X1Y1Z1→X2Y2Z2→XbYbZb XnYnZn旋转ψ旋转θ旋转ϕ对应的基本旋转矩阵如下：
9
郑州轻工业学院硕士学位论文
C1ncosψ−sinψ0sin，C2ψcosψ0=1010001=，Cb0cosθsinθ20−sinθcosθZ2Zn (Z1)θ
cosϕ0−sinϕ010sinϕ0cosϕZbϕϕOψψY2 (Yb)YnY1θψ
θϕX1 (X2)XnXb图2-2 载体空间姿态变换
将上述的3个矩阵按照坐标系旋转的顺序相乘，得到上述坐标系的姿态转换矩阵为：
cosϕsinθsinϕ−cosθsinϕcosψ−sinψ00sinψcosψ0bb21b1cossinCnC2CCθθ===1Cn1Cn01sinϕ−sinθcosϕcosθcosϕ0 (2-1)
cosϕcosψ+sinϕsinψsinθ−cosϕsinψ+sinϕcosψsinθ−sinϕcosθψθψθθsincoscoscossin=sinϕcosψ−cosϕsinψsinθ−sinϕsinψ−cosϕcosψsinθcosϕcosθ其中，若载体俯仰角θ、横滚角ϕ以及航向角ψ角度很小，可以将高阶的小量忽略，从而得到姿态矩阵可简化形式：
1b=Cnψϕ2.2.3载体姿态描述方法
−ψ1−θ−ϕθ 1(1) 欧拉角法是数学家欧拉于1776年提出的姿态描述方法，它利用一组欧拉角来描述动坐标系相对于参考坐标系间的位置关系。只需要解三个微分方程即可完成欧拉角的求解，但是使用计算机进行数值计算的过程中，由于三角函数的存在，计算量并不小。此外，当载体的俯仰角运动姿态为90°时，会出现奇点，所以无法进行全姿态解算，限
10
第二章 捷联惯性导航系统原理
制了该方法的应用范围。
(2) 三角函数法利用三次刚体旋转来等效两坐标系间的关系，将三次转动的正弦余弦来表示姿态转换函数，该方法能够进行全姿态工作，避免了欧拉角法存在奇点的缺点，但是计算量很大，实用性较差。
(3) 四元数法由英国数学家W.R.Hamilton于1843年提出，直到20世纪60年代才被用于姿态解算。四元数由一个标量及三个相互正交的向量组成，也可以表示为两个三角函数和的形式，具有计算量小、精度高、和全姿态解算的优点，四元数法的缺陷在于，其四个参数必须满足归一化条件，系统必须在每次迭代过程中进行一次归一化处理，否则在姿态解算过程中会出现归一化误差。
(4) 等效旋转矢量的概念由Bortz和Jordan于1971年提出，Bortz提出的转动惯量微分方程给计算捷联惯导系统的姿态矩阵奠定了理论基础。由于旋转矢量的微分等于角速度与非交换误差项之和，因此，在快速变化的动态环境下，需要针对非交换误差项开展圆锥补偿算法。
2.3四元数导航解算方法
将四元数的概念定义如下：
Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k (2-2)
其中，q0、q1、q2、q3都是实数；i、j、k是三维复数空间内正交的单位矢量，其模
值为-1，满足下述条件：
i⊗i=j⊗j=k⊗k=−1；
i⊗j=k，k⊗i=j，j⊗k=i (2-3)
式中，符号⊗表示四元数乘法。另外，还可以将四元数表示成Q=s+v的形式，s是四元数Q的标量分量，v是四元数Q的矢量分量，不妨利用四元数的范数来表示四元数的大小：
222Q=q0+q12+q2+q3 (2-4)
若Q=1，则称Q称为单位四元数或规范化四元数。四元数的运算法则如下：
Q1±Q2=[s1+s2,v1+v2] (2-5)
11
郑州轻工业学院硕士学位论文
λQ=[λs,λv] (2-6)
Q1⊗Q2=Q的共轭复数为：
Q=q0−q1i−q2j−q3k (2-8)
*[s1s2−v1v2,s1v2+s2v1+v1×v2] (2-7)
依据四元数范数定义
[42]
Q*，可以得到Q=Q⊗Q，即Q⊗1，通过矩阵求逆法=Q*则，可以得到Q的逆为：
Q* (2-9) Q=Q−1图2-3利用刚体绕定点转动原理来获得坐标转换矩阵，假定载体坐标系b固定在刚
体上，令刚体以角速度ω=ωxi+ωyj+ωzk相对于参考坐标系R绕定点O旋转。若初始时
刻向量OA=r，经过t时刻后向量的位置变为OA′=r′。由欧拉定理可以得出，从A点到
A′点的角位置变换可以等同于定点绕瞬时轴u转过θ角一次旋转完成。刚体从b系至R系的坐标变换矩阵为：
CbR=I+2Usinθ2cosθ2+2sin2uθ2U⋅U (2-10)
AA'θrr'O 图2-3刚体定点转动示意图
对其展开得到：
12
第二章 捷联惯性导航系统原理
θ−0nsin2θθCbR=I+2cosnsin022−msinθlsinθ22222θ−+mnsin()2θ+2lmsin22θlnsin22lmsin2θmsin2θ−lsin20θ222θmnsin2222θ−(l+m)sin2lnsin2 (2-11)
θ−(l2+n2)sin2mnsin2θ2θ2设q0cos,q1lsin,q2msin,q3nsin，那么可将四元数构造为： ====2222θθQ=q0+q1i+q2j+q3k=cos+uRsin (2-12)
22从(2-12)可以得到，四元数Q中囊括了刚体旋转方向uR和转动角度θ的旋转信息。可以用四元数表示变换矩阵：
221−2(q22(q1q2−q0q3)2(q1q3+q0q2)+q3)22RCb=2(q1q2+q0q3)1−2(q1+q3)2(q2q3−q0q1) (2-13)
222(q1q3−q0q2)2(q2q3+q0q1)1−2(q1+q2)θθθθ另外，由于四元数满足规范化约束：
θθ222因此四元数Q是一个单位四元数。+q12+q2+q3=cos2+(l2+m2+n2)sin2=1，Q=q022又因为：
RuR (2-14) ωRb=θ可得：
RdQθ1R1bωRb⊗Q=Q⊗ωRb= =u⊗Q (2-15)
dt222其中，旋转角速度为ωbRb=ωxωyωz，可将四元数微分方程用矩阵形式表示： −ωx0−ωz−ωyT00qqω11=xq22ωyqωz3ωz0−ωxωy−ωzq0q−ωy1 (2-16) ωxq20q3
13
郑州轻工业学院硕士学位论文
2.4 对偶四元数导航解算方法
2.4.1对偶四元数
对偶四元数由Clifford于1873年提出，最初被用来描述螺旋变换。由于对偶四元数具有同四元数相似的运算性质，同时可以将旋转和平移统一表达，因此非常适合用来描述不同坐标系之间转换。在20世纪90年代，前苏联科学家Branets首先提出把对偶四元数应用到惯性导航领域[43-45]，并且构建了利用对偶四元数来描述SINS中各个坐标系间转换关系的运算框架。2005年，武元新[46]在他的博士论文里引入了推力速度坐标系和引力速度坐标系的概念，将其用于对偶四元数坐标系转换，并且利用Plucker直线来推导空间对偶向量的运动公式，使对偶四元数在捷联惯性导航系统中的应用具备了良好的发展前景。
对偶四元数与对偶数的概念类似[47]，只不过它的每个元素由四元数组成，因此，我们可以将对偶四元数其定义为：
q=q+εq′. (2-17)
式中：q为对偶部，q′为对偶向量；ε满足ε2=0且ε≠0，将对偶四元数的范数定义如下：
q2=qq*=q⋅q=q⋅q+ε(2q⋅q′) (2-18)
****式中：q表示对偶四元数的共轭，有q=q+εq′，下面给出对偶四元数的一些运算法
则：
(1) 对偶四元数加法：
′)q+q=(q1+q2)+ε(q1′+q2 12 (2-19)
(2) 对偶四元数与常数的积：
=λqλq+ε(λq′) (2-20)
(3) 对偶四元数乘法：
′+q1′q2)qq=q1q2+ε(q1q2 12 (2-21)
(4) 对偶四元数点乘：
14
第二章 捷联惯性导航系统原理
′+q1′⋅q2)⋅q=q1⋅q2+ε(q1⋅q2q 12 (2-22)
(5) 对偶四元数叉乘：
′+q1′×q2)×q=q1×q2+ε(q1×q2q 12 (2-23)
(6) 对偶四元数圈乘：
′q1q2=q1⋅q2+q1′⋅q2 (2-24)
对于单位对偶四元数也可以用螺旋向量的形式表示：
ssq=cos(),sin()n22 (2-25) 1/2s=(s⋅s)；n=为螺旋轴。 式中：s为对偶角，大小为ss2.4.2对偶四元数导航算法
对偶四元数导航算法流程如图2-4所示，输入量包含加速度计输出的比力信息、陀螺仪输出的姿态角、地球自转角速度及初始位置的重力加速度，首先通过Bortz方程和
对偶四元数运动学方程计算状态转移四元数Qib更新，再通过四元数乘法运算将载体姿态信息转换到地理坐标系，从而得到载体姿态角信息；接下来通过对偶向量定义推力速度和引力速度积分，计算出速度更新方程在通过状态转移四元数将速度更新从惯性系转换到地理系，从而计算出载体速度；最后，通过计算载体惯性系的转动角速度在地理系
的投影，运用对偶四元数微分方程，和姿态转移四元数计算出Qeg，从而得出方向余弦矩阵Ce，进一步求出载体位置信息[48]。
g该算法在流程上根据传统四元数计算方法做出了一些改进，从而能有效提高算法性能：
(1) 运用对偶四元数建模，在计算量没有显著增加的情况下，利用对偶四元数可以将刚体旋转和平移统一处理的性质，简化了计算流程，提高了系统运行效率；
15
郑州轻工业学院硕士学位论文
bfib推力速度增量∆VTQib∆VTi推力速度更新VTibωib 更新Qibeωie Qie更新引力速度增量*QebQieQib∆VGi∆VGQie*QgbQegQebiVG姿态四元数解算姿态角ϕθggVegQe引力速度更新ViiGCgeCVi+R×ωieVie位置计算eiiVNVEVUVig Qig更新QigQegθ、λ更新G
图2-4 对偶四元数导航算法流程图
(2) 传统四元数建模方法在载体坐标系向导航坐标系转换的过程中，忽略了导航坐标系的旋转，而进行姿态更新，会在载体高速运动时引入较大误差，需要对其进行补偿。而改进后的对偶四元数在惯性系下进行姿态更新，由于惯性系是静止的，因此可以确保更新方程的精确性，同时又不增加计算量；
(3) 在进行速度计算的过程中，需要实时更新重力加速度的g值，传统的算法流程需要知道前一个时间点的位置和速度信息，计算过程复杂，难以满足高动态下的系统运算需要。改进算法通过将推力速度和引力速度分别在推力坐标系和引力坐标系下进行计算，并在惯性系中统一处理的方法，使得重力加速度的更新只与前一时刻的位置有关，消除了将已存在的速度误差进行放大的可能性，保证了计算精度。
令T(k)和i(k)分别表示推力坐标系和惯性坐标系在tk时刻的位置，T(k+1)和i(k+1)分别表示推力坐标系和惯性坐标系在tk+1时刻的位置。令单位对偶四元数qT(h)表示T(k)T(k)pi(k)tt从k时刻到k+1时刻的转换，用QiT(k)表示从到的变换。同理，令(h)表示从i(k)到i(k+1)的转换。根据对偶四元数微分方程的性质，可以得到：
i(k+1)T(k+1)*
rQiT(k+1)rQiT(k+1) (2-26)=式中：r为对偶向量，其上标表示对应的坐标系，又因为：
*rT(k+1)=qT(h)rT(k)qT(h) (2-27)
将上式带入到公式(2-26)中，可以得到：
*i(k+1)t(k)*r=QiT(k+1)[qT(h)rqT(h)]QiT(k+1) (2-28)
16
第二章 捷联惯性导航系统原理
=)i(k+1)，公式 (2-26)又可以表示： 由于惯性系是静止的，因此i(ki(k+1)i(k)t(k)* r=r=QiT(k)rQiT(k) (2-29)
将公式(2-28)与公式(2-29)对比可以得到：
*QiT(k+1)=QiT(k)qT(h) (2-30)
上式又可以写成：
* =QiT(t)QiT(tk)qT(t−tk) (2-31) 公式(2-31)对t求导可得：
*dQiT(t)dqT(t−tk) (2-32) =QiT(tk)dtdtT/2可得： 将公式(2-31)带入对偶四元数运动学方程dQiT(t)/dt=QiT(t)ωiTT*dQiT(t)QiT(tk)qT(t−tk)ωiT (2-33) =dt2将公式(2-32)与公式(2-33)进行对比，简化后得到：
1TqT=qTωiT (2-34)
2T式中：ωiT为推力速度对偶向量部分，可以通过Bortz 公式求得：
1Ts=ωiT+s×ωiT+2 (2-35)1ssinsT[1−]s×s×ωiT2(1−coss)s2T通过上述计算流程可以得到从推力速度坐标系到惯性系的姿态转移对偶四元数QiT，根据推力速度坐标系定义可知，载体坐标系b是与推力坐标系T是等价的，因此有QiT=Qib，由此可以得出载体在惯性坐标系下的姿态角信息，再经过惯性系到地球系和地理系的两次转换，即可得到地理系下的姿态转移四元数：
17
郑州轻工业学院硕士学位论文
** Qgb=QegQieQib (2-36)
式中：
λλϕϕ(cos−sin)(cos+sin)2222cos(ωt)ieλλϕϕ−−(cossin)(cossin)0，Q2222Qie=eg=0λλϕϕ+−(cossin)(cossin)ωtsin()2222ieλλϕϕ++(cossin)(cossin). 2222式中：λ，ϕ分别为经度和纬度的对偶四元数。
根据对偶四元数定义s=s+εs′，用s表示推力速度对偶向量积分，其中
s=∫t+∆ttbωibdt，s′=∫t+∆ttfbdt，通过如下公式可以得到推力速度在T坐标系的增量： ∆=s′VTsinss+(s′⋅s)ss22(1−sins
)s (2-37)
sin(s/2)+2s×s′s将速度增量变换到惯性系，与初始速度求和得到载体惯性系下的推力速度更新方程：
T*V(t+=∆t)V(t)+Qie(t)∆VQie(t) (2-38)
iTiT同样，可以类比公式(2-37)~(2-38)求出载体的引力速度，引力速度增量方程如下：
∆V=s′+2s×s′[Gsinsss+(s′⋅s)ss]2(1−sinss)
(2-39)
sin(s/2)ene=ωie⋅∆t；∆t为速度更新周期；G=Cng+ωie×(ωie×R)为地球坐式中：s′=G⋅∆t；s标系e内的重力加速度。可以得到引力速度的更新方程为：
G*V(t+=∆t)V(t)+Qie(t)∆VQie(t) (2-40)
iGiG根据推力速度和引力速度的更新方程(2-38)和(2-40)，可以得到载体在惯性系的速度：
18
第二章 捷联惯性导航系统原理
iiiV=V+V (2-41)iGT将惯性系内的载体速度转换到地理坐标系内，转换过程如下：
=VegCeg(CieVii+R×ωie) (2-42)
式中：Ceg表示从地球坐标变换到地理坐标的方向余弦矩阵；Cie为惯性坐标变换到地球坐标的方向余弦阵；R为载体的位置向量。
对于载体位置的确定，先计算对偶四元数Qig更新，再将其转换到地理坐标系，即可解算出载体的经纬度。Qig更新方程如下所示：
Qig(tk+1)=Qig(tk)qg(h) (2-43)
式中：qg(h)可以通过对偶四元数运动学方程运算得到：
1gqg=qgωig (2-44)
2g式中：ωig为对应对偶向量，其表达式为：
gVigy−RH+mggVigxωig= (2-45)
RH+ngtanVϕigxR+Hn
式中：Vigg为地理坐标系下的推力速度与引力速度的和，通过Vigg=CegCieVii求解得到，Rm表示子午圈的曲率半径，Rn表示卯酉圈的曲率半径。根据公示(2-43)~(2-45)可以求出Qig更新，再通过坐标系变换输出到地理坐标系，变换方程如下：
*
Qeg=QieQig (2-46)
根据公式(2-46)求出的结果，可以得到方向余弦矩阵Ceg，再由此解算出载体的位置信息。
19
郑州轻工业学院硕士学位论文
2.4.3乘性误差对偶四元数
定义乘性误差对偶四元数表达式为：
ˆ∆q =qq(2-47)
根据加性对偶四元数与乘性对偶四元数的关系可以得到：
ˆ−qˆ∆qˆ(1−∆q) (2-48) δq=q=q
即：
ˆ∗δq∆=−q1q (2-49)
根据对偶四元数微分方程(Multiple Dual Quaternion Equation, MDQE)对上式求导：
ˆ∗δqˆ∗δqq=∆q+qˆqˆ∗δqˆ∗δqω=−1+q2 (2-50) 1ˆˆδqω=−−∆+(1q)q2
的表达式为： 利用摄动法得到加性误差对偶四元数δqδq=
12ˆ+qˆδω(δqω) (2-51)
代入到公式(2-48)，并忽略摄动误差的乘积，可以得到：
=∆q12ˆ∆qˆ+δω(ω−∆) (2-52) qωITITTB=ωIB , 针对T系和I系之间刚体运动关系，由坐标系定义可知ωITq∆=IT12BB1ˆB∆qqωδω(ω−∆+IBITITIBIB) (2-53) 2BBB=+εf为对偶四元数旋量的估计值，ωIBωIB式中：ω和f分别表示陀螺仪和加速度计IBBB=δωIB+εδf表示陀螺仪和加速度计的偏移量。将上式拆分成实部与输出，其误差δωIB对偶部：
IT=∆q12BBBˆIBˆIB(ω∆qIT−∆qITω+δωIB) (2-)
20
第二章 捷联惯性导航系统原理
′∆q=IT12BBˆIB′+1′ (2-55) (ω∆qIT2∆qITωIB+f∆qIT−∆qITf+δf)同理可以得到引力坐标系G到惯性系I的MDQE表达式：
IG∆q =′IG∆q =1212EEEˆIEˆIE(ω∆qIG+∆qIGω+δωIE) (2-56)
EEˆIEˆIE′−∆qIG′ω(ω+g∆qIG−∆qIGg+δg) (2-57) ∆qIGGEEˆ=ωεg+式中：ω，实部与对偶部分别指代地球自转角速度与当地重力加速度。U系IGIE到I系的MDQE表达式为：
1ˆU∆qˆU+δωωqω=∆q(−∆)IUIUIUIUIU2 (2-58)
ˆU可以表示为： 式中：旋量ωIUE∗IIˆU=ωωIE+εqIU+vG(vT)qIU (2-59) IUIIvT与vG分别为推力速度和引力速度，式(2-58)可以拆分为：
IU∆q =12EEEˆIEˆIE∆qIU−∆qIUω(ω+δωIE) (2-60)
′IU=∆q
12∗EEIIˆIEˆIE′−∆qIU′ω(ω)qIU∆qIU+qIU+vG∆qIU(vT∗II)qIU+δm)−∆qIUqIU+vG(vT (2-61)
由式(2-59)可以得到：
**UEIIIIˆ=ˆIUˆIU−qˆIUˆIU∆qIU+ε∆qIUq+vG)q+vG)q(vT(vTδωδωIEIU**′q′ˆIUˆIT∆qITˆITˆIU−2∆qIG−2qqq (2-62)
为简化表达，将δm定义为：
(2-63)
*II*IIˆIUˆIU−qˆIUˆIU∆qIU(vT(vTδm=∆qIUq+vG)q+vG)q**′q′ˆIUˆIT∆qITˆITˆIU−2∆qIG−2qqq
由于地球角速度可以通过天文观测达到很高的精度，可以将地球自转角速度误差忽略不计；出于简化计算的目的，将重力加速度误差忽略[49]；同时I系为惯性系，通常表
qIG=qIU1，此时，式(2-57)和式(2-61)可示为t=0时刻的地球坐标系，因此t(0)时刻=以化简为：
21
郑州轻工业学院硕士学位论文
′IG=∆q′IU=∆q12EEˆIEˆIE′−∆qIG′ω(ω∆qIG+δg) (2-)
12EEˆIEˆIE′−∆qIU′ω(ω+δm) (2-65) ∆qIUT选取误差状态对偶四元数的向量部分x=[∆qIT′T∆qIT′T∆qIG′T]T，对式(2-57)式∆qIU(2-)和式(2-65)构建状态方程：
Fmx+Gmu=x (2-66)
式(2-66)中的F、G可表示为：
Bωˆ×IB[sˆ×]F=000BˆIB×ω00EωIE×0−Π−I30G=0EωIE×， 0I3012000I30000I3 0假设非线性随机系统：
=xkfk(xk−1)+wk−1
，k≥0, x0 (2-67)
=ykgk(xk)+vk
式中：xk∈Rn和yk∈Rm分别表示系统状态向量和观测向量；wk−1∈Rp和vk∈Rm分别表示系统过程噪声和观测噪声；x0为系统初始状态。这里对系统的状态噪声和观测噪声的统计特性不做任何假设，只是将wk−1和vk看作是有界的未知扰动输入量。 2.4.4仿真分析
开展四元数、对偶四元数与乘性误差对偶四元数的导航解算方法的对比仿真实验[50]，仿真的初始参数设置为：初始位置在东经108.909°，北纬34.246°高度距离水平面350m；
=ψ14.35°；陀螺常值漂移为=θ10.00°，横滚角ϕ=−8.00°，航向角初始时刻俯仰角
0.02°/h，随机漂移为0.01°/h；加速度计常值偏移为0.1mg，随机偏差为0.05mg；设置
仿真时间为300s。根据式(2-67)所提供的非线性系统方程展开四元数、对偶四元数及乘性对偶四元数对比仿真实验，对其状态协方差P过程噪声wk−1与量测噪声vk的初始方差阵P0、Q0、R0分别设置如下：
22
第二章 捷联惯性导航系统原理
P0=diag{0.012,0.012,0.012,0.012,52,52,52,52,12,12,12,12,52,52,52,52} Q0=diag{0,0.012,0.012,0.012,0,0.052,0.052,0.052,0,52,0.012,0.012,0.012}R0=diag{0.012,0.012} (2-68)
600550500H/m45040035034.4534.434.3534.334.25L / °34.2108.88108.875108.885108.108.5108.9108.905108.91λ / °
图2-5 载体运行轨迹图
20δ Px / m10002050100150200250300t / sδ Py / m10002050100150200250300t / sδ Pz / m100050100150200250300t / s
图2-6 四元数算位置误差估计曲线
23
郑州轻工业学院硕士学位论文
δ Px / m15105001550100150200250300t / sδ Py / m105001550100150200250300t / sδ Pz / m1050050100150200250300t / s
图2-7 对偶四元数算法位置误差估计曲线
δ Px / m1050 01050 01050100150200250300 50100150200250300 t / sδ Py / mt / sδ Pz / m50 050100150200250300t / s
图2-8 乘性对偶四元数位置误差曲线
00 00 00
图2-9 姿态误差曲线
由上述仿真结果可以看出，飞行器在高动态运行条件下，不同导航算法所体现的位
24
第二章 捷联惯性导航系统原理
置跟踪精度是不同的：图2-5中表示飞行器的运行轨迹图，从图中可以看出，飞行器从初始位置开始分别做了两次攀升和降落最后沿纬度线方向终止；图2-6~2-8分别表示应用四元数、对偶四元数及乘性对偶四元数导航算法在地理坐标系三个方向的位置误差曲线；图2-9为姿态误差曲线，通过模拟飞行器产生的姿态角真实值与导航算法姿态解算得到的姿态角计算值比较得到，以区别最为明显的俯仰角误差为例，由图2-9可以看出从仿真的初始时刻到75s，传统的四元数导航算法计算误差明显更大，最大可以达到70'左右，大致从50s开始，四元数方法的导航误差才开始逐渐回落，直到200s以后误差逐渐与对偶四元数算法相一致；而对偶四元数导航算法误差在50s时刻已经收敛至30'以内，经过一系列小幅震荡，在200s左右已基本实现收敛；与对偶四元数误差曲线相比，乘性对偶四元数误差曲线在前100s差别很微小，而在100s至200s的区间内，明显具有更好的收敛速度和稳定性，因此，从姿态误差方面来看，乘性对偶四元数明显具有更好的性能。
2.5 本章小结
本章内容从捷联惯性导航系统的基本知识点和原理出发，对文中用到的几种坐标系及其相互转换关系进行介绍和说明；然后，针对四元数模型在导航解算中存在的局限性问题，本章利用对偶四元数能够将刚体的平移和转动统一描述的特性，构建了对偶四元数姿态转换模型和乘性对偶四元数误差方程，实现了载体姿态与平移运动的整体化设计，具有计算流程简单高效的特点。通过飞行器的姿态估计仿真验证，表明该算法在处理带有乘性误差量的姿态估计系统中的有效性。
25
郑州轻工业学院硕士学位论文
第三章 非线性滤波算法
本章研究内容以Bayesian最优估计理论为出发点，推导了UT变换的UKF算法的采样策略和算法结构；利用高阶矩匹配方法对UKF递推计算过程进行了理论分析，提出了高阶矩匹配UKF滤波算法；接着，利用Gaussian过程建模方法，构建GPs非参数化模型算模型，提出了GPs-CKF算法和增强型GPs-CKF算法，为该方法在SINS中的应用奠定了基础。
3.1 Bayesian最优滤波理论与Gaussian假设
估计是指将隐藏的量从间接或不准确的量测中提取出来的过程[51]。隐藏的量可能是一个参数，或是一个状态变量。这里的参数往往指的是与时间不相关的物理量，可能是一个标量，一个向量，或一个矩阵。状态量往往指的是一个向量，常用在一个随时间变化的方程里，描述一个系统的动力学特性。估计器可以分为两种：参数估计器和状态估计器。估计的主要目标就是将状态和参数估计误差最小化同时对于不确定量和扰动具有鲁棒特性。噪声和扰动是伴随系统量测过程出现的，由仪器本身或环境因素导致。系统不确定性也称为过程噪声往往由建模过程的误差，近似误差，非线性误差，或系统物理参数的变化引起的。
将一个控制系统的状态和参数从系统局部量测中提取出来的过程称为状态和参数估计。其主要目的是将估计误差最小化同时保持鲁棒性避免噪声集模型不确定性对系统造成的影响。估计技术的发展延续了五个多世纪，囊括了来自各个领域的人们对其做出的大量贡献。Thomas Bayes，作为这个领域最早的贡献者，引入了基于统计推断的Bayesian法则，从而提供了一种Bayesian估计方法的基本公式。Carl Friedrich Gauss则更进一步，研究了带噪声数据的最优估计策略，在1795年发明了最小二乘估计方法，用来处理天文数学中的非线性估计问题。后来，Andrei Markov基于概率和统计学方法引入了马尔科夫过程与马尔科夫链理论[52]，他证明了下一状态的概率分布可以通过包含所有已发生的事件现有的分布计算出来。Andrei Kolmogorov于1933年发表了他的著作《概率论基础》，奠定了现代概率理论的基础。Norbert Wiener引入了一种维纳滤波公式用于信号处理方面的应用[53]。Kolmogorov与Wiener的研究为日后预测、滤波及平滑理论建立了基础。同时Kolmogorov地推导出了离散系统的最优线性预测。他们的研
26
第三章 非线性滤波算法
究成果后来以维纳滤波器(Wiener-Kolmogorov filter ,WF)而闻名，为后来Kalman滤波的出现奠定了基础。
在1960年，Rudolf Kalman先生，在前人工作的基础上，提出了状态空间的概念，设计了一种新的线性滤波和系统预测的方法，这种方法后来被称为Kalman滤波。Kalman滤波被美国宇航局NASA成功地应用在阿波罗飞船的导航系统上，并很快获得广泛应用，成为一种最常用的状态估计策略。Kalman滤波使用了一个线性动态模型及连续的系统量测来提供Gaussian噪声下的最优状态估计。对Kalman算法进行适当地拓展，便可以得出EKF和无迹Kalman滤波，这些扩展使得KF算法可以在非线性系统内进行状态和参数的估计。经过后人对KF算法更加深入的研究，逐步发展出了求积分卡尔曼滤波器（Quadrature Kalman Filter, QKF)、混合卡尔曼滤波器（Mixture Kalman Filter, MKF)及容积卡尔曼滤波器CKF等非线性滤波算法。
在Bayesian状态估计方法中，状态的后验概率密度函数是由已知的先验的概率密度函数和更新的量测值为基础计算出来的。计算过程包含两个主要步骤：预测和更新。在预测阶段，系统模型被用来预测状态值，进而基于系统量测来实现预测优化和更新。根据量测与状态的迭代先后关系细分出三个概念：平滑，滤波和预测。状态滞后于量测信息称为平滑器，用于轨道重构；量测信息与状态同步则称为滤波器，滤波器主要对当前噪声进行处理；量测信息滞后与状态称为预报器，实现对系统未来状态的预测。系统状态估计过程所需要的信息总体上由以下四个途径获得：状态转移模型、量测模型、输入或对输入量的概率描述、系统的先验信息。状态估计策略被大量应用在现代工程领域，包括目标跟踪、控制系统、通信、生物工程及经济系统中。在近几年，该领域相关的理论和实际应用都引起了广泛的关注。
将状态量从不准确的、不确定的或带有噪声的量测中提取出来的过程称为状态估计。其主要目标是最小化估计误差，这个误差称为残差或新息。需要注明的是，由于系统不确定性和噪声的存在（环境、器材、或量测误差造成），量测结果不能完全反应状态变量的值。为了导出随机动态系统的状态估计值，可以利用一阶马尔科夫过程建立系统模型：
xk+1=f(xk,uk,wk) (3-1)
zk+1=h(xk,vk)
(3-2)
其中，xk、uk和zk为状态输入和量测向量。此外，wk和vk分别为第k次的过程噪声和量测噪声。假设f、h和uk 已知，wk、vk为互相的随机Gaussian白噪声。滤波问
27
郑州轻工业学院硕士学位论文
题变成对状态估计量xk的迭代计算。通过一个基于k次迭代的量测zk构造Bayesian范式来实现的。有两种统计学概念来简化状态估计的计算，分别为状态的Bayesian范式和Gaussian分布，将在后文中进行详细阐述。 3.1.1 基于Bayesian范式的状态估计
在状态估计中利用Bayesian范式是为了计算假设的后验概率密度函数p(xk+1|zk+1)，其中zk+1是带噪声的量测向量。为了计算状态的后验概率密度函数，当状态的先验概率密度函数p(xk|zk)已知，对其进行两次递归计算即可得到后验概率密度函数。假设状态的初始概率密度函数为p(x0)=p(x0|z0)滤波过程包含两个阶段，预测和更新。Chapman-Kolmogorov方程可以用来表示系统模型(3-1)的预测阶段[] ，如下：
p(Xk+1|zk)=∫p(xk+1|xk)p(xk|zk)dxk
(3-3)
式中，状态转移概率p(xk+1|xk)由状态方程得到，然后，利用Bayesian法则更新的基础，如下：
p(xk+1|zk+1)=p(zk+1|xk)p(xk+1|zk)
p(zk+1|zk) (3-4)
式中，p(zk+1|zk)是归一化常数，由下式得到：
p(zk+1|zk)=∫p(zk+1|xk+1)p(xk+1|zk)dxk
(3-5)
这个值依赖于从量测方程得到的可能性方程p(zk+1|xk+1)，通过后验概率密度函数，一个理论最优状态估计可以通过最小均方误差计算得到：
ˆkMMSEx+1|k+1∫xk+1p(xk+1|zk+1dxk)
(3-6)
相对的，利用最大后验概率得到：
ˆkMAPx=argmaxp(xk+1|zk+1)|kxk
(3-7)
滤波的主要目的是基于所有已知的信息，得到一个准确的状态后验概率密度函数。式(3-3)~(3-5)提供了一个递归估计的基本方案，它说明在附加噪声和不确定性都是Gaussian分布的情况下，解析地计算线性状态转移阵和量测模型的估计后验概率密度函数是可行的。这种情况下，后验概率密度函数可以通过均值和协方差的形式表达，后验均值和协方差可以递归的进行预测和更新计算。然而，这个方法不适用于非线性系统或是不确定为非Gaussian的系统。
28
第三章 非线性滤波算法
3.1.2 Bayesian估计范式的Gaussian假设
为了简化Bayesian范式方程，采用基于Gaussian分布的白噪声，这种基于Gaussian假设的Bayesian滤波称为Gaussian滤波。根据这种论述，之前Bayesian滤波递归计算变成了代数的一阶矩（均值）和二阶矩（协方差）。这个过程分为时间更新和量测更新：
(1) 时间更新:
利用期望算子计算基于Gaussian分布的状态及状态估计误差的一步预测：
ˆk+1|kE{f(==xk,uk)|zk} x
∫Rnxˆk|k,Pk|k)dxkf(xk,uk)×N(xk;x
(3-8) (3-9)
ˆk+1|k)(xk+1−xˆk+1|k)T|zk}Pk+1|k=E{f(xk+1−x=∫Rnxˆk|k,Pk|k)dxk−xˆk+a|k+Qf(xk,uk)×f(xk,uk)×N(xk;xT
ˆk|k,Pk|k) 为Gaussian分布函数。 其中，N(xk;x(2) 量测更新:
由于先验的量测误差为零均值白噪声，可以近似为Gaussian分布，因此重构滤波概率密度为：
先验的量测为
ˆk+1|k,Pzz,k+1|k)p(zk+1|zk)=N(zk+1;z
(3-10)
ˆk+1|k=z
∫Rnxˆk+1|k,Pk+1|k)dxkh(xk+1,uk+1)×N(xk+1;x
(3-11)
先验的协方差和互协方差分别计算如下：
=Pzz,k+1|k∫RTTˆˆˆh(x,u)h(x,u)×N(x;x,P)dx−zzk+1k+1k+1k+1k+1k+1|kk+1|kkk+1|kk+1|k+Rk+1nx
(3-12)
TTˆˆˆxh(x,u)×N(x;x,P)dx−xz Pxz,k+1|k=k+1k+1|kk+1|kkk+1|kk+1|k∫nxk+1k+1k+1R (3-13)
利用更新后的量测zk+1 计算Gaussian滤波的状态后验概率密度函数如下：
ˆk+1|k,Pk+1|k+1) (3-14) p(xk+1|zk+1)=N(xk+1;x可以得到状态和协方差更新：
ˆk+1|k+1=ˆk+1|k+Kk+1(zk+1−zˆk+1|k)xx
(3-15)
Pk+=Pk+1|k−Kk+1Pzz,k+1|kKkT+1 (3-16) 1|k+1Kk+1=Pxz,k+1|kPzz,k+1|k (3-17)
综上所述，对于线性系统的状态和量测方程在零均值Gaussian白噪声的条件下，可以转换为Kalman滤波。对于非线性系统和非Gaussian分布的噪声，仍有可能获得精确
29
郑州轻工业学院硕士学位论文
的解析方法，可以通过线性化或概率密度估计来解决这些估计问题。EKF是最常见的通过线性化解决非线性递归估计问题的Gaussian方法[55]。UKF和CKF则是基于Kalman的一种延伸[56-59]，他们利用某种变换去估计后验Gaussian分布获得二阶精度的均值和协方差，能够达到三阶以上的估计精度[60-62]。
3.2 无迹Kalman滤波算法
无迹Kalman滤波算法是利用UT变换获得系统状态量的一阶矩和二阶矩，用一组加权采样点去估计系统状态的后验概率密度，从而实现一种非线性系统的Kalman滤波算法[63]。它的性能要明显优于扩展卡尔曼滤波，且不需要计算雅可比矩阵，因而具有精度高计算量低的优点，受到了广泛的应用。 假设非线性系统方程为：
xk=f(xk−1,ωk−1) (3-18) zh(x,v) =kkk式中：xk为n维系统状态变量；wk为过程噪声；zk为系统观测量；vk为观测噪声。
ˆk周围选取2n+1个采样点，采样规则如下： 在先验状态估计xˆ=,χ0xi0=
采样点权值为：
(m)ω=λn+λ,0(c)2ω0=λn+λ+(1−a+β) (3-20) (m)ωi=λ2(n+λ),i=1,,2nˆ+((n+κ)P)i,xχi=ˆ−((n+κ)P)i,χi=x1,,ni=i=n+1,,2n (3-19)
式中：ωi是第i个采样点的加权系数，满足关系∑ωi=1。参数λ可以控制高阶项对状态逼近结果的影响，满足λ=a(n+κ)−n。β是一个大于或等于零的加权因子，用于把状态向量的高阶矩信息包含在Sigma点中，同时也可以控制后验分布尾部强度的误差影响。则一个完整的UKF算法流程为：
(1) 滤波参数初始化：
2i−02nˆ0=E[x0],P0=E[(x0−xˆ0)(x0−xˆ0)T] (3-21) x(2) 对于k∈1,,N，获取Sigma点及其权值系数：
30
第三章 非线性滤波算法
ˆ,xχ0=ˆ+((n+κ)P)i,χi=xˆ−((n+κ)P)i,xχi=i=0i=1,,nω0(m)=λn+λ,i=0
(3-22)
ω0(c)=λn+λ+(1−a2+β),i=0i=n+1,,2nωi(m)=1,,2nλ2(n+λ),i=(3) 线性系统方程时间更新：
χi,k|k−1=f(χix,k−1,χiw,k−1)
(3-23)
获得系统状态量的一步预测：
2nxˆk|k−1=∑ω(m)iχi,k|k−1i=0 (3-24)
获得状态向量的一步预测误差协方差矩阵：
2n
Px,k|k−1∑ω(c)i(χx−1−xˆTi,k|kk|k−1)(χxi,k|k−1−xˆk|k−1)i=0 (3-25)
相应的Sigma点更新值为：
=χk|k−1[xˆk|k−1xˆk|k−1+((n+λ)Px,k|k−1)xˆk|k−1−((n+λ)Px,k|k−1)] (3-26)
(4) 测方程更新：
Ζi,k|k−1=h(χxi,k|k−1,χvi,k|k−1) (3-27)
获得观测向量的一步预测值：
2n
=zˆk|k−1∑ω(m)iΖi,k|k−1i=0 (3-28)
得到观测向量的一步预测误差方差矩阵：
Pz,k|k=−1∑2nω(c)i(Ζi,k|k−1−zˆk|k−1)(Ζi,k|k−1−zˆk|k−1)Ti=0 (3-29)
状态向量与观测向量的交叉协方差阵为：
2n
P(c)xz,k|k=−1∑ωi(Ζi,k|k−1−zˆk|k−1)(Ζi,k|k−1−zˆk|k−1)Ti=0 (3-30)
31
郑州轻工业学院硕士学位论文
3.3 HoMM-UKF滤波算法
3.3.1 高阶矩匹配算法
针对非线性系统函数z=f(x)，若状态向量均值为x，利用Taylor级数在均值附近对其展开：
12D∆xf2! (3-31)
131k +D∆xf++D∆xf+k!3!z=f(x)=f(x)+D∆xf+得到映射向量z期望为：
1(2)fPxx2! (3-32)
1(3)1(4)34+fE(x−x)+fE(x−x)+4! 3!=zE={z}f(x)+E(x−x)表示状态向量x的三阶中心矩，E(x−x)表示状态向量x的四阶中心矩，式中：
34定义它们分别为α和β参数：
=βE(x−x) (3-33) =αE(x−x),
34在UKF算法中引入适当的加权因子ωi，有映射向量逼近估计为：
ˆ=∑ωifχ(i) (3-34) zi=02N()对其进行变换可得到：
ˆz=ωf(χ())∑iii=02N (3-35)
ω0f(x)+ω1fx+αNPxx+ω2fX−βNPxx()()在状态变量x均值附近对f展开得到映射变量估计值：
1(2)ˆ=ω0f(x)+ω1f(x)+f(1)αPxx+αPxxzf2!341(3)1(4)+αPxx+αPxx+ff3!4!()2()() (3-36)
21(2)1+ω2f(x)−f()βPxx+βPxxf2!341(3)1(4)+βPxx+βPxx+ff3!4!()()()整理可得：
32
第三章 非线性滤波算法
ˆ=f(x)+f(1)Pxx(αω1−βω2)+z +1(3)f3!1(2)fPxx(ω1α2+ω2β2)2! (3-37)
1(4)23PxxfPxx(ω1α4+ω2β4)(α3ω1−β3ω2)+4!式(3-33)定义的参数α和β作用在于匹配状态变量x的第三阶和第四阶中心矩，也就是状态变量的概率分布平均偏态值和峰值[]。对比式(3-32)和式(3-38)可以获得参数α和β的其表达式为：
12=±−αφφφ431212 (3-38) 12β=−φ14φ2−3φ12式中：φ1和φ2由状态向量平均偏态值和峰值计算获得：
WjΨj∑∑j1=j1==φ1=,φ2 (3-39) nnnn34N=N=∑l1=∑k1Slk∑l1=∑k1SlknnSlk=式中：
(Pxx)lk表示系统状态向量的方差矩阵Cholesky分解因子矩阵的第l行第k列
(i)ji=Wj元素，这里
∑ω(χi=02N−χj=，Ψj)3∑ω(χii=02N(i)j−χj)4分别表示状态向量的第三阶中心矩和
T第四阶中心矩，很明显所有Sigma点的加权均值及其方差满足如下条件：
∑∑2Ni=02Ni=0 ωiχ=χ0m ( ×10m×1) (3-40)
(i)12ωiχ−χχ−χ((i))((i))T=diag{Pxx Q R} (3-41)
2N1n(i)ωχ∑∑ij−χjj1=i0n=2N1n(i)ωχ∑j1=∑i0ij−χjn=(())431n=∑j1ψj (3-42) n=1n=∑j1Wj (3-43) n=3.3.2 高阶UKF估计算法
若考虑离散非线性动态系统为：
xk=f(xk−1,wk−1) (3-44) z=hx,v()kkk式中：xk∈R表示k时刻系统状态向量；wk是m1维的过程噪声；zk∈R表示k时刻系统量测向量；vk是m2维量测噪声；Qw和Rv分别是过程噪声方差矩阵和量测噪声方差矩阵。
(1) 滤波参数初始化：
33
nm郑州轻工业学院硕士学位论文
ˆ0ˆ0=Exx0,P0=Ex0−x(Tˆx−x (3-45) )(00)T对状态向量初始协方差阵和系统初始的噪声方差阵进行Cholesky分解P0=S0S0,
=SSTQ,0Q,0。
(2) Sigma点及权值系数计算：
=χxˆk−10,k−1=0,m i0 1×1=χxˆk−1+(αNSk−1)1i,k−1=i,i1, (3-46)
0m1×1,nχ2i,k−1=xˆk−1−(βNSk−1)i 0m1×1,i=n+1,,2n 其对应的权值系数为：
ω0=1−∑2Ni=1ωi,i=0=ω1iα=(α+β)N,i1,,n (3-47)ω1i=β(α+β)N,i=n+1,,2n同时系统噪声的Sigma采样点及其权值系数为：
χ (xˆk−1)3i,k−1=NS,i=2n+1,,2n+mQ,k−11i−2nχ xˆk−14i,k−1=−(NS,i=2n+mQ,k−1)1+1,,2n+2m1 i−2n (3-48)
其中权值系数为：
ωi=1(2N),N=n+m1 (3-49)
(3) 系统方程时间更新：
=χi,kk−1f=(χji,k−1),j0,1,2,3,4 (3-50)
获得的Sigma点预测向量，得到状态向量一步预测值：
2Nxˆkk−1=∑ωiχi,kk−1i=0
状态向量一步预测误差方差阵：
34
(3-51)
Q0
第三章 非线性滤波算法
ˆkk−1χi,kk−1−xˆkk−1Pxk,kk−1=∑ωiχi,kk−1−xi=02N()()T
(3-52)
对预测方差矩阵开展Cholesky分解，得到分解后的半正定矩阵：
TPxk,kk−1=Sxk,kk−1Sxk,kk−1 (3-53)
T相应Sigma同时对观测噪声的方差矩阵也开展Cholesky分解操作获得Rk=SR,0SR,0，
χxˆkk−10,kk−1=, i00 m2×1=χxˆkk−1+(αNSx1i,kk−1k,kk−1)i= 0,i1,,nm2×1χxˆkk−1−βNSx,kk−12i,kk−1=(k)i,i=n+1,,2n 0m2×1 (3-)
其对应权值系数取式(3-47)，同样地观测噪声项的Sigma采样点更新为：χ3i,kk−1= xˆ(kk−1),i=2n+1,NS,2n+m2R,k−1i−2nχ xˆkk−14i,kk−1=,i=2n+m2+1,−(NS,2n+2m2 (3-55) R,k−1)i−2n其权值系数取为式(3-49)。
(4) 系统观测方程更新：
=z�i,kk−1h=(χji,kk−1),j0,1,2,3,4
获得观测向量预测值：
2Mzˆkk−1=∑ωi�zi,kk−1i=0 (3-57)
观测向量的一步预测误差方差阵：
2MPz,kk−1=∑ωi(zi,kk−1−zˆkk−1zi,kk−1−zˆkk−1i=0)()T (3-58)
状态与观测量的互协方差阵为：
2MPxz,kk−1=∑ωi(χi,kk−1−xˆTkk−1zi,kk−1−zˆkk−1i=0)() (3-59)
(5) 计算相应的Kalman增益阵：
35
(3-56)
点的更新值为：=
郑州轻工业学院硕士学位论文
1Kk=Pxz,kk−1Pz−,kk−1 (3-60)
状态向量估计值：
ˆkk−1+Kkzk−zˆk�ˆkk−1=xx() (3-61)
相应估计误差方差阵为：
ˆP=−KkPzKkTPxx,kk−1 (3-62)
3.3.3 仿真分析
为了验证HoMM-UKF算法的有效性，与传统UKF算法和CDKF算法开展对比仿真验证。选取式(3-44)作为非线性系统状态空间模型，从而获得如图3-1基于UKF算法的系统姿态角估计误差，图3-2是CDKF模型算法系统姿态角的估计误差，图3-3是HoMM-UKF模型算法得到的系统姿态角的估计误差。从图3-1和图3-3估计误差曲线可以看出，采用高阶矩匹配方法的HoMM-UKF算法，具有更佳的计算效率，横滚角和俯仰角误差都能迅速地收敛并保持稳定，估计精度保持在10＇以内，偏航角误差在小幅的震荡以后也可以快速收敛并保持稳定。UKF算法的横滚角和俯仰角估计精度在10＇左右,偏航角估计精度甚至只有60＇，说明该算法偏航角误差收敛困难并且稳定性差。利用高阶矩匹配方法进行估算算法得到的数据更加精确，稳定性较强，原因在于高阶矩匹配方法能够精确匹配系统状态参数的预测采样点集及其权值概率分布平均偏态和峰值，达到修正采样点集及其权值使其能够精确逼近状态参数最优估计目的。
00
图3-1 系统姿态角的UKF算法估计误差曲线
36
第三章 非线性滤波算法
00
图3-2 系统姿态角的CDKF算法估计误差曲线
00
图3-3 系统姿态角的HoMM-UKF算法估计误差曲线
另外，SINS系统姿态估计模型采用传统CDKF模型算法和HoMM-UKF算法进行计算效能比较[65]，获得如图3-2所示的基于CDKF模型算法系统姿态角的估计误差曲线。对图3-2和图3-3的误差数据进行比较，可以发现HoMM-UKF算法横滚角和俯仰角估计误差不大于20＇，能够快速收敛并保持稳定，而CDKF算法横滚角和俯仰角估计精度在20＇之内,但偏航角估计精度在60＇之内，收敛速度慢并且稳定性差。与CDKF算法相比，HoMM-UKF算法计算精度优于前者，且具有较好的数值计算稳定性，说明HoMM-UKF算法拥有良好的性能及精度优势。
3.4 GPs-CKF滤波算法
GPs是一种非参数化的递归推理函数工具，它无需开展复杂函数的概率分布的数值计算就可以从观测数据中获取系统状态参数的最优估计结果，它可以灵活的建立系统状态参数的模型，能够从被噪声污染的观测数据中获得系统状态参数的不确定性估计。GPs可以看作是系统函数的Gaussian分布，或者更精确地说，GPs描述的随机过程是一个Gaussian过程，其中的系统函数的随机变量，也就是建模函数的输出变量是满足联合
37
郑州轻工业学院硕士学位论文
Gaussian分布的。假设我们已经获得了一组输入输出数据集合，
D={(x1,y1),(x2,y2),,(xn,yn)}，其满足如下的方程：
=yif(xi)+ε
(3-63)
2式中：xi是一个n维输入向量；yi是一组输出向量；ε是满足N(0,s)，那么我们可以
把不确定输入变量X可以表示为，X=[x1,x2,,xn]，把观测输出变量y表示为，
Ty=[y1,y2,,yn]，那么带有噪声的输出变量y的联合概率分布函数满足Gaussian分布
函数：
2p(y)N(0,K(X,X)+snI)=T
(3-)
式中：K(X,X)是核矩阵，其组成元素为Kij=kxi,xj，核函数是输入变量间的一个观
2I项引入Gaussian噪声，起作用类似于系统变量的噪声项ε，主要是基于已测表示，sn()知的观测数据条件化输出量的Gaussian分布，核函数的具体表达式为：
T1−1(3-65) ')s2exp''k(x,x=xxWxx−−−()()f2
式中：矩阵W是一个对角阵。对于给定的不确定输入和观测数据对x,y，s2f是符号方差，若给定一个测试数据x*，那么可以定义一个GPs预测分布，其均值满足：
*2=GPk*T(K(X,X)+snI)yµ(x,X,y)−1
2n−1(3-66)
其方差矩阵满足：
GP∑(x*,X,yk(x,x)−k(K(X,X)+sI))=**∗Tk*
(3-67)
式中：k*表示测试输入数据与观测数据集合间的核向量；GPs参数θ=W,sf,sn可以描述GPs核函数和过程噪声，也可以称之为GPs的超级参数集。这些超参数可以利用输入观测量与训练输出量的极大似然函数法来计算：
θmax=argmaxlog(P(yx,θ))θ{} (3-68)
3.4.1 GPs模型
这里我们考虑离散化非线性系统动态模型方程为：
=xkf(xk−1,uk−1)+εk相应的系统状态变量的观测方程为：
38
(3-69)
第三章 非线性滤波算法
=zkh(xk−1)+δk
(3-70)
式中：xk是系统状态变量；uk是输入控制量；zk是输出观测量；εk和δk是加性的零均值Gaussian白噪声，其方差分别是为Qk和Rk，也就是εk∼N(0,Qk)和δk∼N(0,Rk)。
这里我们应用GPs工具和输入和输出数据对系统过程方程和观测方程的整理与变换，使其能够反映系统状态变量和控制量到系统状态变量估计残差之间的映射关系，假设k时刻的系统状态变量为xk，下一时刻为xk+1，那么系统残差可表示为∆xk=xk+1−xk；这些输入数据和输出数据同时观测模型实现从系统状态变量xk到观测量zk的映射关系，集合可以分别表示为：
Df=(X,U),X'
(3-71) (3-72)
Dh=X,Z
这里X表示前面步骤获得系统状态变量估计值的矩阵，X′表示系统状态相邻两步估计的残差向量，X'=∆[x1,∆x2,,∆xk]，那么我们可以利用GPs工具重构系统过程方程和观测方程为：
(3-73)
=+xkGPµfxuDε,,(k−1k−1)fk
TzkGPµh(xk,Df)+δk=这里系统过程噪声εk满足εk∼N0,GP∑f(xk−1,uk−1),Df，同时δk满足
()δkN(0,GP∑h[xk,Dh])。那么这里的计算目标是利用GPµf和GPµh逼近系统非线性函数方
h程和观测方程f和g的输出均值，利用GP∑f和GP∑逼近系统非线性函数方程中的噪声方
差Q和观测方程中的观测噪声方差R。
3.4.2增强型GPs模型
当然为了改进GPs系统建模效能，利用GPs学习实际系统与逼近系统模型之间的残差，由此获得一种增强型的GPs模型，具体来说就是当输入输出数据对不能完全覆盖整个系统状态变量空间时，利用增强型GPs模型可以获得更好的逼近实际系统的模型，
fˆˆh这里若逼近参数化的过程和观测方程分别为f和，那么残差GPs可以定义为GP和
hGP，这样就可以获得增强型GPs模型表达式为：
fˆ([x,u])+GPˆ(3-74) xk=fµ(xk−1,uk−1),Df+εkk−1k−1
39
郑州轻工业学院硕士学位论文
相应的GPs观测方程为：
hˆ(x)+GPˆzk=hµ(xk),Dh+δkk
(3-75)
其数据训练集合可分别表示为：
ˆ=Df{(X,U),X'−fˆ(X,U)}，ˆ=Dh{ ˆ(X)X,Z−h (3-76)
}fhˆˆ。 这里系统噪声εkN0,GP∑(xk−1,uk−1),Df，观测噪声满足δkN0,GP∑xD,kh()()3.4.3 GPs-CKF算法
下面我们可以利用这些知识改造传统的CKF算法，获得一种新型的GPs-CKF算法。
ˆk−1和二阶矩方差矩阵Pk−1，对第假设第k−1步骤已经获得了系统状态变量一阶矩均值xk−1步的系统状态方差矩阵开展Cholesky分解操作，获得：
Pk−1=Sk−1SkT−1 (3-77)
ˆk−1，i=1,2,,2nx其中： =Sk−1xi+x进而构建k−1步的容积点：χi,k−1nxei i=1,2,,nxxi= (3-78)
i=nx+nx+2,,2nx−nxei 1,构建系统状态变量的预测模型训练数据集合Df,k−1，进而获得系统状态变量的一步预测GPs模型，开展容积点的第k−1步的预测计算，其预测均值计算表达式：
[i][i]uk−1,χki=0,1,,2nx (3-79) χk−1,Df,k−1,,k−1=GPµ()相应GPs模型的噪声方差计算表达式为：
(uk−1,xˆk−1),Df,k−1 (3-80)Qk−1=GP∑进而按照CKF算法计算系统状态变量预测更新值为：
[i][i]ˆk,k−1=∑ωmxχk,k−1i=02nx
(3-81)
[i]其中权值系数=ωm1,i0,1,,2nx；nx表示系统状态变量的维数。以及相应的=2nx系统状态变量的第k−1步预测方差为：
=Pk,k−1[i],T[i][i]Tˆˆ−ωχχxx∑mk,k−1k,k−1k,k−1k,k−1+Qk−1i=02nx
(3-82)
从而完成系统状态变量第k−1步的预测计算任务。下面我们考虑GPs-CKF算法的观测更新计算任务。对系统状态向量的预测方差矩阵开展Cholesky分解获得：
40
第三章 非线性滤波算法
Pk,k−1=Sk,k−1SkT,k−1按照CKF算法计算观测更新容积点：
(3-83)
ˆk,k−1,=Sk,k−1xi+xi0,1,,2nx (3-84) Ζ[k,]k−=1i构建系统状态变量的观测更新模型训练数据集合Dh,k−1，进而获得系统状态变量的一步预测GPs模型，开展容积点的第k−1步的预测计算，其预测均值计算表达式：
ˆ[i]=GPΖ[i],D(3-85)Ζ=in,0,1,,2µk,k−1k,k−1h,k−1x
()相应的系统观测噪声方差为：
ˆk,k−1,Dh,k−1) (3-86)Rk=GP∑(x那么相应的系统观测向量的估计值为：
2nx
ˆk,k−1=∑ωmZk,k−1 zi=0[i]ˆ[i](3-87)
系统观测向量的估计方差计算为：
=Sk,k−1∑ωi=02nx2nx[i][i]mˆZk,k−1Zk,k−1−z[i],TTk,k−1k,k−1ˆz+Rk
(3-88)
那么系统状态变量与量测之间的互协方差矩阵计算：
=Ξxzk,k−1∑ω(χ[i]mi=0[i]k,k−1ˆk,k−1Zk,k−1−zˆk,k−1−x)([i])T
(3-)
从而Kalman增益可以计算为：
xz−1(3-90)Kk=ΞkS ,k−1k,k−1进而可以获得第k步的系统状态变量的估计计算：
ˆk,k−1+Kk(zk−zˆk=ˆk,k−1) (3-91)xx相应的第k步的系统状态矢量的协方差阵为：
=PkPk,k−1−KkSk,k−1KkT (3-92)
ˆk和估计方差矩阵Pk，执行下一步的GPs建保留第k步的系统状态变量的估计值x模、预测和观测更新计算，直至获得系统状态变量的最优估计结果。
这里我们也可以采用增强型GPs模型方法开展增强型GPs-CKF算法的设计工作，做GPs-CKF算法的具体实施步骤中的GPs模型的替换工作。其中第k−1步中的容积点
ˆˆ，那么其容积点更更新GPs模型替换为增强型GPs模型，构建其训练数据集合为Df,k−1新计算为：
41
郑州轻工业学院硕士学位论文
[i]µu,χ[i],Dˆ([x,u])GPˆˆ,i= (3-93) fχk0,1,,2nx=+,k−1k−1k−1k−1k−1f,k−1
相应增强型GPs模型的噪声方差计算表达式为：
ˆ f(3-94)ˆˆQk−1=GP∑(xk−1,uk−1),Dfˆ,k−1
()ˆˆ，把观测更新GPs模型替换为增强型GPs模型，构建系统观测更新数据训练集合Dh,k−1那么增强型GPs观测更新模型为：
µΖ[i],DˆΖ[i]+GPˆˆ,i=ˆ[i]=h0,1,,2nxΖ (3-95) k,k−1k,k−1k,k−1h,k−1
应的系统观测噪声方差修改为：
ˆ h(3-96)ˆk,k−1,Dhˆ,k−1 Rk=GP∑x()ˆh()()其余计算步骤都如GPs-CKF算法，这样我们就获得了一种增强型GPs-CKF算法。 3.4.3 仿真分析
在SINS/CNS组合导航系统的仿真验证中，对非线性系统模型(3-69)和(3-70)进行GPs重构，获得状态方程和观测方程(3-73)，对其展开GPs-CKF算法仿真。假设运动载体做机动运动，其运动轨迹如图3-4所示，运动载体的初始位置为东经124°，北纬26°，运动载体首先做20s的地面加速滑行，然后加速爬高50s，再改平飞600s等动作。设定SINS惯导系统的等效陀螺漂移0.1°/h，等效加速度计零偏为0.001g，陀螺仪的一阶Markov过程相关时间为3600s，加速度计零偏的一阶Markov过程相关时间为1800s，仿真中假设天文敏感器输出时间周期为1s，GPs-CKF滤波器工作初值设置为：水平姿态角误差为3.6＇；航向角误差为18＇；速度误差为0.5m/s；经纬度误差为100m；高度误差为100m。从而我们可以获得如图3-5~3-7所示的系统位置误差的估计结果。
42
第三章 非线性滤波算法
600550500H/m45040035034.3634.3434.3234.334.2834.26L / °34.24108.875108.88108.885108.108.5108.9108.905108.91λ / °
图3-4 SINS/CNS组合系统仿真运行轨迹图
12010080δ Lδ λδ Hδ P / m60402000 50100150200250300t / s
图3-5 CKF算法位置误差曲线
108δ Lδ λδ Hδ P / m20 050100150200250300t / s43
郑州轻工业学院硕士学位论文
图3-6 GPs-CKF算法位置误差曲线
108δ Lδ λδ H δ P / m20 050100150200250300t / s
图3-7 增强型GPs-CKF算法位置误差曲线
为了验证GPs-CKF算法及其增强型算法的性能，与传统CKF算法进行仿真比较[66]，在基于加性四元数建立SINS/CNS组合系统模型方程的基础上，分别开展CKF滤波算法、GPs-CKF滤波算法及增强型GPs-CKF滤波算法，获得系统位置误差曲线。从图3-5、3-6、3-7的位置误差估计曲线可以看出，采用增强型GPs-CKF算法获得的位置误差估计曲线能够在50s内迅速收敛并保持稳定，稳定误差较小；采用GPs-CKF算法获得的位置误差估计曲线收敛速度较慢，在70s左右保持收敛，收敛速度与五阶CKF算法相比稍差；采用CKF算法获得的位置误差估计曲线收敛速度是三者之中最慢，并且稳定性最差，收敛误差较大，在250s左右有发散迹象。从位置误差估计曲线的角度来看，
ˆ及观测方由于增强型GPs-CKF算法计算在迭代过程中加入了逼近参数化的过程方程fˆ，因而得到的数据更加平滑，稳定性较强。 程h3.5 本章小结
本章内容以Bayesian理论为基础，对基于Bayesian范式的状态估计和Gaussian假设进行了详细推导，从而为Kalman滤波算法奠定了理论基础；给出了UT变换的无迹Kalman滤波算法计算流程和算法结构；针对UKF算法采样点预测值偏离Gaussian分布的情况，利用高阶矩匹配方法精确匹配系统状态参数的预测采样点集及其权值概率分布平均偏态和峰值，提出了高阶矩匹配UKF滤波算法，通过舰载SINS四元数姿态估计仿真验证，可知该方法具有较高的精度和数值稳定性；接着，针对组合导航系统的非线性计算复杂性的问题，利用Gaussian过程建模方法，构建GPs非参数化模型，提出了GPs-CKF算法和增强型GPs-CKF算法，并对其进行仿真验证，相对于传统EKF算法及
44
第三章 非线性滤波算法
CKF算法，增强型GPs-CKF算法和GPs-CKF算法计算效能具有明显优势，具有良好的应用价值。
45
郑州轻工业学院硕士学位论文
第四章 传递对准的Krein空间EKF算法
本章所提出的Krein空间鲁棒模型算法是将Kalman滤波计算流程从常用的Hilbert空间中引申出来并应用在不定度规特性的Krein空间中的一种新型鲁棒滤波理论，其系统结构下的噪声方差阵是完全由目标优化问题获得的，其内积不要求正定性，因此这种滤波方法为很多难以满足正定性要求的系统提供了一种设计的可能性，近年来Krein空间估计理论凭借着设计简单、结构灵活、应用范围广等优点逐渐获得了国内外专家学者的重视和推广。
针对一些实际的工程问题对于建模准确度和滤波的精度要求逐渐增高，目前非线性系统的Krein空间扩展Kalman最优滤波算法成为人们研究的热点，人们尝试提出的一些基于信号统计特性来估算系统状态参数的统计特性而开展估计计算的方法，如Bayesian估计、极大似然估计法、UKF算法、CDKF算法以及CKF算法等，还有粒子滤波算法，它是建立在Monte Carlo法基础上的一种非线性最优估计算法，通过状态参数已知统计特性开展一系列采样，再经过数据处理和非线性函数传递实现系统状态参数的估计计算，且可以得到比EKF算法更高的后验均值和方差矩阵，但是利用统计特性设计的滤波器计算负担重，工程实用性不强。
Bucy和Sunahara等学者提出的EKF算法，通过非线性函数的线性化处理，将原非线性问题转移到线性空间设计Krein空间线性估计计算，实现了Kalman滤波算法在非线性系统下的应用，该方法以Taylor级数扩展系统非线性函数，保留低阶线性部分替代原非线性函数，实现标准Kalman滤波计算。但是对于强非线性系统中，由于高阶项进行忽略而致使的高阶截断误差过大，会导致EKF算法估计误差显著增长乃至丧失其非线性特征，由此人们提出了EKF算法的不同改进策略，如迭代Kalman滤波算法，Samer S. Saab改变非线性函数权重值来补偿高阶截断误差等方法。
针对扩展EKF算法的微分计算复杂性，本文提出了一种基于Stirling插值的无微分计算的Krein空间扩展Kalman最优滤波算法[67]，它是利用Stirling中心插值方法，获得非线性系统函数的一阶近似的线性化处理表达式，把非线性系统转移到线性Krein空间估计计算框架中去，从而获得一种新型的Krein空间Kalman最优计算方法。
4.1速度匹配传递对准模型
根据速度匹配建立传递对准模型的方案，主要是利用MINS和SubINS的速度误差
46
第四章 传递对准的Krein空间EKF算法
作为观测量，计算 MINS和SubINS的计算地理坐标系间的失准角修正SubINS的姿态矩阵[68]，速度误差方程为：
mmδVs=fm×ϕm−(2ωie+ωem)×δVsd2DmsdDms+ω×ω)×R0++ω×ω)×Dmsω+(ω|+3×|b+(ωb22 (4-1) dtdt+ω×ω)×δR0+δ∇s+(ω0s=I3×3−Φ，Φ=ϕz设SubINS对MINS的失准角Cm−ϕy−ϕz0ϕxϕyϕx ，
0ssmmsmm 得到：=ϕm=[ϕxϕyϕz]T，=ωisCmωim+ϕϕωis−(I3×3−Φ)ωim则由，进一步变换为：
msmmmss=ϕωis−ωim−Ωimϕ。其中，对于SubINS有ωis=ωin+εs+µs，对于MINS有mmsmωim=ωin+εm+µm，令ωin=ωin，对上式整理得到：
mmm=−Ωimϕ+∆εs+∆µ. (4-2) ϕsm式中：ε为陀螺漂移，有∆εs=εs−εm；µ为陀螺噪声，有∆µµ−µ；
根据上面所推导的主子惯导速度误差及姿态误差方程，建立系统状态方程：
s−(2Ωm+Ωm)ΩmδVsA+A+A+Cs∇bδVieemfdlcb=++W1 (4-3) mmsbm0Cbε−Ωimϕϕmmmmm式中：Ωie和Ωem分别是ωie和ωem的反对称矩阵；Ωm的反对称矩阵； f是f
d2DmsdDms+ω×ω)×Dms为挠曲振动引起的加速度； =+ω×Ad|2|b+(ωb2dtdt+ω×ω)×R0为杆臂加速度误差引起的加速度； Al=(ω+ω×ω)×δR0为标称位置误差引起的加速度；∇b为加速度计零偏；εb为陀螺漂Ac=(ω移；W1为扰动噪声。
在不考虑垂向速度与加速度垂向零偏的情况下，对系统的状态量选取如下：
XδVxs=δVysmmϕxϕyϕzm∇bx∇by∇bzbεxbεyεzb (4-4)
则相应的Kalman滤波器状态方程为：
AX+BW (4-5) =X选择姿态误差与速度误差作为观测量，得到观测方程为：
TT =ZδVδϕ=HX+V (4-6) T
47
郑州轻工业学院硕士学位论文
4.2 Stirling插值原理
这里考虑一个非线性向量函数[69]：
y=f(x) (4-7)
式中：x是具有均值x和协方差Pxx的随机向量。若函数f是解析的，其关于x均值x的Taylor级数展开式可表示为：
y≅f(x+=∆x)f(x)+D∆xf+1213D∆xf+D∆xf+ (4-8) 2!3!ii式中：因子D∆xf表示f(x)的微分，D∆xf可表示为：
Df∆x1∂∂x+∆x2∂∂x++∆xn∂∂xf(x) (4-9)
12nx=xi∆xii式中：D∆xf的一阶和二阶因子表达式可写为：
n∂D∆=fx∆∑p∂xf(x) (4-10) xpp=1x=xnn∂D=f∑∑∆xp∆xq∂f(x) (4-11) ∂∂xxpqp1=q1=x=x2∆x若在函数f(x)的二阶插值逼近中应用类似于Taylor级数逼近的Stirling插值函数：
!f+1D!2f (4-12) y∼f(x)+D∆x∆x2!!和D!2的表达式为： 式中：插值因子D∆x∆xn1!f=∆Dxxµδ()∑pppf(x)∆xhp=1 (4-13) n1 =∑µpδpf(x)∆xphp=12!=D∆xfnn1n22∆+∆∆xδxxδµδµ∑∑pppq(pp)(qq)f(x) (4-14) 2∑h==p1p1=q1式中：h为迭代步长，通常对于一个Gaussian分布，h可设置为h=3；δp和µp分别表示偏差因子和偏差平均因子：
δpf(x)=fx+h2ep−fx−h2ep (4-15) µpf(x=)1fx+hep−fx−hep222()(){()()} (4-16)
48
第四章 传递对准的Krein空间EKF算法
其中e表示在由x扩展生成空间中沿坐标轴单位向量。
考虑到非线性函数的随机向量间的耦合关系，首先对随机向量进行解耦。若假设：
z=Sx−1x (4-17)
!，这里Sx是x协方差矩阵Pxx的Cholesky分解因子。定义一个新函数f则其与函数f之间存在着关系：
!f==f(SxZ)f(x) (4-18)
!的插值函数产生的结果不同，原因其Taylor级数展开式和f函数相同，但是f和f在于：
!(z)=f!(z+he)−f!(z−he)=f(z+s)−f(z−s) (4-19) 2δpµpfpppp!f和D!2f与Df和D2f是这里sx,p表示Sx的第p列向量。可以看到插值因子D∆x∆x∆x∆x不同的。
4.3非线性系统Krein空间EKF算法
考虑非线性随机系统：
=xkfk(xk−1)+wk−1
，k≥0 (4-20)
=ykgk(xk)+vk
其中xk∈Rn和yk∈Rm分别表示系统状态向量和观测向量；wk−1∈Rp和vk∈Rm分别表示系统过程噪声和观测噪声；x0为系统初始状态。不需要再对过程噪声和量测噪声的统计特性进行假设，只要将wk−1和vk看作是有界的未知扰动输入量就可以满足算法要求。
非线性系统Krein空间EKF问题就是利用观测序列集合yjj=0,,k来为状态向量xk的任意线性组合：
{}zk=Lkxk (4-21)
ˆkk，满足性能指标[70]： 寻求一个滤波器zk2∑supx0,wj,vj∈h2j=0ˆjj−Ljxjz+∑wjk2ˆ0x0−x2∏−1j0=j0=+∑vjk2 49 {}郑州轻工业学院硕士学位论文 上述Krein空间扩展Kalman最优滤波问题可以转化为一个不定二次型问题来解决。 ˆ0=0，可以把上式转化为不定二次型： 首先假设x Jk(x0,w0,,wk,y0,yk)=x∏x0+∑vj∗0−1∗0−1kk2=j0=j0+∑wjk2−g2=j0∑kˆjj−Ljxjz∗2=x∏x+∑ww+∑yj−g(xj)yj−g(xj) −g2k∗0jj=j0=j0k∗ (4-24) (4-23) zˆjj−Ljxjzˆjj−Ljxj∑j=0∗k∗jkyj−g(xj)=x∏x0+∑wwj+∑ˆ−j=0zj=0jjLjxj∗0−1I 0yj−g(xj)−2ˆ0 −gIzjj−Ljxj其满足条件： Jk(x0,w0,,wk,y0,yk)>0 (4-25) 然后设计出Krein空间非线性系统模型： =xkf(xk−1)+wk−1 (4-26) ykg(xk)=+vkzˆkkLkxkI 0w,w=Iδ其中ksδ是不定矩阵，wk和vk分别表示k时刻的Kreinks，vk,vs=2ks0 −gI空间状态空间模型的过程噪声和观测噪声。 ˆkk和一步预测xˆkk-1，则对非线性函数f(xk)和假设已知当前时刻状态参数估计xˆkk和一步预测值xˆkk-1处实施Stirling一阶插值逼近 g(xk)分别在估计值x!fxˆkk+Dˆkk+f(xk)=fkx∆xkn =fk()()1ˆˆ)∆x+(x)+h∑µδf(xkkppkkkp=1 (4-27) Γk(xk)=Θk+ (4-28) 1nˆkk，Θk=ˆkk。 ˆkk，其中∆xk=xk−x令Fk=∑µpδpfkxΓkxhp=1()() 50 第四章 传递对准的Krein空间EKF算法 !gxˆkk−1+Dˆkk−1+g(xk)=gkx∆xkn =gk()()1ˆ)∆yˆ+µδxg(x()h∑kk−1ppkkk−1p=1kk−1+ (4-29) 1mˆkk−1，其中∆ykk−1=yk−ykk−1。 令Hk=∑µpδpgkxhp=1()系统非线性函数的其余二阶以上的高阶项定义为： ˆkk−Fkxˆkk=ukfkx=nkkkk−1k () (4-30)ˆ)−Hxˆ g(x (4-31) kk−1它们都可以在线计算获得，在模型数据处理中作为系统噪声或者观测噪声的一部分来考虑。此时的系统模型可以近似为线性模型： =Fkxk+Θkwk+ukxk+1 nkykHk=++xvkkzˆkk0 (4-32)Lk此时应该考虑二次型性能指标为： Jk(x0,w0,,wk,y0,yk)x∏x0+∑w∗=jwj∗0−1k ∗j=0yj−Hjxj−nj+∑ˆ zLx−j=0jjjjkI 0yj−Hjxj−nj−2ˆ zLx−jjjj (4-33)0 −gI从而可以对线性化系统利用Krein空间EKF算法实施非线性系统状态参数的最优估计计算，获得系统状态参数k+1时刻的估计值和估计方差矩阵： ˆk+1k=fkxˆkkx() (4-34) (4-35) (4-36) =Pk+1kFkPkkFk∗+ΘkΘ∗kˆk+=ˆk+1k+Kk+1ek+1xx1k+1Hk+1ˆk+1k+vk+1 (4-37)=ek+1xk+1−xLk+1()∗∗ Hk−1Kk+1=Pk+1k+1 Lk+1Re,k+1 (4-38) 51 郑州轻工业学院硕士学位论文 ∗I+HkPk+1kHk HkPk+1kL∗ k (4-39)Re,k+1=∗∗2 LkPk+1kHk −gI+LkPk+1kLk ∗Pk+=P−KRKk+1e,k+1k+11k+1k+1k (4-40) 其中为了使Riccati方程计算量增加最小，其中L矩阵可选择为单位阵I。 4.4 仿真分析 为了验证该Krein空间EKF算法的性能，对舰船三轴摇摆运动模型仿真舰载机惯导系统进行海上对准研究，仿真条件设置如下：舰船俯仰角、横滚角和航向角摇摆幅度分别为5°、4°和7°；摇摆频率分别是0.05Hz、0.1Hz和0.05Hz；初始角分别设为0°、0°和90°。在给定仿真环境和条件下对系统模型进行仿真研究，为了验证传递对准系统基于Stirling插值的Krein空间EKF算法的有效性及其计算精度优势，根据文献[71]，针对系统计算失准角与速度误差模型开展基于Stirling插值EKF算法与传统Krein空间EKF算法的对比仿真实验，获得SINS系统失准角与速度估计误差数据，如图4-1和4-2所示分别为两种算法载体俯仰角、横滚角和航向角姿态误差数据，图4-3和图4-4分别表示两种算法的东向和北向速度估计误差曲线。 4 φE / ′20 04100200300400500φEt / s600 φN / ′20 0100200300400500φN600 t / s400φU / ′ 2000 0100200300400500 φU600t / s 图4-1 Stirling插值Krein空间EKF算法失准角估计误差曲线 52 第四章 传递对准的Krein空间EKF算法 5 / 0E -50100200300t / s5 400500600 / 0 -50100200300400500600Nt / s1000 / 0U-10000100200300t / s400500600 图4-2 Krein空间EKF算法失准角估计误差曲线 0.20.180.160.14 δ V E δ V Nδ V / m/s0.120.10.080.060.040.020 0100200300400500600t / s 图4-3 Stirling插值Krein空间EKF算法速度误差估计曲线 10987x 10-3 δ VEδ VNδ V / m/s6321 0100200300400500600t / s 53 郑州轻工业学院硕士学位论文 图4-4 Krein空间EKF算法速度误差估计曲线 表5-1基于Stirling插值的Krein空间EKF算法的姿态失准角估计误差均值和方差 估计变量 失准角估计误差(º) 速度误差(m/s) 均值 -0.086 0.033 0.008 0.013 0.025 方差 0.0149 0.0278 0.0075 0.00 0.00 δφx δφy δφz δVx δVy 表5-2传统Krein空间EKF算法的姿态失准角估计误差均值和方差 估计变量 失准角估计误 差(º) 速度误差 (m/s) 均值 -0.093 0.037 0.009 0.016 0.023 方差 0.0174 0.0286 0.0095 0.0102 0.0034 δφx δφy δφz δVx δVy 从仿真结果可以看出在图4-1 Stirling插值Krein空间EKF算法失准角估计误差曲线图中，航向角和横滚角在30s就可以收敛到2′以内，俯仰角则在100s处实现收敛，收敛速度较快，系统稳定性好；图4-2是采用传统Krein空间EKF算法的失准角误差曲线，该算法收敛速度较慢，且收敛后的稳态误差要大；对比图4-3和图4-4，以及表5-1和表5-2，可以看出，基于Stirling插值的Krein空间EKF算法的初始对准速度估计误差曲线不仅收敛速度快，而且收敛后能够保持稳定，误差极小，优于基于传统Krein空间EKF算法的速度估计误差曲线。说明应用Stirling中心插值的方法在计算精度上对结果不仅没有影响，反而有所提升，这是因为Stirling插值逼近的计算效率比传统的一阶Taylor级数的计算效率更高，说明该算法能够满足系统对准实时性要求。 4.5 结论 本章以主子惯导速度误差及姿态误差的运动方程为基础，建立基于速度匹配的传递对准非线性系统模型；在非线性滤波算法方面，针对Krein空间中的EKF算法的线性化过程面临的Jacobin矩阵计算问题，结合Stirling中心插值方法，提出了基于Stirling插值逼近的Krein空间EKF算法；通过速度匹配算法建立舰载体捷联惯性导航系统的传递对准模型，以主子惯导系统的速度误差量和姿态误差量作为观测数据，利用所提出的算法对其进行导航滤波，仿真结果表明，算法计精度较高，计算稳定性较好，能够满足系 第四章 传递对准的Krein空间EKF算法 统对准实时性要求，实现了该惯导系统传递对准的快速高精度估计。 55 郑州轻工业学院硕士学位论文 第五章 总结与展望 总结 本文以捷联惯性导航系统为研究对象，通过研究捷联惯导系统的四元数模型、导航系统模型的强非线性、统计特性传递偏差以及计算复杂性问题和Krein空间中的导航滤波算法，达到提高导航系统精度的目的。主要工作和结论如下： 1. 构建了对偶四元数姿态转换模型和乘性对偶四元数误差方程。通过对常用的姿态更新策略优缺点进行比较，针对四元数模型在导航解算中存在的局限性问题，利用对偶四元数能够将刚体的平移和转动统一描述的特性，构建了对偶四元数导航解算方法。通过构建对偶四元数姿态转换模型和分析SINS的误差特性，推导出具有乘性误差的对偶四元数状态模型。通过飞行器的姿态估计仿真验证，表明该导航算法在处理带有乘性误差量的姿态估计系统中的有效性。 2. 对基于确定性采样策略的非线性滤波算法做出了改进：（1）提出了HoMM-UKF算法。该方法利用Taylor级数对状态参数和预测采样点的分布情况进行匹配，有效地改善了UKF算法采样点预测值偏离Gaussian分布的问题。通过舰载SINS四元数姿态估计仿真验证，表明该算法能够显著提高导航精度。（2）提出了GPs-CKF算法和增强型GPs-CKF算法。该方法通过构建GPs非参数化导航系统模型，简化了组合导航系统中CKF导航滤波算法的计算复杂性。SINS/CNS组合导航系统的仿真验证表明，相对于传统CKF算法，增强型GPs-CKF算法和GPs-CKF算法在同等精度条件下，能明显减少计算量、提高导航系统计算效率。 3. 提出了基于Stirling插值逼近的Krein空间EKF算法。该方法基于Stirling中心插值方法和Krein空间中的EKF算法，避免了导航滤波算法的线性化过程面临的计算Jacobin矩阵的问题。基于速度匹配方法，构建了舰载体捷联惯性导航系统的传递对准模型。仿真结果表明，本文提出的导航算法具有较高的导航精度，较好的计算稳定性，能够满足系统对准实时性要求。 展望 通过对以上研究内容的总结与分析，结合自己对SINS导航算法的理解，提出了一些新的想法： 1.对偶四元数能够实现刚体的平移与旋转同时处理，但是在算法实现的过程中，依 56 第五章 总结与展望 然将对偶部与实部拆分，进行分步处理，下一步会尝试使用并行处理的方法计算对偶四元数更新算法，进一步提高运算效率。 2. 对基于Stirling插值逼近的鲁棒Krein 空间EKF算法进一步改进和优化，加入GPS卫星导航来修正观测数据，使其更好的运用到传递对准的实践中去。 57 郑州轻工业学院硕士学位论文 致谢 值此论文完成之际，谨向我的导师致以最衷心的感谢！本论文从选题、开题到论文的撰写、修改、定稿，每一步都包含了导师的大量心血。在硕士学习期间，老师深厚的理论素养和丰富的实践经验开拓了我的视野，他对国际前沿理论的深入研究给我的课题研究做出了方向性指导；老师严谨的治学态度和认真的工作作风为我提供了良好的学习榜样；在学习、生活等方面更是给予了我大力支持和无私帮助；对我下一步的研究学习提供了指导意见。借此机会，向老师表示衷心的感谢！ 在论文撰写期间，娄老师对我的文章结构编排与逻辑优化方面提供了很多帮助，并为我下一个阶段攻读博士学位提供了很多帮助，在此向娄老师致以衷心的感谢！ 在三年研究生学习期间，杨院长、吴老师以及辅导员在生活和学习上都给予过我巨大的帮助，在此向他们表示衷心的感谢！ 在实验室工作学习，210教研室的师兄、师弟等同学对我论文中的研究工作给予了热情的帮助；学校生活期间，我的室友在生活上给予了我的照顾。在此之际，向他们表示我诚挚的谢意。 感谢撰写这篇论文所涉及到的众多学者，文中引用了数位学者的研究文献，正是各位学者的研究成果对我的启发，我才能顺利完成本篇论文的写作。 感谢我的父母、伴侣对我多年的关心和照顾，在背后默默地付出，对我的学习与研究工作始终给予坚定不移的支持，使我不敢有任何懈怠，唯有更加的努力进取、创造佳绩来回馈他们，在此致以衷心的感谢！ 由于本人学术水平有限，所写论文难免有不足之处，恳请各位老师和学者批评指正！ 58 参考文献 参考文献 [1] 卞鸿巍.漫话导航[J].海陆空天惯性世界,2001,(1):36-38. [2] 范利涛.自动转移飞行器自主导航方法研究[D].长沙:国防科学技术大学.2009,04. [3] 徐景硕,周胜明,蒋华君.惯性导航系统的发展及其关键技术综述[J].科技信息, 2009(35):865-866. [4] 周年芳,柏林森.惯性导航基本理论及发展综述[J].农家科技,2011(04):112. [5] 徐林,李世玲,屈新芬.惯性导航系统传递对准技术综述[J].信息与电子工程, 2010,8(06):633-0. [6] 徐景硕.惯性传感器技术及发展[J].传感器技术, 2001,20(05):1-4. [7] 俞济祥.惯性导航系统各种传递对准方法讨论[J].航空学报,1988,09(05):211-217. [8] 葛磊.容积卡尔曼滤波算法研究及其在导航中的应用[D].哈尔滨:哈尔滨工程大 学.2013,05. [9] 程建华,王通达,宋春雨等.舰船捷联惯导传递对准的改进自适应滤波算法[J]. 系统工程与电子技术, 2016,38(03):638-3. [10] 熊明, 捷联惯性导航系统高阶导航算法研究, 2016, 郑州轻工业学院. 第页. [11] 彭智宏,穆京京,张力军等.基于对偶四元数的航天器相对位置和姿态耦合控制[J].飞 行器测控学报, 2013,32(06):9-5. [12] 钱华明,孙龙,黄蔚等.基于加性对偶四元数的惯性/天文组合导航算法[J].北京航空航天大学学报, 2013,39(06):739-744. [13] 张秋昭,张书华,刘志平等.基于奇异值分解的鲁棒容积卡尔曼滤波及其在组合导航中的应用[J].控制与决策,2014,29(02):341-346. [14] 潘泉,杨峰,叶亮等.一类非线性滤波器——UKF综述[J].控制与决策,2005,20(05):481-494. [15] 邓红,刘光斌,陈昊明等.基于UKF的导弹SINS/CNS姿态估计方法[J]. 系统工程与电子技术, 2010,32(09):1987-1990. [16] 王剑颖,孙兆伟,梁海朝等.基于对偶四元数的航天器单目视觉导航算法[J].哈尔滨工业大学学报, 2013,45(01):7-13. [17] 钱萍,王惠南.基于对偶四元数的航天器交会对接位姿双目视觉测量算法[J]. 宇航学报, 2013,34(01):32-38. 59 郑州轻工业学院硕士学位论文 [18] 王红辉,杨绍卿,吴成富等.旋转矢量在高动态全姿态飞行器运动方程中的应用[J].兵 工学报, 2016,37(03):439-446. [19] 韩萍,干浩亮,何炜琨等.基于CDKF的飞机姿态角估计[J]. 数据采集与处理, 2013,28(06):790-795. [20] 丁国强.惯性导航系统传递对准技术关键问题研究[D].哈尔滨:哈尔滨工程大学.2010,05. [21] 王彤,马建仓,秦涛等.基于旋转四元数的姿态解算算法[J].弹箭与制导学报, 2014,34(03):15-20. [22] 张荣辉,贾宏光,陈涛等.基于四元数法的捷联式惯性导航系统的姿态解算[J]. 光学精密工程, 2008,16(10):1963-1970. [23] 吴高龙,刘华伟,郝顺义等,对偶四元数导航的简化算法[J].应用科学学报, 2010,28(06):6-6. [24] 吴高龙,刘华伟,郝顺义等.对偶四元数导航算法的改进及仿真研究[J]. 系统工程与电子技术[J], 2011,33(04): 862-868. [25] Qiao.B, Tang.S.R, Ma.K.X, et al. Relative position and attitude estimation of spacecrafts based on dual quaternion for rendezvous and docking[J]. Acta Astronautica, 2013, 91(6): 237-244. [26] 严恭敏.车载自主定位定向系统研究[D].西安:西北工业大学,2006,05. [27] 王励扬,翟昆朋,何文涛等, 四阶龙格库塔算法在捷联惯性导航中的应用[J].计算机仿 真,2014,31(11):56-59. [28] 刘江.基于鲁棒估计理论的列车组合定位方法研究[D].北京:北京交通大学,2011,06. [29] 杜继永,张凤鸣,李建文等.高动态环境下SINS姿态更新的改进等效旋转矢量算法[J]. 空间科学学报, 2013,33(01):85-91. [30] 韩萍,干浩亮,何炜琨等.基于迭代中心差分卡尔曼滤波的飞机姿态估计[J]. 仪器仪表学报, 2015,36(01):187-193. [31] 秦永元,张士邈.捷联惯导姿态更新的四子样旋转矢量优化算法研究[J].中国惯性技术学报,2001,09(04):1-7. [32] 李荣冰,刘建业,曾庆化等.基于MEMS技术的微型惯性导航系统的发展现状[J]. 中国 惯性技术学报, 2004,12(06):90-96. [33] 杨建强,刘斌,侯建军等.一种改进的速度加姿态匹配传递对准方法[J].光学与光电技术, 2014,12(06):61-73. 60 参考文献 [34] 钱伟行.捷联惯导与组合导航系统高精度初始对准技术研究[D].南京:南京航空航天 大学. 2010,01. [35] 丁国强,马军霞,熊明等.大失准角传递对准杆臂效应影响研究[J].郑州大学学报(工学版), 2015,36(02):110-114. [36] 陈文颉,马韬,陈杰.对偶四元数捷联惯性导航系统初始对准方法[J].北京理工大学学报, 2012. 32(1):56-61. [37] 严恭敏,严卫生,徐德民.简化UKF滤波在SINS大失准角初始对准中的应用[J]. 中国惯性技术学报, 2008,16(03):253-2. [38] JIN Y, HE Y, FU J, et al. A fine interpolation based parame tricinter polation method with a novel real-time look ahead algorithm [J].Computer-Aided Design, 2014( 55) : 37-48． [1] Yu M J,Lee J G,Park H W.Comparison of SDSINS in-flight alignment using equivalent error models[J].IEEE Transactionon Aerospace and Electronic Systems, 1999, 35(3): 1046-10. [40] 陈亮.基于对偶四元数的捷联惯性导航算法[D].哈尔滨:哈尔滨工业大学.2011,06. [41] 周江华,苗育红,李宏等.四元数在刚体姿态仿真中的应用研究[J].飞行力学, 2000,18(04):28-32. [42] 费景高.捷联式制导系统中四元数的保范递推计算[J].航天控制,2000(03):37-43. [43] Dong.H.Y, Hu.Q.L, Ma.G.F. Dual-quaternion based fault-tolerant control for spacecraft formation flying with finite-time convergence[J]. ISA Transactions, 2016, 61(1): 87-94. [44] Wang.Y.Q, Lu.R.Q, Wang.J.Z, et al. Distributed consensus for multiple rigid-bodies based on unit dual quaternion[C]. IFAC-PapersOnLine. 2015: 616-621. [45] Cheng.J.T, KIM.J, Jiang.Z.Y, et al. Dual quaternion-based graphical SLAM[J]. Robotics and Autonomous Systems, 2016, 77(1): 15-24. [46] 武元新.对偶四元数导航算法与非线性Gaussian滤波研究[D].长沙:国防科学技术大学,2005,05. [47] Wang.X.K, Yu.C.B. Unit dual quaternion-based feedback linearization tracking problem for attitude and position dynamics[J]. Systems & Control Letters，2013, 62(1): 225-233. [48] Kenwright, B. Dual-Quaternions From Classical Mechanics to Computer Graphics and Beyond[J]. 2012(12):1-11. [49] 李涛,武元新,薛祖瑞等.捷联惯性导航系统误差模型综述[J].中国惯性技术学报, 2003,11(04):67-73. 61 郑州轻工业学院硕士学位论文 [50] 刘巧光,杨海军,郭美凤等.捷联惯性导航系统的姿态算法优化设计[J].中国惯性技术学报, 1999,07(02):2-6. [51]H.H.Afsharia, S.A.Gadsden. S.Habibi. Gaussian filters for parameter and state estimation: A general review of theory and recent trends[J]. Signal Processing, 2017,135(01):218-238. [52] Karasalo M, Hu X M. An optimization approach to adaptive Kalman filtering[J]. Automatica, 2011, 47(8): 1785-1793. [53] Bavdekar V A, Deshpande A P, Patwardhan S C. Identification of process and measurement noise covariance forstate and parameter estimation using extended Kalman filter[J]. Journal of Process Control,2011,21(4): 585-601. [] 赵琳,王小旭,孙明等.基于极大后验估计和指数加权的自适应UKF滤波算法[J]. 自动 化学报, 2010,36(07):1007-1019. [55] 闫林波,贾维敏,姚敏立等.基于扩展卡尔曼滤波的动中通低成本姿态估计[J]. 电光与控制, 2013,20(07):53-57. [56] 徐树生,林孝工,赵大威等.强跟踪SRCKF及其在船舶动力定位中的应用[J].仪器仪表学报, 2013,34(06):67-73. [57] 廖鹤,王本利,曹正礼.用于无陀螺卫星姿态确定的预测UKF算法[J]. 南京理工大学学报, 2011,35(05):687-692. [58] 张瑜,房建成.基于Unscented卡尔曼滤波器的卫星自主天文导航研究[J]. 宇航 学报, 2003,24(06):6-650. [59] 郝燕玲,杨峻巍,陈亮等.平方根容积卡尔曼滤波器[J].弹箭与制导学报, 2012,32(02):169-172. [60] 张鑫春,郭承军.均方根嵌入式容积卡尔曼滤波[J].控制理论与应 用,2013,30(09):1116-1121. [61] 刘颖,苏军峰,朱明强.基于平方根容积卡尔曼滤波的RSSI定位参数估计算法[J].系统仿真学报,2014,26(01):119-124. [62] 赵欣,王仕成,廖守亿等.基于抗差自适应容积卡尔曼滤波的超紧耦合跟踪方法[J].自 动化学报,2014,40(11):2530-20. [63] 侯建华等.尺度因子自适应的UKF算法在目标跟踪中的应用[J].中南民族大学学报, 2012.,31(02):85-. [] 丁国强,张铎,熊明等.SINS姿态估计的HoMM-UKF模型算法[J].郑州大学学 报,2017(02):31-37. 62 参考文献 [65] 丁国强,熊明,张铎等.SINS/GPS组合导航的机载SAR姿态算法[J].河南大学学报,2016,46(06):674-682. [66] 赵利强,陈坤云,王建林等.基于矩阵对角化变换的高阶容积卡尔曼滤波[J].控制与决策, 2016,31(06):1080-1086. [67] 丁国强,马军霞,付金华等. 一种基于Stirling插值多项式逼近的椭球集员滤波方法: 中国, CNIO522278OA [P].2016,1. [68] 乔相伟.基于四元数非线性滤波的飞行器姿态确定算法研究[D].哈尔滨:哈尔滨工程大学.2011,04. [69] 王小旭,潘泉,黄鹤等.非线性系统确定采样型滤波算法综述[J].控制与决策, 2012,27(06):801-812. [70] 李金梁,黄国荣.H-infinity鲁棒滤波增益研究[J].弹箭与制导学报, 2007,27(05):108-112. [71] Howard Musoff,Murphy J H.Study of Strapdown Navigation Attitude Algorithms [J]. Joumal of Guidanee,control and dynamies,1995,18(2):287-290. 63 郑州轻工业学院硕士学位论文 附录1 硕士期间发表论文目录 [1]. SINS/GPS组合导航的机载SAR姿态算法[J].河南大学学报(自然科学版). [2]. SINS姿态估计的HoMM-UKF模型算法[J].郑州大学学报(工学版). 2017(02). [3]. WinCE 系统流程场景化设计方法研究与应用[J].自动化仪表. 2016(10). [4]. 触摸式工业分析仪的研究与设计[J].电子技术. 2016(03). [5]. 传递对准的扩展Krein Space鲁棒模型算法[C].中国宇航学会深空探测年会. 中国,上 海,2016(10). 附录2 附录2 硕士期间获得科成果 发表专利 [1]. 发明：一种基于Stirling多项式逼近的椭球集员滤波算法，专利号： CN105222780A 科研成果 [1]. 参加国家自然科学基金项目：大失准角传递对准模型及其并行集员估计算法研究（项 目编号：U1204603） [2]. 参与厅级重点项目：低功耗无线传感网络终端系统，并获得河南省教育厅科技成果 二等奖 [3]. 2016年获得校二等奖学金 软件著作权 [1]. SINS/GPS紧组合导航状态参数估计系统V1.0，登记号：2015SR210730 [2]. 惯导初始对准方法性能比较评价系统V1.0，登记号：2015SR260411 [3]. 量测平均算法性能评估系统V1.0，登记号：2015SR140267 [4]. 乘性与加性四元数传递对准算法模型系统V1.0，登记号：2015SR141752 [5]. 基于粒子滤波算法的目标跟踪定位系统V1.0，登记号：2015SR140562 [6]. 基于Kalman滤波算法的目标跟踪定位系统V1.0，登记号：2015SR140261 65 因篇幅问题不能全部显示，请点此查看更多更全内容
Copyright © 2019- efsc.cn 版权所有 赣ICP备2024042792号-1
违法及侵权请联系：TEL:199 1889 7713 E-MAIL:2724546146@qq.com
本站由北京市万商天勤律师事务所王兴未律师提供法律服务