P09-捷联惯导系统-姿态算法(矩阵)

1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。

于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。

捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。

因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。

现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。

惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。

在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。

它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。

所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。

惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。

捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。

平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。

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

捷联惯导系统从20世纪60年代初开始发展起来，在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。

捷联式惯性导航(strap-downinertialnavigation)，捷联（strap-down）的英语原义是“捆绑”的意思。

因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在导弹需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。

一、捷联惯导系统工作原理及特点惯导系统基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，之后将其变换到导航坐标系，得到在导航坐标系中的速度、偏航角和位置信息等。

捷联惯导系统（SINS）是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。

由于惯性元器件有固定漂移率，会造成导航误差，因此导弹通常采用指令、GPS或其组合等方式对惯导进行定时修正，以获取持续准确的位置参数。

如采用指令+捷联式惯导捷联惯导系统能精确提供载体的姿态、地速、经纬度等导航参数，是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。

在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。

它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。

所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点。

除此以外捷联惯导系统的最大特点是没有实体平台，即将陀螺仪和加速度计直接安装在机动载体上，在计算机中实时的计算姿态矩阵，通过姿态矩阵把导航加速度计测量的载体沿机体坐标系轴向的加速度信息变换到导航坐标系，然后进行导航计算。

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种方法：方向余弦法、欧拉-克雷罗夫角法以及四元数法。

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

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

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

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

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

专门用语：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项与 12AA q 的互为相反数1q =单位四元数，它的第1项为1，其余3项为0V =无具体坐标系定义的矢量A V =列向量，它的各项元素等于矢量V 在坐标系A 的各轴上的投影A V ⨯（） =向量A V 的反对称（或交叉积）形式，代表如下矩阵：00ZA 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.概论惯性导航是通过对速度积分得到位置并对总加速度积分得到速度的过程。

捷联式惯性导航积分算法设计上篇：姿态算法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.概论惯性导航是通过对速度积分得到位置并对总加速度积分得到速度的过程。

1、方向余弦表cos cos sin sin sin sin cos cos sin sin cos sin sin cos cos cos sin cos sin sin sin cos sin sin cos sin cos cos cos C ψϕψθϕψϕψθϕθϕψθψθθψϕψθϕψϕψθϕθϕ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+-⎣⎦（1.0.1）X E Y C N Z ζ⎡⎤⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（1.0.2） 在列写惯导方程需要采用方向余弦表，因为错误！未找到引用源。

α较小，经常采用两个假设，即：cos 1sin 1αα≈≈ （1.0.3）式中 α-两坐标系间每次相对转动的角度。

由于在工程实践中可以使其保持很小，所以进一步可以忽略如下形式二阶小量，即：sin sin 0αβ≈ （1.0.4）式中β-两坐标系间每次相对转动的角度。

可以将C 近似写为：111C ψϕψθϕθ-⎡⎤⎢⎥=-⎢⎥⎢⎥-⎣⎦（1.0.5） 2、用四元素表示坐标变换对于四元素123q p i p j p k λ=+++，可以表示为如下形式cossincos sincos sincos 2222q i j k θθθθαβγ=+++ （2.0.1）式（2.0.1）的四元数称为特殊四元数，它的范数1q =。

1'R q Rq -= （2.0.2）式中''''R xi yj zk R x i y j z k=++=++ （2.0.3）将q 和1q -的表达式及式（2.0.3）带入（2.0.2），然后用矩阵表示为：()()()()()()()()()22221231231322222123213231222213223131222''22'22p p p p p p p p p x x y p p pp p p p p p yz z p p p p p p p p p λλλλλλλλλ⎡⎤+--+-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=-+--+⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎢⎥+-+--⎣⎦（2.0.4）由四元素到方向余弦表的建立123cos cos22sin cos22sin sin22cos sin22p p p θψϕλθψϕθψϕθψϕ-=-=-=+= （2.0.5） 将式（2.0.5）带入式（2.0.4），有cos cos sin cos sin cos sin sin cos cos sin sin sin cos cos cos sin sin sin cos cos cos cos sin sin sin sin cos cos C ϕψϕθψϕψϕθψϕθϕψϕθψϕψϕθψϕθθψθψθ-+⎡⎤⎢⎥=---+⎢⎥⎢⎥-⎣⎦（2.0.6）3、四元数转动公式的进一步说明采用方向余弦矩阵描述飞行器姿态运动时，需要积分姿态矩阵微分方程式，即C C =Ω （3.0.1）式中 C -动坐标系相对参考坐标系的方向余弦阵Ω-动坐标系相对参考坐标系角速度ω的反对称矩阵表达式 其中C 为公式（1.0.5）提供000z y zx y xωωωωωω⎡⎤-⎢⎥Ω=-⎢⎥⎢⎥-⎣⎦（3.0.2）采用（3.0.1）计算需要列写9个一阶微分方程式，计算量大。

捷联惯导系统姿态算法比较
捷联惯导系统姿态算法比较
姿态算法是捷联惯导系统算法中的一个重要组成部分,解算姿态阵相当于建立起数学平台,其精度对捷联惯导系统的精度影响很大.该文就实际应用,对欧拉角法、方向余弦法、四元数算法、罗德利格参数法、优化旋转矢量算法及一种改进的递推旋转矢量算法做了分析,并在典型圆锥运动输入下,对后五种算法进行了仿真,为姿态算法的研究提供了参考.
作者：孙丽秦永元 SUN Li QIN Yong-yuan 作者单位：西北工业大学自动化学院,西安,710072 刊名：中国惯性技术学报ISTIC PKU 英文刊名：JOURNAL OF CHINESE INERTIAL TECHNOLOGY 年，卷(期)： 2006 14(3) 分类号： U666.1 关键词：捷联姿态算法精度圆锥运动比较。