捷联系统四元数姿态解算的精细积分法

合集下载

捷联惯导系统中四元素法求解姿态角仿真模拟

捷联惯导系统中四元素法求解姿态角仿真模拟
捷联惯导系统中四元素法求解姿态角仿真模拟
在捷联惯导系统中,姿态矩阵的'四元素微分方程求解一般采用四阶龙格库塔法,从运算精度与速度上考虑,提出了另一种有效的四阶泰勒展开法.并在典型圆锥运动环境下,对两种算法进行了姿态角误差仿真.通过仿真分析,四阶泰勒展开法的计算精度比四阶龙格库塔法高出1～2个数量级,为改进捷联惯导系统姿态算法提供了理论参考依据.
作者：孙冬梅田增山韩令军 SUN Dongmei TIAN Zengshan HAN Lingjun 作者单位：重庆邮电大学移动通信技术重点实验室,重庆,400065 刊名：弹箭与制导学报 PKU 英文刊名： JOURNAL OF PROJECTILES, ROCKETS, MISSILES AND GUIDANCE 年，卷(期)：2009 29(1) 分类号：V448.22 关键词：四元素四阶龙格库塔法四阶泰勒展开法姿态角。

@捷联惯导系统姿态解算模块的实现---具体实现过程

收稿日期 :2008208207 ; 修订日期 :2008209205. 基金项目 : 吉林省杰出青年基金资助项目 ( No . 20060115) ; 中国科学院 “三期创新” 平台资助项目
© 1994-2009 China Academic Journal Electronic Publishing House. All rights reserved.
摘要 : 提出了一种实现捷联惯性导航系统姿态解算模块的方法。基于 DSP 的硬件平台和四元数的数学平台 ,设计了捷联惯性导航系统的姿态解算模块。介绍了捷联惯性导航系统的工作原理和姿态解算的基本算法 , 并给出了四元数法的四阶龙格2库塔数值解法。设计了姿态解算模块的硬件电路和软件实现程序。实验测试结果表明 ,在增量角 < 5° 的情况下 ,用四阶龙格2库塔法进行姿态解算 , 误差 < 0. 005 3 % ; 应用 TMS320C6713B 进行硬件电路设计 , 每次解算时间 <

