捷联式航姿系统中四元素算法
捷联惯导系统中四元素法求解姿态角仿真模拟
捷联惯导系统中四元素法求解姿态角仿真模拟
捷联惯导系统中四元素法求解姿态角仿真模拟
在捷联惯导系统中,姿态矩阵的'四元素微分方程求解一般采用四阶龙格库塔法,从运算精度与速度上考虑,提出了另一种有效的四阶泰勒展开法.并在典型圆锥运动环境下,对两种算法进行了姿态角误差仿真.通过仿真分析,四阶泰勒展开法的计算精度比四阶龙格库塔法高出1～2个数量级,为改进捷联惯导系统姿态算法提供了理论参考依据.
作者：孙冬梅田增山韩令军 SUN Dongmei TIAN Zengshan HAN Lingjun 作者单位：重庆邮电大学移动通信技术重点实验室,重庆,400065 刊名：弹箭与制导学报 PKU 英文刊名： JOURNAL OF PROJECTILES, ROCKETS, MISSILES AND GUIDANCE 年，卷(期)：2009 29(1) 分类号：V448.22 关键词：四元素四阶龙格库塔法四阶泰勒展开法姿态角。
应用虚拟仪器技术的捷联式航姿系统
第13卷第2期中国惯性技术学报 2005年4月文章编号：1005-6734(2005)02-0054-04应用虚拟仪器技术的捷联式航姿系统赵岳生1，2，邓正隆1，林玉荣 1（1.哈尔滨工业大学控制科学与工程系，哈尔滨 150001；2.中国西昌卫星发射中心，西昌 615000）摘要：结合捷联式航姿系统的研究开发和运行特点，阐述了应用虚拟仪器技术提高研发效能的优越性。
用虚拟仪器的体系结构实现了硬件与软件的完美结合。
使用四阶龙格－库塔法实现四元数捷联修正算法，应用虚拟仪器技术设计了一套捷联式航姿系统。
关键词：捷联惯导；虚拟仪器；航姿系统中图分类号：U666.1 文献标识码：AAHRS Design Applying Virtual Instrument TechnologyZHAO Yue-sheng1，2, DENG Zheng-long1, LIN Yu-rong1(1. Department of Control Science and Engineering, Harbin Institute of Technology, Harbin 150001, China;2. Xichang Satellite Launch Center, Xichang 615000, China)Abstract: With the specific features of SINS (Strapdown Inertial Navigation System), this paper explains the necessity of applying virtual instruments technology to promote the efficiency and capacity of SINS’ development. As an instance, the attitude heading reference system is re-designed.Key words: SINS; virtual instrument technology; AHRS0 前 言捷联惯性导航系统的研究、开发和运行有其自身的特点，如理论基础深厚，专业性强，测试设备复杂，调校步骤繁琐，算法独特，并且有很强的实时性要求。
基于卡尔曼滤波器的数字式捷联航姿系统算法设计
Fig.5 Map of vehicle-bore data
字式捷联航姿系统的关键，经半实物仿真证明，该算法合理、有效，为研发新型航姿系统替代老式模拟航姿器奠定了基 础。由于民用飞机的飞行非常平稳，几乎没有大的机动，载机进行加速或者大角速度转弯的时间很短，因此以上的航姿 算法应用在民用飞机等低动态载体上非常适合；同时，民用飞机上还有诸如大气数据等分系统，未来研究中将会进一步 将大气数据纳入航姿滤波器中进行综合，进一步提高航姿系统的动态适应性和测量精度。 参考文献(References)： [1] 曾庆化，刘建业，赖际舟，杜亚玲. DSP硬件算法在捷联惯性AHRS系统中的实现[J]. 中国惯性技术学报，2006，14(6)：1-4. ZENG Qing-hua, LIU Jian-ye, LAI Ji-zhou, DU Ya-ling. Realization of DSP hardware algorithms for strapdown inertial AHRS[J]. Journal of Chinese Inertial Technology, 2006, 14(6): 1-4. [2] Gebre-Egziabher D, Hayward R C, Powell J D. A low-cost GPS/inertial attitude heading reference system (AHRS) for general aviation applications[C]//IEEE1998 Position, Location and Navigation Symposium, April 1998: 518-525. [3] 王宇，王庆，吴峻，万德钧. 光纤捷联航姿基准系统的设计与实现[J]., 中国惯性技术学报，2007，15(2)：128-131. WANG Yu, WANG Qing, WU Jun, WAN De-jun. Design and realization on strapdown attitude and heading reference system based on FOG[J]. Journal of Chinese Inertial Technology, 2007, 15(2): 128-131. [4] 陈小凤，关政军. 船用光纤陀螺捷联航姿基准系统[J]. 大连海事大学学报，2005，31（1）：26-28. CHEN Xiao-feng, GUAN Zheng-jun. Fiber optic gyro strapdown attitude and heading reference system for ship[J]. Journal of Dalian Maritime University, 2005, 31(1): 26-28. [5] Rios J A, white E. Low cost solid tate GPS/INS package[EB/OL]. [2006-11-08]. . （上接第 207 页） [3] 黄谟涛，翟国君，管诤，等. 海洋重力场测定及其应用[M]. 北京：测绘出版社，2004：69-70. HUANG Mo-tao, ZHAI Guo-jun, GUAN Zheng, et al. The measurement and application for sea gravity measurements[M]. Beijing: Publishing Company of Mapping, 2004: 69-70. [4] 吴太棋，黄谟涛，陆秀平，金迹航. 重力场匹配导航的重力图生成技术[J]. 中国惯性技术学报，2007，15(4): 438-441. WU Tai-qi, HUANG Mo-Tao, LU Xiu-ping, JIN Hang-ji. Gravity map creating technology in gravity matching navigation[J]. Journal of Chinese inertial technology, 2007, 15(4): 438-441. [5] Jacques Hinderer, David Crossley, Olivia Jensen. A search for the slichter triplet in superconducting gravimeter data[J]. Physics of The Earth and Planetary Interiors, 1995, 90(3-4): 183-195. [6] Cheinway Hwang, Yu-Shen Hsiao, Hsuan-Chang Shih. Data reduction in scalar airborne gravimetry: Theory, software and case study in Taiwan[J]. Computers & Geosciences, 2006, 32(10): 1573-1584. [7] Pálinkáš V. Precise tidal measurements by spring gravimeters at the station Pecný[J]. Journal of Geodynamics, 2006, 41(1-3): 14-22. [8] Guo J Y, Dierks O, Neumeyer J, Shum C K. Weighting algorithms to stack superconducting gravimeter data for the potential detection of the slichter modes[J]. Journal of Geodynamics, 2006, 41(1-3): 326-333. [9] Merriam J B. The response method applied to the analysis of superconducting gravimeter data[J]. Physics of The Earth and Planetary Interiors, 2000, 121(3-4): 289-299.
捷联系统航姿算法分析
们可以利用前一时刻的角速度采样值对旋
转矢量双子样法中的角增量进行修正 ,由此 4 数字仿真
提出双子样改进算法 。
我们用 MATLAB 编写了仿真程序 ,采用
修正后 ,双子样法的角增量由以下公式 角速度输出的陀螺 ,在圆锥运动下进行了数
提取
字仿真 。不同算法的时间复杂度取捷联矩
Δθ1 = [ 8ω(t - T2/ 2) - ω(t) + 5ω(t + T2/ 阵一次递推所需运算次数 ,如下表所示
<20 8
+ζ·3<8404 ,d2≈0. 5 -
<20 48
Δ; (10) Δθ3 = [ω(t + 2T3/ 3) +ω(t + T3) ]·T3/ 6 +Δ;
式中 ζ, = 0. 8 ,目的是为了减小漂移误差 。 若认为在式 (5) 中 ,一个计算周期内ω
= a + b·t ,并作一定的近似 ,可推导出等效 旋转矢量双子样计算公式为
自动化技术与应用 Vol. 19 ,No. 1 2000 年第 1 期 7
q (t + T1) = q (t) + (k1 + k2 + k3 + k4) / 6
若认为在一个计算周期内ω= a + b·t +
(7) c·t2 ,可导出等效旋转矢量三子样计算公式
式中 ,T1 为计算周期 ; k1 = 0. 5·T1[ωb (t) ]·q (t) ; k2 = 0. 5·T1 [ωb (t + T1/ 2) ] ·( q (t) + k1/
对于双子样法 Δθ1 = [ω(t) +ω(t + T2/ 2) ]·T2/ 4 ;
基于四元数法的捷联式惯性导航系统的姿态解算
0
sin C
cos C
b X EbZ
( 1) 根据欧拉角微分方程, 由角速度可 以求解 3 个姿态角。欧拉角微分方程式只有 3 个, 但每个 方程 Û x = f ( co s x , sin x ) X 都含有三角函数的运 算, 计算速度慢 , 且方程会出现/ 奇点0 , 方程式退 化, 故不能全姿态工作。 2. 2 方向余弦矩阵微分方程式 当一个坐标系相对另一个坐标系做一次或多 次旋转后可得到另外一个新的坐标系 , 前者往往 被称为参考坐标系或固定坐标系, 后者被称为动 坐标系, 他们之间的相互关系可用方向余弦表来 表示。方向余弦矩阵微分方程式可写为载体坐标 系相对导航坐标系旋转角速度的斜对称矩阵表达 式, 方向余弦表是对这两种坐标系相对转动的一 种数学描述。
E b C ÛE b = Cb 8 Eb , b Eb
2
姿态矩阵的计算方法
由于载体的姿态方位角速率较大 , 所以针对
姿态矩阵的实时计算提出了更高的要求。通常假 定捷联系统/ 数学平台0 模拟地理坐标系 , 即导航 坐标系; 而确定载体的姿态矩阵即为研究载体坐 标系 ( b) 和导航坐标系 ( E ) 的空间转动关系 , 一般
第 16 卷
第 10 期
光学 精密工程
O pt ics and Precision Eng ineer ing
2008 年 10 月 文章编号 1004 -924X( 2008) 10 - 1963 - 08
V ol. 16 N o. 10 Oct. 2008
பைடு நூலகம்
捷联系统四元数姿态解算的精细积分法
１４ ０
四 川 兵 工 学 报
用解析公式 ， 因此它 可 以大大 提高计 算精 度 ， 数值 结果 其
可 以比拟于精 确解 的数值 结果 。该 方法 自提 出 以来 在力
其中： ＝ ｏ ； Ｃ 下 ｉ 。 Ｑ ｑ＋ Ｏ ０＋ ｓ Ｓ ０
四元数 的实 时值 可通过 求解 四元数 姿态 微分方 程而
得到 了广泛 的研 究 。而且 四元数 法 的正确 性和有 效 性 已
假 定矢 量 ｒ 绕通过定点 Ｏ某一轴转动了一个角度 ０如 ，
转 动后 的四元数用 ｒ 表示 ， 用 四元 数表示 ｒ ｒ 的关 则 间 和
系 如 下 ｒ＝９ ｒ Ｑ
被相关专家证明 ， 四元 数法 由于其计 算无 奇点 ， 计算 量
抗干扰能力 强 ， 目前 已广 泛应 用 于航 空 航 天 、 海 、 器 航 机
人、 智能交通 、 无人 飞机 和精 确制 导炸 弹等 飞行器 的 导航 控制当 中。实时高 精度 的姿 态解算 是捷 联式 惯性 导航 系 统的关键技术 ， 它在很大程度上 影响着捷 联惯性 导航 系统 的导航 精 度 ， 姿态 解算 主 要指 的就 是 姿态变 换 矩 阵 的求 解， 通过姿态变换矩阵可以得到 载体 的姿 态和导航 参数计
ห้องสมุดไป่ตู้
肇 收稿 日期 ：０ ０— ２—１ ２１ ０ ０ 基金项 目： 国家 自然科学基金资助项 目（０ ３ ０５ ６ ８５ ０ ） 作者简介 ： 项凤涛 （９ ６ ） 男 ， １８ 一 ， 硕士研究生 ， 主要从 事组合导航研究 ； 王正志 （９ ５ ） 男 ， １４ 一 ， 教授 ， 博士生导师 ， 主要从事 自动控制 ， 模式识别 与图像处理和生物信息学研究 。
Ｑ ＝ｑ ０＋ｑ：＋ｇ ＋ｑ ｌ ３
捷连贯性导航四元算法分析
捷连贯性导航四元算法分析作者：姬庆华王洪源来源：《数字技术与应用》2009年第11期[摘要]本文介绍了捷联式惯性导航系统的基本理论,包括:坐标系的定义,各坐标系之间的转换关系。
[关键词]捷联惯导系统姿态算法四元算法[中图分类号]U[文献标识码]A[文章编号]1007-9416(2009)11-0088-011 捷联惯导系统算法概述捷联惯导系统的主要特征是由导航计算机来完成导航平台的功能。
它利用固联在载体上的陀螺测量角速度来计算姿态矩阵。
姿态矩阵的计算精度直接影响到导航的精度。
因此,姿态算法是捷联惯导算法的核心,因此研究高精度的姿态更新算法具有重要的意义。
目前在工程实践多采用四元数法。
但是,采用毕卡逼近法求解四元数微分方程时使用了陀螺的角增量输出,角增量虽然微小,但不能视作无穷小,而刚体作有限转动时,刚体的空间角位置与旋转次序有关,对于小角度的转动我们近似认为是可以交换的,这样,四元数法中不可避免地引入了不可交换性误差。
在典型圆锥环境时,这种误差达到最大,称为圆锥误差。
因此,设计合理的姿态算法以客服圆锥误差,从而获得准确的捷联姿态矩阵。
2 捷联惯导系统的姿态算法更新捷联惯导系统的姿态更新就是利用陀螺测量的载体角速度实时计算姿态矩阵。
由于载体的姿态角速率较大,可高达,所以姿态矩阵的实时计算,对计算机提出了更高的要求。
姿态实时计算是捷联惯导的关键技术,也是影响捷联惯导系统算法精度的重要因素。
载体的姿态和航向是载体坐标系和地理坐标系之间的方位关系,两坐标系之间的方位关系问题,实质上等效于力学中的刚体定点转动问题。
在刚体定点转动理论中,描述动坐标系相对参考坐标系方位关系的方法有欧拉角法、四元数法和方向余弦法,此外还可以用等效旋转矢量法来描述刚体的定点转动。
但是在工程实际中常用四元数法。
3 姿态算法程序调试及运行结果在捷联惯性系统中,求解姿态微分方程是其中非常重要的一项工作。
在现行的捷联制导系统中,转动四元数法因为具有方程无奇异性,求解速度快,线性化程度高,精度高,载体可以全姿态运行等显著优点而被广泛采用。
捷联惯导四元数的四阶龙格库塔姿态算法
捷联惯导四元数的四阶龙格库塔姿态算法史凯;刘马宝【摘要】针对捷联惯导姿态更新算法高精度、结构复杂度低的需求,为了满足常规武器工程化的需求,提出捷联惯导四元数的四阶龙格库塔姿态解算算法.根据载体初始姿态角确定姿态转换矩阵,由姿态转换矩阵确定四元数初值,用四阶龙格库塔法解四元数微分方程,更新四元数,从而根据四元数与姿态角之间对应关系解算弹体姿态角.120迫弹平台仿真结果验证了四阶龙格库塔姿态更新算法的正确性,姿态解算精度0.01°,开发实用化样机进行实际抛洒实验,结果表明,该算法切实可行,可工程化使用.【期刊名称】《探测与控制学报》【年(卷),期】2019(041)003【总页数】5页(P61-65)【关键词】捷联惯导;姿态更新算法;四阶龙格库塔法;四元数【作者】史凯;刘马宝【作者单位】西安交通大学航天学院,陕西西安 710049;西安机电信息技术研究所,陕西西安710065;西安交通大学航天学院,陕西西安 710049【正文语种】中文【中图分类】V241.6220 引言捷联惯性导航系统不需要任何外来信息,也不会向外辐射任何信息,仅靠惯性导航系统本身就能在全天候条件下进行三维定向[1],通过弹载三轴正交陀螺仪直接解算弹体姿态,其结构简单,抗干扰能力强。
姿态解算是捷联惯性导航系统的关键技术,目前姿态解算的精度一方面受制于陀螺仪自身器件的水平;另一方面是受姿态解算算法的约束,捷联式惯导姿态更新算法的精度以及复杂程度直接影响整个姿态测量系统的解算精度。
目前姿态解算的主要方法有欧拉角法、四元数法和方向余弦法。
欧拉角法不能用于飞行器全姿态解算而难以广泛应用于工程实践,且实时计算困难。
方向余弦法避免了欧拉角法的“奇点”现象,但方程的计算量大,工作效率低。
四元数法只需要求解四个未知量的线性微分方程组,计算量比方向余弦法小,且算法简单,易于操作,是较实用的工程方法。
关于四元数的求解方法,很多文献都采用数字积分的方法解算载体的姿态[2],或只将龙格库塔算法应用到弹体的滚转角解算,并没有进行三个方向的全姿态角解算[3]。
用于微机械捷联式航姿系统的四元素算法卡尔曼滤波器
用于微机械捷联式航姿系统的四元素算法卡尔曼滤波器
高钟毓; 牛小骥; 郭美凤
【期刊名称】《《中国航空学报（英文版）》》
【年(卷),期】2002(015)003
【摘要】介绍了一种卡尔曼滤波器 ,它适用于由微机械惯性传感器构成的捷联式航姿系统。
文中阐述了系统构成和原理 ,基于四元素算法公式推导了姿态算法和系统误差模型 ,并设计了实时卡尔曼滤波器。
仿真结果表明 ,当没有横向加速度干扰时系统精度优于 0 .0 4度 ,当出现 0 .1g/1Hz的横向交变加速度干扰时 ,精度降为
0 .4 4度。
初步测试结果表明系统的静态精度为 :俯仰和横滚 +/-0 .2度 ,航向 +/- 0 .3度。
【总页数】5页(P171-175)
【作者】高钟毓; 牛小骥; 郭美凤
【作者单位】Department of Precision Instruments and Mechanology Tsinghua University Beijing 100084 China
【正文语种】中文
【中图分类】U666.12; V241.62
因版权原因，仅展示原文概要，查看原文内容请购买。
捷联惯导系统全姿态初始对准方法
2013年第6期 导 弹 与 航 天 运 载 技 术 No.6 2013 总第329期 MISSILES AND SPACE VEHICLES Sum No.329收稿日期：2012-10-21；修回日期：2013-08-12基金项目：民用航天专业技术预先研究项目（D010101）作者简介：刘生炳（1986-），男，工程师，主要从事导航、制导与控制专业研究文章编号：1004-7182(2013)06-0060-04 DOI ：10.7654/j.issn.1004-7182.20130614捷联惯导系统全姿态初始对准方法刘生炳，魏宗康，陈东生，吴 涛（北京航天控制仪器研究所，北京100039）摘要：捷联惯导系统开始导航解算时需要初始对准，工程中常用的静基座初始对准方法有基于克雷洛夫角的静基座初始对准、基于克雷洛夫角的四元数初始对准。
前者由于需要首先求解克雷洛夫角，因此存在与旋转顺序相关和不能全姿态工作的问题；后者需要首先求解克雷洛夫角，然后求解四元数，因此同样存在不能全姿态工作的问题。
针对上述问题，提出了一种避免求解克雷洛夫角，直接通过四元数姿态变换矩阵求得姿态四元数的初始对准方法。
仿真验证结果表明：四元数直接求解初始对准方法可以完成捷联系统静基座初始对准，克服与旋转顺序相关的问题，并且可以实现全姿态初始对准。
关键词：捷联惯导系统；静基座初始对准；克雷洛夫角；四元数 中图分类号：V448.22 文献标识码：AAll Attitude Initial Alignment of Strapdown Inertial Navigation SystemLiu Shengbing, Wei Zongkang, Chen Dongsheng, Wu Tao(Beijing Institute of Aerospace Control Devices, Beijing, 100039)Abstract: It is necessary to perform initial alignment before using SINS, immobile platform initial alignment based on Krylov angle is usually used in project. However, the method is associated with order of rotation, and is unable to be used in all attitude calculation. The other method used in attitude calculation is quaternion initial alignment based on Krylov angle. As it needs to calculate the Krylov angle at the first step, and then calculate quaternion, therefore this method is also unable to be used in all attitude calculation. In order to solve this problem, a new method that can avoid calculating Krylov angle and directly obtain quaternion through attitude transform matrix is proposed. The simulation result shows that the new method can complete initial alignment in SINS and is independent of order rotation, and also can be used in all attitude initial alignment.Key Words: Strapdown inertial navigation system (SINS); Immobile platform initial alignment; Krylov angle; Quaternion0 引 言目前，在捷联惯性系统姿态角解算中主要有以下3种方法：方向余弦法、欧拉-克雷罗夫角法以及四元数法。
捷联惯导系统航姿算法的比较及仿真分析
ｌ ｇ０２＋ｑ 一ｑ；一ｑ； ２（ｑ１ｇ２一ｇｏｇ３） ２（ｇＩｑ２＋ｑｏｑ３）Ｉ ｃ ＝Ｉ ２（ｇ。ｑ２＋ｑｏｑ３） ｇ 一ｑ ＋ｇ；一ｇ； ２（ｇ１ｑ２一ｑｏｑ３）ｌ
ｌ ２（ｇｌｇ３一ｇｏｇ２） ２（ｇ２ｇ３＋ｇ０ｇ１） ｇ —ｇ 一ｇ；＋ｇ Ｉ
（２）
利 用式 （２）便 可 以从 由载 体 坐标 系 转 移 至导 航 地 理 坐 标 系 的姿 态 转 移 矩 阵 中提 取 实 时姿 态 角 供导 航 系统使 用 。 １．２ 四阶龙 格一 库塔 法 求解 四 元数微 分 方程
Ｑ（ｔ＋ ）＝Ｑ（ｆ）＋（ ＋ｋ２＋ｋ３十 ）／６ ｋｌ＝０．５ｈ［ｃｏ（ｔ）］Ｑ（ｔ、 ｋ２＝０．５ｈ［ａ￣（ｔ＋ｈ／２）］［Ｑ（ｆ）＋ｋｉ／２］ （４） ｋ３＝０．５ｈ［ｃｏ（ｔ＋ｈ／２）］［Ｑ（ｆ）＋ｋ２／２］ ｋ４＝０．５ｈ［ｃｏ（ｔ＋ ）儿Ｑ（ ）＋ｋ３］
收稿 日期：２０１６．０１．０５。王小峰 （１９８３一 ）：陕西人，工程 师 ，工 学硕 士，研究方向：惯性及卫星导航 。
矢 量 三子样 法 进行 仿真 分析 比较 ，在典 型 圆锥运 动
下 ，四元 数 算法利 用 四阶 龙格 一库 塔法 对 四元数 微
分 方程 进 行姿 态求 解 ，而 旋转 矢量 三子 样算 法主 要
周 期一 般 都很短 ， 很 小 ， 的高 次项可 略 去不计 ， 得 工程 上 常用 的近 似方 程 ：
＝∞＋二２ ×∞＋ ×（ ×∞） （６）
四元数 微分 方程 为 ：
１
．
ａ＝去Ｑ
（３）
其 中， 表示 由机体 坐 标 系到 导 航坐 标 系 在机 体 坐 标 系 中 的投 影 。式 （３） 的求 解 可 以通 过 用 四阶 龙 格一 库塔 法来 求 取如 下 的 四元 数 递推 公式 ：
用四元数法的捷联惯性导航姿态解算程序
;no dirg ;)')s/m( /yticoleV'(lebaly ;)')sm01( / emiT'(lebalx ;)'noitaluclaC yticoleV'(eltit ;)'Z :etanidrooC noitagivaN','Y :etanidrooC noitagivaN','X :etanidrooC noitagivaN'(dnegel )yticolev,mt(tolp erugif ;no dirg ;)')eerged( /legnA'(lebaly ;)')sm01( / emiT'(lebalx ;)'noitaluclaC erutseG'(eltit ;)'legnA waY','legnA lloR','legnA hctiP'(dnegel ;)'.-b',):,3(ELGNA,mt,'.g',):,2(ELGNA,mt,'-r',):,1(ELGNA,mt(tolp ;ip/081*ELGNA=ELGNA erugif dne
值比度速加/压电的向轴个三计度速加% ?=za_orez% ?=ya_orez% ?=xa_orez% 压电点零的向轴个三计度速加%
*********************************************************% % 值度速加的应相为换转据数将% 3*4201/cca_atad=cca_atad ;]z_cca;y_cca;x_cca[=cca_atad 压电拟模为换转字数度速加% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ;)'txt.zetarelecca'(daol=z_cca ;)'txt.yetarelecca'(daol= y_cca ;)'txt.xetarelecca'(daol= x_cca ;4201/3=etar_cca% **************据数的计度速加入读****************% ;)'txt.zoryg'(daol=z_oryg ;)'txt.yoryg'(daol=y_oryg ;)'txt.xoryg'(daol=x_oryg 据数的仪螺陀入读**********% 据数入读****************************% ;']g-,0,0[=G 2^s/m8.9 位单%;24108.9=g 量矢度速加的生产力重% ;lla raelc ;lla esolc
捷联式航姿系统中四元素算法
万方数据
捷联式航姿系统中四元素算法Kalman滤波器的实现研究
作者： 作者单位：
刊名： 英文刊名： 年，卷(期)： 被引用次数：
施闻明， 徐彬， 陈利敏， SHI Wen-ming， XU Bin， CHEN Li-min 施闻明,徐彬,SHI Wen-ming,XU Bin(青岛海军潜艇学院,山东,青岛,266071)， 陈利敏,CHEN Li-min(北舰航保处,山东,青岛,266071)
Ａｎｄ ｔｈｅ Ｋａｌｌｌｌａｎ矗ｌｔｅｒ ｉｓ ｄｅｓｉｇｎｅｄ．Ｏｎ ｔｈｉｓ ｂａｓｅ，Ｔｈｅ ｅⅡｂｃｔ ０ｆ ｔｈｅ ｍｎｄｏｍ ｎｏｉｓｅ ｉｓ ａｎａｌｙｚｅｄ ａｒ】ｄ出ｅ ａｚｉｍｕｔｈ ｃｏｍｐｅｎｓａｔｉｏｎ ｉ５ ｐｒｏｐｏｓｅｄ．Ｔｈｅ ｓｉｍｍａｔｉｏｎ ｒｅｓｕｌｔ ｓｈｏｗｓ ｔｈａｔ ｔｈｉｓ Ｋａｌｍａｎ ６ｌｔｅｒ ｃａｎ ｅｎｓｕｒｅ ｔｈｅ ｒｅｑｕｉｒｅｄ ａＺｉｍｕｔｈ ｐｒｅｃｉｓｉｏｎ．
度计的输出［吼，ｏ，，吼］７满足方程
万方数据
（１４）
砌响邺０ｆＡＩＩｌｏｍａｔｉｏｎ＆Ａ雨眦咖ｌ ７
‘自动化技术与应用＞２００５年第２４卷第１１期
控制理论与应用
鱼！型旦塑型皇型娅巳！ｌ型幽
估计误差方差阵：
Ｐ（％／后）＝［，一Ｋ（尼）日（后）］Ｐ（Ｊ｜｝／七一１）
（１５）
嗽卜㈦跚，
㈤，
由方程（１４），可以得到 ｇ（南／＿｜｝）＝ｑ（．｜｝，矗一１）ｏ（１，ｇ。（尼／．ｊ｝））
3.赖际舟.刘建业.林雪原 捷联航姿系统中的降阶滤波器的实现研究 2003 4.普航仪器 1985
相似文献(1条)
1.学位论文 张力 基于卡尔曼滤波器的捷联惯性航姿系统研究 2005
用四元数法的捷联惯性导航姿态解算程序
用四元数法的捷联惯性导航姿态解算程序close all;clear all;%重力产生的加速度矢量g=9.80142;%单位9.8m/s^2G=[0,0,-g]';%****************************读入数据%**********读入陀螺仪的数据gyro_x=load('gyrox.txt');gyro_y=load('gyroy.txt');gyro_z=load('gyroz.txt');%****************读入加速度计的数据**************%acc_rate=3/1024;acc_x =load('acceleratex.txt');acc_y =load('acceleratey.txt');acc_z=load('acceleratez.txt'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %加速度数字转换为模拟电压data_acc=[acc_x;acc_y;acc_z];data_acc=data_acc/1024*3%将数据转换为相应的加速度值%%*********************************************************%加速度计三个轴向的零点电压%zero_ax=?%zero_ay=?%zero_az=?%加速度计三个轴向的电压/加速度比值%rate_ax=? %单位是m/s^2/V%rate_ay=?%rate_az=?%acc_x=acc_x*acc_rate;%acc_y=acc_y*acc_rate;%acc_z=acc_z*acc_rate;aver_acc_x=mean(acc_x)aver_acc_y=mean(acc_y)aver_acc_z=mean(acc_z)%采样时间dtime=0.01;tm=0:dtime:0.01* (size(gyro_x,2)-1);%个数numn_point=size(gyro_x,2);%图1figureplot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.'); title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.');%title('加速度的计的采样曲线'):%对采样曲线进行低通滤波a=[1,2,4,2,1];%gyro_x=filter(a/sum(a),1,gyro_x);%gyro_y=filter(a/sum(a),1,gyro_y);%gyro_z=filter(a/sum(a),1,gyro_z);%比例变换gyro_x=gyro_x/1024*3/0.6;gyro_y=gyro_y/1024*3/0.6;gyro_z=gyro_z/1024*3/0.6;%零点电压--陀螺仪，取前80个数的平均电压zero_gx=sum(gyro_x(1:80))/80zero_gy=sum(gyro_y(1:80))/80zero_gz=sum(gyro_z(1:80))/80%减去零点gyro_x=(gyro_x-zero_gx)/0.0125/180*pi; gyro_y=(gyro_y-zero_gy)/0.0125/180*pi; gyro_z=(gyro_z-zero_gz)/0.0125/180*pi;%gyro_x=(gyro_x-2.5)/0.0125/180*pi;%gyro_y=(gyro_y-2.5)/0.0125/180*pi;%gyro_z=(gyro_z-2.5)/0.0125/180*pi;%测试数据accelerate=zeros(3,n_point);accelerate(1,1:100)=10;accelerate(1,101:200)=-10;accelerate(1,201:300)=0;%陀螺仪数据gyro_x=zeros(1,n_point);gyro_y=zeros(1,n_point);gyro_z=zeros(1,n_point);gyro_z(1:100)=pi/3;gyro_z(101:200)=-pi/3;%重力轴始终有加速度accelerate(3,:)=accelerate(3,:)+9.8;figureplot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.');title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%画出陀螺仪的采样曲线figureplot(tm,gyro_x,'r-',tm,gyro_y,'g.',tm,gyro_z,'b-.');title('陀螺仪的采样曲线');legend('x_Gyro','Y_Gyro','Z_Gyro');xlabel('Time / (10ms)');ylabel('Angel_rate/ (degree/s)');grid on;%size(gyro_x)%size(gyro_y)%size(gyro_z)data_gyro=[gyro_x;gyro_y;gyro_z];%转移矩阵--即方向余弦矩阵T=eye(3); %T是3*3的单位矩阵,初始转移矩阵%四元数矩阵，存储每步更新之后的四元数，方便以后绘图Q=zeros(4,n_point);%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。
飞行器姿态测量系统设计
本系统采用 Cortex-M3 为内核的 STM32F103CBT6 单片机作为微控制器，由全球首 例整合 3 轴加速度计和 3 轴陀螺仪的运动处理组件作为惯性测量单元分别感应 x 轴、 y 轴、z 轴的角速度和重力加速度。论文分析了基于姿态四元数、一阶毕卡解法的捷 联惯导姿态确定方法、基于加速度计/陀螺仪的姿态确定方法，利用 Mahony 的互补 滤波函数对 MEMS 陀螺仪随机漂移误差进行了建模和补偿。
捷联系统的四元数法姿态算法
捷联系统的四元数法姿态算法算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。
具体算法：1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A5.重复2～4部，即可连续更新姿态6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态核心算法1，欧拉角转四元数void Quaternion::FromEulerAngle(const EulerAngle &ea){float fCosHRoll = cos(ea.fRoll * .5f);float fSinHRoll = sin(ea.fRoll * .5f);float fCosHPitch = cos(ea.fPitch * .5f);float fSinHPitch = sin(ea.fPitch * .5f);float fCosHYaw = cos(ea.fYaw * .5f);float fSinHYaw = sin(ea.fYaw * .5f);w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;}核心算法2，四元数转欧拉角EulerAngle Quaternion::ToEulerAngle() const{EulerAngle ea;ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));return ea;}核心算法3，四元数乘法Quaternion Quaternion::Multiply(const Quaternion &b){Quaternion c;c.w=w*b.w -x*b.x -y*b.y -z*b.z;c.x=w*b.x +x*b.w +y*b.z -z*b.y;c.y=w*b.y -x*b.z +y*b.w +z*b.x;c.z=w*b.z +x*b.y -y*b.x +z*b.w;c.Normalize();return c;}次要的规范化算法：void Quaternion::Normalize(){float s=getS();w/=s;x/=s;y/=s;z/=s;}float Quaternion::getS(){return sqrt(w*w+x*x+y*y+z*z);}我的loop函数，算法的集成部分：Quaternion nowQ;void loop() {int intx, inty,intz;float pitch,roll,yaw;gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw); EulerAngle dt;dt.fRoll=roll;dt.fPitch=pitch;dt.fYaw=-yaw;Quaternion dQ;dQ.FromEulerAngle(dt);nowQ=nowQ.Multiply(dQ);count++;if (count>1000){EulerAngle nowG=nowQ.ToEulerAngle();Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚Serial.print(",");Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰Serial.print(",");Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航Serial.print(",");Serial.print(nowQ.getS(),11);//偏航Serial.println();count=0;}}四元数与欧拉角之间的转换在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。
四元数用于飞机姿态解算
由上述矩阵L 可解算出三个姿态角
a rc ta n L L 1 1 2 1 a rc sinL 1 3 a rc ta n L L 3 2 3 3
奇异性问题：当
2
时，L110,L330
, 无法确定，姿态角解算出现奇异
姿态矩阵及姿态角的解算涉及超越函数计算，运算量较大
四元数理论及其应用
目 录 Contents
四元数的产生背景 四元数的定义和性质 四元数的应用举例 总结
四元数的产生背景
起源
发展
应用
Hamilton 于1843年 扩展复数到 更高维的层 次，指出矢 量之间的变 换
Maxwell将 四元数数量 部分和矢量 部分的分开 ，作了大量 矢量分析等
在飞行器 姿态解算 中的应用 ，来解决 大姿态角 的控制问 题
q0 q1i q2 j q3k q0 q1i q2 j q3k
q0 q1i q2 j q02 q12 q22
q3k q32
q* q2
2
q*q0q1iq2jq3k
q 2
q0 2q12q2 2q3 2
如果 q 1 ,则 q 1 q*
q
2
四元数的定义和性质
除法：分为左除和右除。对四元数 x , p 和 q
可同样求解，但 和 会出现突变，角度变化不平滑
若 2 ，cos0
0
0
1
Lsgnsin cos 0
sgncos sin 0
物理意义：第2次旋转使得第1次旋转和第3次旋转重合 无法具体确定 和 ，即有多组欧拉角与姿态四元数对应
四元数的应用举例（4） 大角度四元数与欧拉角的转换（姿态四元数 欧拉角） ①姿态小幅调整（如姿态稳定控制）
经济型捷联航姿系统关键技术研究
代号分类号学号密级10701TP273 公开0532121444题（中、英文）作者姓学科门西安电子科技大学学位论文独创性（或创新性）声明秉承学校严谨的学风和优良的科学道德，本人声明所呈交的论文是我个人在导师指导下进行的研究工作及取得的研究成果。
尽我所知，除了文中特别加以标注和致谢中所罗列的内容以外，论文中不包含其他人已经发表或撰写过的研究成果；也不包含为获得西安电子科技大学或其它教育机构的学位或证书而使用过的材料。
与我一同工作的同志对本研究所做的任何贡献均已在论文中做了明确的说明并表示了谢意。
申请学位论文与资料若有不实之处，本人承担一切的法律责任。
本人签名：日期西安电子科技大学关于论文使用授权的说明本人完全了解西安电子科技大学有关保留和使用学位论文的规定，即：研究生在校攻读学位期间论文工作的知识产权单位属西安电子科技大学。
学校有权保留送交论文的复印件，允许查阅和借阅论文；学校可以公布论文的全部或部分内容，可以允许采用影印、缩印或其它复制手段保存论文。
同时本人保证，毕业后结合学位论文研究课题再撰写的文章一律署名单位为西安电子科技大学。
（保密的论文在解密后遵守此规定）本学位论文属于保密，在年解密后适用本授权书。
本人签名：日期导师签名：日期摘要本文对微机械陀螺/微机械加速度计/电子罗盘/GPS的经济型组合捷联航姿系统的关键技术及工程实现问题进行了深入研究和必要的工程实验，为该项技术的推广应用奠定了基础。
传感器误差是影响姿态解算精度的主要因素。
因此本文对传感器的误差进行分析和补偿，并对传感器的数据进行处理。
重点研究了微机械陀螺的随机漂移误差建模方法，将小波分析理论引入到微机械陀螺的随机漂移建模中，提高了建模精度。
从工程实用的角度出发，对现有捷联姿态算法进行分析和比较，得出四阶龙格库塔法更适合实际应用的结论。
由于微机械陀螺长时间运行后存在较大的积累误差，本文采用基于Kalman 滤波器的多传感器信息融合技术来修正该误差，并给出了Kalman滤波算法和四阶龙格库塔算法组合校正陀螺漂移误差的定姿算法。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
文献［２］给出姿态四元素满足跚：
ｇｏ
ｏ
～彬 附
ｇｏ
９１
‰
０ｏ
＝１／２
ｑ２
～ ‰‰
ｑ１ （２）
ｑ２
９３
一 ％
甜竹
９３
状态角可以由方向余旋矩阵ｃ：的成分计算
航向角Ｈ＝ａｒｃｔａｎｃｌ２／ｃ１１
（３）
纵摇角妒＝一ａｒｃｓｉｎｃ。，
（４）
横摇角臼＝ａＩ℃协ｎ％／ｃ。
（５）
姿态矩阵拜为正交矩阵，所以有曩＝（碍）～＝（冗）７。因 此，若知道砖的元素值，就可以求出ｃ：的元素值。
万方数据
捷联式航姿系统中四元素算法Kalman滤波器的实现研究
作者： 作者单位：
刊名： 英文刊名： 年，卷(期)： 被引用次数：
施闻明， 徐彬， 陈利敏， SHI Wen-ming， XU Bin， CHEN Li-min 施闻明,徐彬,SHI Wen-ming,XU Bin(青岛海军潜艇学院,山东,青岛,266071)， 陈利敏,CHEN Li-min(北舰航保处,山东,青岛,266071)
《自动化技术与应用》２００５年第２４卷第１１期
控韦Ｕ理论与ｌ立用
捷联式航姿系统中四元素算法 Ｋａｌｍａｎ滤波器的实现研究
施闻明１，徐彬１Biblioteka 陈利敏２（１．青岛海军潜艇学院，山东青岛２㈣１；２．北舰航保处，山东青岛２６６０７１）
摘要：本文基于四元素算法推导了姿态算法和捷联惯导系统误差模型，并设计了Ｋａｈａｎ滤波器。在此基础上分析了误差模型的 随机噪声补偿和提出了航向修正：仿真结果表明，本文讨论的这种ＫａＩⅡ砌滤波器能保证航向精度，具有实际应用意义：
其中，仃乙，ｄ乙，盯：为陀螺的白噪声漂移均方根值，ｄ乞，盯乙，
ｄ乞为陀螺的一阶马尔可夫漂移均方根值，仃乙，盯：，，盯乞为加速度
计白噪声均方根值，打为相关时间。
Ｓｔｅｐ Ｒｅｓｐｏｎｓｅ
㈨肌 ＾＾．＾。 一
ｙ ｙ Ｖ ｙ｝『’７ｌｙ
Ｓｔｅｐ Ｒｅｓｐｏｎｓｏ
／一 ——‘—、——Ｌ
，
｛
Ｔｉｍｅ（ｓｅｃ） 原函数
自动化技术与应用 TECHNIQUES OF AUTOMATION AND APPLICATIONS 2005，24(11) 2次
参考文献(4条) 1.张树侠 捷联式惯性导航系统 1992
2.韦锡华.杨晓东 基于最优控制的四元数据误差传播捷联矩阵算法分析[期刊论文]-中国惯性技术学报 2003(02)
２．２状态模型 真实姿态可以由四元素乘法的形式表示，如下
ｇ＝ｇ《多ｇｅ
小：卧㈩
㈤
带进方程ｃｓ，得［三］＝ｃ厶。，一２ｃ吼，，毪［兰］＋”ｃｔ，，即
ｃ《｝艿：６＝［圣］一爸：·［兰］，：６＝一爸：·［兰］，
最后，构造测量矢量ｚ＝［主！］，测量噪声矢量ｙｃ“，＝ 【釜ｌ‘女’］，和钡ｎ量矩阵，了ｃｔ。，：＝ｃ，，。ｃ ｚ；，，ｃ，ｔ。，，，其中Ｈ，ｃｔ。，＝＝ 嘲
厂ｇ：＋ｑ：一ｇ；一ｑ；
ｃ：＝ｌ ２（ｇ。ｇ：一ｇ。９３）
Ｌ ２（ｑｌ ９３＋ｇｏ ９２）
２（ｇｌ ９２＋ｇｏ ｑ３） ｇ：一９；＋ｇ；一ｇ； ２（９２ ９３一ｇｏ ｑ１）
们 啦 一 ％啦
吼 啦 ＋ 铷吼
１Ｉ Ｊ 纵纵：铀 一 ：吼 一 ：啦 ｝ ，，靠
ｑ
这里上标６和下标ｎ分别表示“载体坐标系”和“地理坐标系”。
２．１ 姿态角和方向余旋矩阵
文献［１］给出姿态矩阵的表达式…如下：
ｒ ｃｏｓ＾ｃｏｓ口＋ｓｉｎ日ｓｉｎ驴ｓｉｎ目 ｓｉｎＨｃｏｓ驴
死＝Ｉ ｃｏｓＨｓｉｎ驴ｓｉｎ目一ｓｉｎ日ｃｏｓ日ｃｏｓ妒ｃｏｓＨ
ｌ
一。０ｓ驴。ｉ。口
。ｉ。妒
ｃｏｓ日ｓｉｎ目一ｓｉｎＨｓｉｎ妒ｃｏｓ口］
—ｓｉｎ日ｓｉｎ日一ｃｏｓ日ｓｉｎ ５ｆｌｃｏｓ８
运算。
３误差模型的随机噪声补偿
将加速度计的过程噪声和陀螺的常值漂移等相关噪声都简 化为白噪声，这使得估计的实际误差增大，甚至会滤波发散。文
献［３］指出，可以采用加大噪声方差的方法来补偿模型误差。 取系统的白噪声矢量为：
形＝［训弘训盯
％ 训攒
训叮
加。
训麒
叫＂
叫ⅢＪ
对应地取白噪声方差Ｏ（￡）为：
ｄ嵫［叮：盯≥ｄ；２盯：／死２仃≥／巧２盯乙／砭ｄ乞 盯乙 ｄ乞］
［３］赖际舟，刘建业，林雪原，等．捷联航姿系统中的降阶滤波器的 实现研究［Ａ］．南京惯性技术学会第九次学术年会论文［ｃ］．南京，２００３．
１０．２４—２７
［４］普航仪器［Ｍ］．北京：海军司令部航海保证部，１９８５．７ｌ一８０
作者简介：施闻明（１９８２一），男。硕士研究生，专业方向为载运工具运用工程。
Ｔｉｍｅ（ｓｅｃ）
加Ｋａｌｍａｎ滤波器的函数
图２加Ｋａｌｌｌ，ａｎ滤波器前后系统幅频比较
６结束语
本文从计算速度和计算精度的角度出发，采用四元素法来 解算姿态矩阵和推导了Ｉ（ａｌＩＩｌａｎ滤波器的方程，并在此基础上对 误差模型的随机噪声补偿和航向修正进行了分析。仿真结果表
明，基于四元素算法的Ｉ（ａｌｒ咖滤波器具有较高的滤波性能。
２ ｋａｌｍａｎ滤波器的设计二
在捷联惯导系统中，三个陀螺互相垂直配置来测量运载体
收稿日期：２００５—０５一０８ ６ Ｔｅｃｈｎｉｑｕｅｓ ｏｆ ＡＩｌｌｏｍａｔ咖＆Ａｐｐ¨例ｉｏｎｓ
万方数据
的角速度矢量（ｐ，ｇ，ｒ）。同样，三个加速度计测量加速度矢量 （吼，ｏ，，吼）。然后三个姿态角和他们的角速度可以通过相关的 转换和硒ｌＩＩｌａｎ滤波器获得。
（１卜１）
Ｂ（后，七）＝Ｂ（知，七一１）＋△Ｂ（知，南）
（１６—２）
埘（１｜｝，｜｜｝）＝ｌｌＪ（｜ｊ｝，尼一１）＋曰（七／七）
（１卜３）
从加速度计的信号口６，可以通过相关变换和最小迭带法，
盛羞二！：卢 ［堕［］＿Ｊ塑叫
５仿真和测试结果
得到初始姿态四元素ｑ（ｏ／０）和它的估计误差ｇ。（ｏ／Ｏ） 方程（１１）一（１６）加上初始值便构成完整的ＫａｌＩＩｌａｎ滤波器
3.赖际舟.刘建业.林雪原 捷联航姿系统中的降阶滤波器的实现研究 2003 4.普航仪器 1985
相似文献(1条)
1.学位论文 张力 基于卡尔曼滤波器的捷联惯性航姿系统研究 2005
惯性导航作为一种自主的导航方法，在导航技术中占有突出的地位，捷联式惯性导航系统是惯性导航技术的发展方向。为适应现代作战飞机要求惯 性导航系统向小型化、数字化和综合化的方向发展，有必要对基于低成本的微型惯性测量单元组成的捷联式组合惯性导航系统做进一步的理论研究和工 程化样机的研发。 本文首先阐述了一般捷联式惯性导航系统的原理，在此基础上，介绍了这种先进的速度组合捷联式航姿系统的原理和构成。本文着重讨论了卡尔曼 (Kalman)滤波器在本项目中的工程应用：间接法闭环校正的线性卡尔曼滤波器结构的确定；选择适当的参数进行误差建模；给出速度组合的离散卡尔曼 滤波器的状态方程、量测方程及卡尔曼算法流程。通过理论分析和对多传感器组合的导航系统仿真结果说明，采用Kalman滤波器对各信号组合后，克服 了由低精度低成本IMU构成的纯捷联惯导系统导航参数发散很快的现象，导航参数的精度得到了很大的提高；引入速度再进行组合后，姿态精度也得到较 大改善，各项导航参数可以满足一般导航定位的要求。为此，确定了捷联式惯性航姿系统的最小化组成。 在相关单位的大力支持下，先期完成了捷联式惯性航姿系统工程样机的硬件制作、软件开发和软硬件的组装和调试；地面跑车试验结果也证明，这 种基于低成本、低精度微型惯性测量器件的采用卡尔曼滤波器实现最优组合的捷联式惯性航姿系统不仅具有性能稳定、可靠性高、成本低的优点，而且 输出了较高精度的航姿参数，是一种具有独特优点的先进实用的捷联式惯性导航系统。 本项目仅以IMU、磁传感器和导航计算机组成，是目前国内最小的捷联式惯性航姿系统；也是自主开发的捷联式惯导在国产歼击机上的首次应用。
这里ｉ是真实状态四元素，ｉ是姿态四元素的估计，ｉ是；的误
差。 利用方程（２）对方程（６）微分，并在线性估计后得到ｊ。＝一
叫吼一１／２（加一埘）这里估计误差（训一埘）为系统白噪声阢（ｆ）所 致。陀螺的随机误差综合考虑为白噪声漂移和随机一阶马尔可
夫过程。令陀螺偏差△Ｂ，它由随机偏差率曲＝％（￡）模型化。
扩展状态矢量ｘ＝㈦和状态噪蒯扯哦ｎ
扩展状态方程表示为 克＝Ｆ（ｔ）Ｘ（ｔ）＋形（ｔ）
（７）’
．这里６×６维矩阵Ｆｃｔ，＝［：二１一芝：３”］，｛形ｃ ｔ，｝是
独立的高斯白噪声，即均值为Ｅ｛形（￡）｝＿０，方差为哳｛形（￡）
旷（ｆ）｝＿Ｑ（￡）艿（￡一ｒ）。
？
２．３测量模型
讨论加速度计的测量方程。在捷联状态系统中，三个加速
Ａｎｄ ｔｈｅ Ｋａｌｌｌｌａｎ矗ｌｔｅｒ ｉｓ ｄｅｓｉｇｎｅｄ．Ｏｎ ｔｈｉｓ ｂａｓｅ，Ｔｈｅ ｅⅡｂｃｔ ０ｆ ｔｈｅ ｍｎｄｏｍ ｎｏｉｓｅ ｉｓ ａｎａｌｙｚｅｄ ａｒ】ｄ出ｅ ａｚｉｍｕｔｈ ｃｏｍｐｅｎｓａｔｉｏｎ ｉ５ ｐｒｏｐｏｓｅｄ．Ｔｈｅ ｓｉｍｍａｔｉｏｎ ｒｅｓｕｌｔ ｓｈｏｗｓ ｔｈａｔ ｔｈｉｓ Ｋａｌｍａｎ ６ｌｔｅｒ ｃａｎ ｅｎｓｕｒｅ ｔｈｅ ｒｅｑｕｉｒｅｄ ａＺｉｍｕｔｈ ｐｒｅｃｉｓｉｏｎ．
ｊ
。０ｓ妒。ｏｓ口
：弹斗…：瞄篡＝／驯；
Ｌ乃。 乃： 乃，Ｊ
【横摇角：臼＝ａｒｃｔａｎ（一乃，／乃，）
假设姿态四元素ｑ＝％＋ｇ。ｉ＋ｇ：Ｊ．＋９３＆，满足限制条件时
７＊ｉ＝１，称ｉ为单位四元素。
将地理坐标系利用四元素转换为载体坐标系时，其方向余
控带Ｕ理论与应用
婴！型塑塑型璺型娅巳！！型幽
《自动化技术与应用》２００５年第２４卷第”舅
△ｃ，。＝Ａ＋Ｂｓｉｎ￡，＋Ｃｃｏｓ￡，＋Ｄｓｉｎ（２毋≯＋Ｅｃｏｓ（２己，） 其中：Ａ——圆周罗差，由软铁磁场产生；