和调试。综上所述 ,DSP 系统具有速度快、精度高、体积小、成本低、可靠性高以及抗干扰能力强等优点 ,可以满足捷联式惯性导航系统对硬件平台的要求 [ 122 ] 。
© 1994-2009 China Academic Journal Electronic Publishing House. All rights reserved.
,
( 5)
2
K23 T
2
如下 : ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos cos + sin sin sin 2 2 2 2 2 2 λ (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos sin - sin sin cos p1 (0) 2 2 2 2 2 2 = , ψ θ γ ψ θ γ p2 (0) 0 0 0 0 0 0 cos sin cos + sin cos sin 2 2 2 2 2 2 p3 (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 sin cos cos - cos sin sin 2 2 2 2 2 2

捷联惯导系统中一种高精度姿态更新算法的推证

捷联惯导系统中一种高精度姿态更新算法的推证
康帅;庞华伟;周伟林
【期刊名称】《中国空间科学技术》
【年(卷),期】2004(024)005
【摘要】在捷联惯导系统中,姿态更新的计算是捷联惯导系统的关键问题之一.文章对四元数更新算法进行了研究,提出了一种新的四元数四阶泰勒展开高精度航姿算法,并在理论上给出了详细的数学推导和证明;为改进捷联惯导系统算法提供了重要的参考依据.
【总页数】5页(P67-71)
【作者】康帅;庞华伟;周伟林
【作者单位】国防科学技术大学,长沙,410073;国防科学技术大学,长沙,410073;国防科学技术大学,长沙,410073
【正文语种】中文
【中图分类】V4
【相关文献】
1.一种改进的捷联惯导系统姿态算法 [J], 张朝飞;杨孟兴;吴明强
2.捷联惯导系统姿态更新旋转矢量算法的优化 [J], 郭访社;于云峰;刘书盼;龚宇迪;李仪
3.捷联惯导系统姿态更新旋转矢量算法的优化研究 [J], 马得草
4.捷联惯性导航中一种算法的推证 [J], 郭圣权;贾建芳;吴强
5.一种新的捷联矩阵更新算法在无陀螺捷联惯导系统中的应用 [J], 赵龙;史震;马澍田
因版权原因，仅展示原文概要，查看原文内容请购买。

一种新的捷联惯性导航系统姿态四元数方程求解方法

Ｘｉ’ａｎ７１００２５，Ｓｈａａｎｘｉ，Ｃｈｉｎａ）
Ａｂｓｔｒａｃｔ：Ａｎｅｗｃａｌｃｕｌａｔｉｏｎｍｅｔｈｏｄｏｆｑｕａｔｅｒｎｉｏｎｄｉｆｆｅｒｅｎｔｉａｌｅｑｕａｔｉｏｎ，ｗｈｉｃｈｉｓｃａｌｌｅｄｔｈｒｅｅ－ｓｕｂｓａｍｐｌｅｑｕａｔｅｒｎｉｏｎａｌｇｏｒｉｔｈｍ，ｉｓｐｒｅｓｅｎｔｅｄｔｏｓｏｌｖｅｔｈｅｐｒｏｂｌｅｍｏｆｌｏｗｃａｌｃｕｌａｔｉｏｎａｃｃｕｒａｃｙｏｆｔｒａｄｉｔｉｏｎａｌｑｕａｔｅｒｎｉｏｎａｌｇｏｒｉｔｈｍｂｙｕｓｉｎｇｔｈｅｃａｌｃｕｌａｔｉｏｎｐｒｏｃｅｓｓｏｆｔｈｒｅｅ－ｓｕｂｓａｍｐｌｅｒｏｔａｔｉｏｎｖｅｃｔｏｒａｌｇｏｒｉｔｈｍｆｏｒｒｅｆｅｒｅｎｃｅ．Ｉｎｔｈｅｃａｌｃｕｌａｔｉｏｎｐｒｏｃｅｓｓ，ｔｈｅｈｉｇｈｅｒｄｅｒｉｖａｔｉｖｅｏｆｒｏｔａｔｉｏｎｑｕａｔｅｒｎｉｏｎｉｓｃａｌｃｕｌａｔｅｄｏｎｔｈｅｂａｓｉｓｏｆｑｕａｔｅｒｎｉｏｎｄｉｆｆｅｒｅｎｔｉａｌｅｑｕａｔｉｏｎ，ｔｈｅａｎｇｕｌａｒｖｅｌｏｃｉｔｙｏｆｃａｒｒｉｅｒｉｓｆｉｔｔｅｄｂｙｔｈｅｐａｒａｂｏｌａ，ｔｈｅｅｘｐｒｅｓｓｉｏｎｉｓｇｉｖｅｎｆｏｒａｎｇｕｌａｒｉｎｃｒｅｍｅｎｔ，ａｎｄｔｈｅｎａｍｏｄｅｌｆｏｒｃａｌｃｕｌａｔｉｎｇｑｕａｔｅｒｎｉｏｎｄｉｆｆｅｒｅｎｔｉａｌｅｑｕａｔｉｏｎｉｓｅｓｔａｂｌｉｓｈｅｄｂｙｍｅａｎｓｏｆｔｈｅＴａｙｌｏｒｅｘｐａｎｓｉｏｎｏｆｒｏｔａｔｉｏｎｑｕａｔｅｒｎｉｏｎ．Ｔｈｅｓｉｍｕｌａｔｅｄｒｅｓｕｌｔｉｎｄｉｃａｔｅｓｔｈａｔｔｈｅｔｈｒｅｅ— ｓｕｂｓａｍｐｌｅｑｕａｔｅｒｎｉｏｎａｌｇｏｒｉｔｈｍｉｓｂｅｔｔｅｒｔｈａｎｔｈｒｅｅ－ｓｕｂｓａｍｐｌｅｒｏｔａｔｉｏｎｖｅｃｔｏｒａｌｇｏｒｉｔｈｍ．Ｋｅｙｗｏｒｄｓ：ｓｔｒａｐｄｏｗｎｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｓｙｓｔｅｍ；ｔｈｒｅｅ—ｓｕｂｓａｍｐｌｅｑｕａｔｅｒｎｉｏｎａｌｇｏｒｉｔｈｍ；ｒｏｔａｔｉｏｎｖｅｃｔｏｒａｌｇｏｒｉｔｈｍ；Ｔａｙｌｏｒｓｅｒｉｅｓ

捷联系统航姿算法分析

们可以利用前一时刻的角速度采样值对旋
转矢量双子样法中的角增量进行修正 ,由此 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 ;

捷连贯性导航四元算法分析

捷连贯性导航四元算法分析作者：姬庆华王洪源来源：《数字技术与应用》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]。

捷联惯导系统全姿态初始对准方法

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);%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。

捷联系统的四元数法姿态算法

捷联系统的四元数法姿态算法算法输入：物体的初始姿态，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图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。

基于四元数方法的姿态解算方法分析

基于四元数方法的姿态解算方法分析摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。

分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。

最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。

数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。

1引言捷联惯导是一种自主式的导航方法。

该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。

由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。

姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。

载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。

通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。

目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。

三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。

欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。

方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。

随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。

本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。

捷联式惯性导航积分算法设计方案姿态算法

捷联式惯性导航积分算法设计上篇：姿态算法Paul G. SavageStrapdown Associates, Inc., Maple Plain, Minnesota55359摘要：本论文分上下两篇，用于给现代捷联惯导系统的主要软件算法设计提供一个严密的综合方法：将角速率积分成姿态角，将加速度变换或积分成速度以及将速度积分成位置。

该算法是用两速修正法构成的，而两速修正法是具有一定创新程度的新颖算法，是为姿态修正而开发出来的，在姿态修正中，以中速运用精密解读方程去校正积分参数(姿态、速度或位置>，其输入是由在参数修正(姿态锥化修正、速度摇橹修正以及高分辨率位置螺旋修正>时间间隔内计算运动角速度和加速度的高速算法提供的。

该设计方法考虑了通过捷联系统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态基准和矢量速度积分的导航系旋转问题。

本论文上篇定义了捷联惯导积分函数的总体设计要求，并开发出了用于姿态修正算法的方向余弦法和四元数法；下篇着重讨论速度和位置积分算法的设计。

尽管上下两篇讨论中常常涉及到基本的惯性导航概念，然而，本论文是为那些已对基础惯导概念很熟悉的实际工作者而写的。

专门用语：123,,,A A A A =任意坐标系12A A C =将矢量从2A 坐标系投影到1A 坐标系的方向余弦矩阵 I =单位矩阵12A A q =从2A 坐标系投影到1A 坐标系的旋转矢量所构成的姿态变化四元数 1*2A A q =12A A q 的共轭四元数，它的第1项与12A A q 的首项相同，余下的2~4项与 12A A q 的互为相反数 1q =单位四元数，它的第1项为1，其余3项为0V =无具体坐标系定义的矢量A V =列向量，它的各项元素等于矢量V 在坐标系A 的各轴上的投影A V ⨯（） =向量AV 的反对称<或交叉积）形式，代表如下矩阵：000ZA YA ZA XA YAXAV V V V V V -⎡⎤⎢⎥-⎢⎥⎢⎥-⎣⎦其中：XA V ，YA V ，ZA V 是AV 的分量，AV ⨯（）与A 系矢量的矩阵乘积等于AV 与该矢量的叉积A q V =与A V 等量的四元数矢量，0A V ⎡⎤⎢⎥⎣⎦2A ω1A =2A 坐标系相对于1A 坐标系的角速率，当1A 为惯性系<I 系）时，2A ω1A 是由安装在2A 坐标系上的角速率传感器所测到的角速率1.概论惯性导航是通过对速度积分得到位置并对总加速度积分得到速度的过程。

基于面向对象的捷联惯导系统中四元数计算方法

感谢您的观看
汇报人：
四元数的基本运算包括加法、减法、乘法和除法等
在面向对象的捷联惯导系统中，四元数计算方法用于描述载体姿态和角速度等运动参数
四元数的计算方法
定义：四元数是复数在超复数代数中的推广，由实部和虚部组成。
运算规则：遵循超复数代数的运算规则，包括加法、减法、乘法
和除法等。
物理意义：在捷联惯导系统中，四元数表示方向和姿态信息，通过四元数计算方法可以方便地实现姿态和方向的
关键问题2：如何处理系统中的非线性问题解决方案2：采用非线性优化算法或近似线性化方法，如牛顿迭代法、梯度下降法等解决方案2：采用非线性优化算法或近似线性化方法，如牛顿迭代法、梯度下降法等
关键问题3：如何实现高效的四元数计算解决方案3：采用并行计算、分布式计算等技术，提高计算效率解决方案3：采用并行计算、分布式计算等技术，提高计算效率
算法实现：利用面向对象编程语言实现四元数计算的核
心算法
定义四元数：由实部和三个虚部组成的数学概念，用于表示旋转和方向
验证结果：通过实验验证算法的正确性和可行性
实现过程中的关键问题及解决方案
关键问题1：如何保证四元数计算的精度和稳定性解决方案1：采用高精度算法和数值稳定技术，如四元数差分法、滤波算法等解决方案1：采用高精度算法和数值稳定技术，如四元数差分法、滤波算法等
应用范围：四元数计算方法适用于多种惯导系统
与其他算法的比较
计算精度：四元数计算方法具有更高的计算精度，能够更准确地描述旋转运动。
稳定性：四元数计算方法具有更好的数值稳定性，能够避免一些常见的数值问题。
实时性：与其他算法相比，四元数计算方法在实时性方面表现良好，能够满足捷联惯导系统的实时性要求。

捷联惯性导航系统的姿态算法

收稿日期 : 2003207225 基金项目 : 国防预研资助项目 (18 YXGFK D118) 作者简介 : 刘危 (1974 - ) ,男 ,江西都昌人 ,博士生 , bullkill @21cn. com.
© 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved.
2005 年 1 月第 31 卷第 1 期
北京航空航天大学学报 Journal of Beijing University of Aeronautics and Astronautics
January 2005 Vol. 31 No11
捷联惯性导航系统的姿态算法
刘危解旭辉李圣怡
( 19)
Δ T ,则近似中的算法误差为若令 λ= Ω Δ δ ^Φ - δ Φ ≈δ Φ =
N
- 2 ( p + N)
2
p + N +1
∏
( p + N) ! 2 ( p + N ) +1 λ ab p + N +1 ( 2 k - 1) k =1
( 13)
A =
… … … … … … … A i ,1 … A i , N - 1 A i , N … A i , N + p - 1 A i , N + p … … … … …

第 1 期刘危等 : 捷联惯性导航系统的姿态算法
47
k1 K( N + p - 1) × 1 = k2
d1
其中 , A ij ( j ≠( N + p) ) 的定义如式 ( 11) ; 而 A i , N + P 的定义如下 :

四区间双插值捷联惯姿算法

四区间双插值捷联惯姿算法
陶杰武;陈国光;田晓丽
【期刊名称】《应用基础与工程科学学报》
【年(卷),期】2005()S1
【摘要】提出一种减小捷联惯导系统姿态算法误差的运算方法,该算法运用拉格朗日插值逼近载体系下角速率和姿态四元数,通过求解四元数方程组实现姿态更新.本文在典型圆锥运动和规则进动下,将该算法与常见的四子样旋转矢量算法进行了仿真对比,结果表明,新算法能有效提高姿态求解精度.
【总页数】4页(P103-106)
【关键词】捷联姿态算法;四元数;拉格朗日插值
【作者】陶杰武;陈国光;田晓丽
【作者单位】中北大学机械电子工程系
【正文语种】中文
【中图分类】V249.3
【相关文献】
1.捷联惯导系统航姿算法的比较及仿真分析 [J], 王小峰;董正芳
2.弹道修正火箭的捷联惯姿算法比较研究 [J], 纪彩华;陈国光;陈阳;李伟光;何璐;鲍亚琪
3.捷联惯导系统双矢量定姿方法研究 [J], 张剑慧;秦永元;龙瑞
4.双矢量定姿算法提高海参捕捞装置捷联惯导系统粗对准精度 [J], 包建华;乔曦;李
道亮
5.基于捷联惯导的悬臂式掘进机位姿解算算法研究 [J], 郑伟雄;沈阳;马浚清;胡立同;符世琛;吴淼
因版权原因，仅展示原文概要，查看原文内容请购买。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

１４０
四川兵工学报
用解析公式，因此它可以大大提高计算精度，数值结果其
可以比拟于精确解的数值结果。该方法自提出以来在力
其中：＝ｏ；Ｃ下ｉ。Ｑｑ＋Ｏ０＋ｓＳ０
四元数的实时值可通过求解四元数姿态微分方程而
得到了广泛的研究。而且四元数法的正确性和有效性已
假定矢量ｒ绕通过定点Ｏ某一轴转动了一个角度０如，
转动后的四元数用ｒ表示，用四元数表示ｒｒ的关则间和
系如下ｒ＝９ｒＱ
被相关专家证明，四元数法由于其计算无奇点，计算量
抗干扰能力强，目前已广泛应用于航空航天、海、器航机
人、智能交通、无人飞机和精确制导炸弹等飞行器的导航控制当中。实时高精度的姿态解算是捷联式惯性导航系统的关键技术，它在很大程度上影响着捷联惯性导航系统的导航精度，姿态解算主要指的就是姿态变换矩阵的求解，通过姿态变换矩阵可以得到载体的姿态和导航参数计
ห้องสมุดไป่ตู้
肇收稿日期：００— ２—１２１００基金项目：国家自然科学基金资助项目（０３０５６８５０）作者简介：项凤涛（９６）男，１８一，硕士研究生，主要从事组合导航研究；王正志（９５）男，１４一，教授，博士生导师，主要从事自动控制，模式识别与图像处理和生物信息学研究。
Ｑ＝ｑ０＋ｑ：＋ｇ＋ｑｌ３
弦虽然消除了欧拉方程的奇异性，但为了减小正交误差不得不以计算时间为代价。随着飞行器导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节更加合理地描述载体的空间运动，四元数法
助矩阵法和力学中的刚体定点旋转的理论。通过矩阵法推导方向余弦表，刚体定点运动旋转的理论表明，而定点运动刚体的任何有限位移都可以用绕过定点的某一轴的
一
四元数的数学概念是１４８３年由哈密顿首先提出的，它是把三维空间和１四维空间联系起来，四维空间的性个用质和运算规则来研究三维空间中的刚体定点转动问题。设坐标系，为空间中的一个惯性参考坐标系，机体坐
四元数姿态微分方程的一种有效手段，精度较高，运其但
捷联惯性导航控制系统（ＩＳ是目前自主导航控制ＳＮ）
领域研究热点之一，该方法将陀螺仪和加速度计直接安装
在载体上，掉机电式导航平台，省利用导航计算机软件建立一个 “ 数学平台” 来代替机电平台实体。由于其结构简单，成本低廉，易于实现惯性测量单元（ＭＵ）Ｉ的小型化，且
到有效抑制，以为捷联式惯性导航技术的工程实践提供参考。可关键词：捷联惯导系统；四元数姿态微分方程；精细积分；龙格库塔法
中图分类号：Ｐ０Ｔ３１
文献标识码ｉＡ
文章编号：０６— ７７２１）５一１３一４１０００（０００ｏＯｏ小，所以常用于飞行器实时解算。四元数的另外一个优点是便于采用卡尔曼滤波实现四元数最优估计，也正是基于这个原因，四元数广泛应用于航天器的姿态控制系统。近年来，出现了很多有效求解四元数微分方程的方法，例如龙格库塔法、旋转矢量算法等。四阶龙格库塔算法是计算
算需要的数据，是捷联式惯导算法中的重要工作。
算量很大，并且对数据光滑性要求较高。旋转矢量算法是基于１７９１年Ｂｒ提出的旋转矢量理论及旋转矢量微分方ｏｚｔ
程，该算法能够对圆锥误差进行有效补偿。本文中把精
细积分法应用于四元数微分方程的求解，数值仿真结果表
明相同条件下它是进一步减小姿态解算误差的有效方法，为捷联式惯性导航技术的工程实践提供参考。
载体的姿态和航向体现了载体坐标系与导航坐标系
之间的方位关系，确定２个坐标系之间的方位关系需要借
１四元数微分方程及其龙格库塔解法
１１四元数姿态微分方程．
次转动来实现。目前，主要的研究方法有：欧拉法、向方
余弦法与四元数法。已有大量科研文献和工程实践表明，四元数法相对于其他两种方法的优越性，比如在大姿态角情况下，欧拉方程存在奇异点，又称作平衡环锁定；向余方
标系曰为空间中的一个运动坐标系，假设机体坐标系曰到惯性坐标系，的姿态变换四元数为 ‘
第３卷１
第５期
四川兵工学报
２１００年５月
【其他研究】
捷联系统四元数姿态解算的精细积分法
项凤涛，王正志，吴第曼，岳
（国防科学技术大学机电工程与自动化学院，长沙
达
４０７）１０３
摘要：了基于四元数的相关理论，研究把精细积分方法应用于求解四元数姿态微分方程当中，以典型圆锥运动并作为输入条件对该算法进行了仿真分析。相同仿真步长条件下的仿真结果表明，该方法在高动态角运动环境下的解算精度要优于四阶龙格库塔算法，由圆锥运动引起的俯仰角算法漂移误差相对于于四阶龙格库塔方法也得