P09-捷联惯导系统-姿态算法(矩阵)
捷联惯导初始对准以及姿态解算
第三部分：基于“存储数据与迭代计算对准”罗经法对准
3.2 罗经法对准过程中的调整策略（以北向通道为例）
g
y
f
p N
1
VN
1
s
R
-
K1
K2 R
K3 s
Control algorithm
cE -
x
1
s
x
-
ie cos L
z
实线所示的北向通道：本质上是一个休拉回路，失准角作无阻尼振荡。
采取的策略：1）引入内反馈环节（虚线）实现衰减振荡；2）引入前馈环节（点画 线）缩短振荡周期；3）引入积分环节（双点画线）消除罗经项的影响。
3.5 SINS罗经法对准如何实现迭代计算？
fˆNn -
b ib
fb cU
Cˆbn
Cˆbn
b ib
Cnbine
Cnbc
cN
fˆ n Cˆbn f b
1
VN
s
1
cE
R
K1
K2 R
K3 s
Control algorithm
上述过程中，可以实现迭代计算。
Page 15
第三部分：基于“存储数据与迭代计算对准”罗经法对准
导航坐标系 n （b）SINS
GINS中的测量数据直接反映失准角的大小； SINS中的测量数据不直接反映失准角；只有投影数据能够反映失准角的大小；相同 的测量数据经过不同的姿态矩阵进行投影，可以获取不同的投影数据。 注：上述均不考虑仪表误差。
对于SINS而言，分析一种理想的情况：仪表无误差，载体无机动，此时在整个对准 过程中，仪表测量数据均相等。整个对准过程，其实只用了一组仪表参数。
3.6 SINS罗经法对准中存储数据如何使用？
捷联式惯性导航系统
1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。
于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。
捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。
因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。
现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。
惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。
在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。
它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。
所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。
惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。
捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。
平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。
惯性导航P09_SINS_Intro_and_Algorithms_(DCM)
Lecture 9 -- Algorithms for SINS
3
1.2 Mechanism of SINS
AX specific forces
AY in body frame AZ
GX GY GZ
inertial devices
angular rates in body frame
transformation of specific forces
C
G B
C21
C22
C23
j
i'
j j'
j k'
C31 C32 C33 k i' k j' k k'
Rotation
of
vehicle
causes
change
of
C
G B
k'
B
j' i'
j k
i
RG
Lecture 9 -- Algorithms for SINS
7
2.3 Changing rate of DCM
Strapdown Inertial Navigation System
Introduction and Algorithms
捷联惯导系统——介绍及算法（DCM）
Lecture 9 -- Algorithms for SINS
1
Outline - Strapdown INS
Introduction to SINS Deriving differential equations for DCM Solution: angular increment algorithm（角增量算法）
捷联惯导系统中四元素法求解姿态角仿真模拟
捷联惯导系统中四元素法求解姿态角仿真模拟
捷联惯导系统中四元素法求解姿态角仿真模拟
在捷联惯导系统中,姿态矩阵的'四元素微分方程求解一般采用四阶龙格库塔法,从运算精度与速度上考虑,提出了另一种有效的四阶泰勒展开法.并在典型圆锥运动环境下,对两种算法进行了姿态角误差仿真.通过仿真分析,四阶泰勒展开法的计算精度比四阶龙格库塔法高出1～2个数量级,为改进捷联惯导系统姿态算法提供了理论参考依据.
作者：孙冬梅田增山韩令军 SUN Dongmei TIAN Zengshan HAN Lingjun 作者单位：重庆邮电大学移动通信技术重点实验室,重庆,400065 刊名：弹箭与制导学报 PKU 英文刊名： JOURNAL OF PROJECTILES, ROCKETS, MISSILES AND GUIDANCE 年，卷(期)：2009 29(1) 分类号：V448.22 关键词：四元素四阶龙格库塔法四阶泰勒展开法姿态角。
捷联惯导系统算法.ppt
cos
b Ebz
注意事项：当 θ= 90 度时，方程出现奇点
姿态计算 矩阵方程精确解1
二、方向余弦矩阵微分方程及其解 C C
其中
C bE
CbE
b Eb
0
b Eb
z
z
0
y
x
y x
0
由于陀螺仪直接测得的是载体 相对惯性空间的角速度，所以：
CbE
b ib
E iE
C
E b
或四元数微分方程：
q(t)
(
b ib
b iE
)q(t)
注意事项： 1、上述两个方程中的角速度表达式不一样 2、方程第二项较小，计算时速度可以低一些
增量算法 矩阵方程精确解
一、角增量算法
角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量
一个采样周期内，陀螺输出脉冲数对应的角增量为：
C
0
0
c os
0 0 0 sin
sin
sin
c os
cos cos
求解欧拉角速率得
1 0
0
cos
0 sin
惯性器件的误差补偿
姿态计算 欧拉角微分方程1
姿态矩阵的计算 假设数学坐标系模拟地理坐标系 飞行器姿态的描述：
航向角ψ、俯仰角θ、滚动角γ 一、欧拉微分方程
从地理坐标系到载体坐标系 的旋转顺序：
Ψ →θ →γ
捷联惯性导航系统的解算方法课件
02
CATALOGUE
捷联惯性导航系统组成及工作 原理
主要组成部分介绍
惯性测量单元
包括加速度计和陀螺仪，用于测量载体在三个正交轴上的加速度 和角速度。
导航计算机
用于处理惯性测量单元的测量数据，解算出载体的姿态、速度和 位置信息。
控制与显示单元
用于实现人机交互，包括设置导航参数、显示导航信息等。
工作原理简述
学生自我评价报告
知识掌握情况
学生对捷联惯性导航系统的基本原理、解算 方法和实现技术有了深入的理解和掌握。
实践能力提升
通过实验和仿真，学生的动手实践能力得到了提升 ，能够独立完成相关的实验和仿真验证。
团队协作能力
在课程项目中，学生之间的团队协作能力得 到了锻炼和提升，能够相互协作完成项目任 务。
对未来发展趋势的预测和建议
捷联惯性导航系统的解算 方法课件
CATALOGUE
目 录
• 捷联惯性导航系统概述 • 捷联惯性导航系统组成及工作原理 • 捷联惯性导航系统解算方法 • 误差分析及补偿策略 • 实验验证与结果展示 • 总结与展望
01
CATALOGUE
捷联惯性导航系统概述
定义与基本原理
定义
捷联惯性导航系统是一种基于惯性测量元件（加速度计和陀螺仪）来测量载体（如飞机、导弹等）的加速度和角 速度，并通过积分运算得到载体位置、速度和姿态信息的自主导航系统。
01
高精度、高可靠性
02
多传感器融合技术
随着科技的发展和应用需求的提高， 捷联惯性导航系统需要进一步提高精 度和可靠性，以满足更高层次的应用 需求。
为了克服单一传感器的局限性，可以 采用多传感器融合技术，将捷联惯性 导航系统与其他传感器进行融合，提 高导航系统的性能和鲁棒性。
基于四元数法的捷联式惯性导航系统的姿态解算
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
பைடு நூலகம்
捷联惯导详细讲解
捷联惯导系统从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种方法：方向余弦法、欧拉-克雷罗夫角法以及四元数法。
捷联惯导系统航姿算法的比较及仿真分析
ｌ ｇ０２＋ｑ 一ｑ；一ｑ； ２（ｑ１ｇ２一ｇｏｇ３） ２（ｇＩｑ２＋ｑｏｑ３）Ｉ ｃ ＝Ｉ ２（ｇ。ｑ２＋ｑｏｑ３） ｇ 一ｑ ＋ｇ；一ｇ； ２（ｇ１ｑ２一ｑｏｑ３）ｌ
ｌ ２（ｇｌｇ３一ｇｏｇ２） ２（ｇ２ｇ３＋ｇ０ｇ１） ｇ —ｇ 一ｇ；＋ｇ Ｉ
（２）
利 用式 （２）便 可 以从 由载 体 坐标 系 转 移 至导 航 地 理 坐 标 系 的姿 态 转 移 矩 阵 中提 取 实 时姿 态 角 供导 航 系统使 用 。 １．２ 四阶龙 格一 库塔 法 求解 四 元数微 分 方程
Ｑ（ｔ＋ ）＝Ｑ（ｆ）＋（ ＋ｋ２＋ｋ３十 ）／６ ｋｌ＝０．５ｈ［ｃｏ（ｔ）］Ｑ（ｔ、 ｋ２＝０．５ｈ［ａ￣（ｔ＋ｈ／２）］［Ｑ（ｆ）＋ｋｉ／２］ （４） ｋ３＝０．５ｈ［ｃｏ（ｔ＋ｈ／２）］［Ｑ（ｆ）＋ｋ２／２］ ｋ４＝０．５ｈ［ｃｏ（ｔ＋ ）儿Ｑ（ ）＋ｋ３］
收稿 日期：２０１６．０１．０５。王小峰 （１９８３一 ）：陕西人，工程 师 ，工 学硕 士，研究方向：惯性及卫星导航 。
矢 量 三子样 法 进行 仿真 分析 比较 ，在典 型 圆锥运 动
下 ，四元 数 算法利 用 四阶 龙格 一库 塔法 对 四元数 微
分 方程 进 行姿 态求 解 ，而 旋转 矢量 三子 样算 法主 要
周 期一 般 都很短 ， 很 小 ， 的高 次项可 略 去不计 ， 得 工程 上 常用 的近 似方 程 ：
＝∞＋二２ ×∞＋ ×（ ×∞） （６）
四元数 微分 方程 为 ：
１
．
ａ＝去Ｑ
（３）
其 中， 表示 由机体 坐 标 系到 导 航坐 标 系 在机 体 坐 标 系 中 的投 影 。式 （３） 的求 解 可 以通 过 用 四阶 龙 格一 库塔 法来 求 取如 下 的 四元 数 递推 公式 ：
捷联惯性导航系统的解算方法
CC
RP
0 0
H.
C
R
P0. 0
R0.
0
2010-03-19
整理课件
Yb'' Yb Yb' H
N
17
b nbx b nby
cos
0
R
b nbz
sin R
P. cos R 0
R.
0
1
H. sin R 0
0 sin R cos P P.
1
sin P R.
kb jn
ib j
kn k
b n kb kn
2010-03-19
21
整理课件
固定矢量的坐标变换
矢量的坐标变换
固定矢量的坐标变换是一个在空间大小和方向都不 变的矢量在两个不同方位的坐标系轴向分量之间的变 换关系，也即同一个矢量在两个不同的坐标系轴向投 影之间的变换关系。
旋转矢量的坐标变换
是指一个矢量大小不变，但在方向上转动了一个位 置，这个矢量转动前和转动后在同一个坐标系轴向 分量之间的变换关系。
2010-03-19
22
整理课件
固定矢量的坐标变换
一个矢量r，写成载体坐标系轴向分量形式：
r X bib Yb jb Z b kb r bT b
rb
X Y
b
b
ib j
b
Zb
kbb
b:载体坐标系 n:地理坐标系
同一个矢量r，如果写成地理坐标系轴向分量形式：
r X nin Yn jn Z n k n r nT n 由于r是同一个矢量，故
惯性导航系统原理
1
整理课件
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个一阶微分方程式，计算量大。
捷联惯性导航系统的姿态算法
收稿日期 : 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 的定义如下 :
捷联惯导系统姿态算法比较
捷联惯导系统姿态算法比较
捷联惯导系统姿态算法比较
姿态算法是捷联惯导系统算法中的一个重要组成部分,解算姿态阵相当于建立起数学平台,其精度对捷联惯导系统的精度影响很大.该文就实际应用,对欧拉角法、方向余弦法、四元数算法、罗德利格参数法、优化旋转矢量算法及一种改进的递推旋转矢量算法做了分析,并在典型圆锥运动输入下,对后五种算法进行了仿真,为姿态算法的研究提供了参考.
作者：孙丽秦永元 SUN Li QIN Yong-yuan 作者单位：西北工业大学自动化学院,西安,710072 刊名：中国惯性技术学报ISTIC PKU 英文刊名：JOURNAL OF CHINESE INERTIAL TECHNOLOGY 年，卷(期)： 2006 14(3) 分类号： U666.1 关键词：捷联姿态算法精度圆锥运动比较。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
可记为: 近似计算:
order S C
G0 G0 CB (T ) CB (0) I S C 2
1 1
2 1
3
4
0
1 2
02 1 6 1 2
02 1 6 1 02 2 24
Lecture 10 -- Algorithms for SINS
j
k
i
R
载体的旋转导致 C 发生变化
S1
Lecture 10 -- Algorithms for SINS
8
2.2 方向余弦矩阵的导数
dj' dk' di' i dt i dt i dt i i' i j ' i k ' di' dj ' dk ' C j i ' j j ' j k ' j so C j j dt dt dt k i ' k j ' k k ' k di' k dj' k dk' dt dt dt 设载体坐标系相对于导航坐标系的角速度为 X Y Z T 表示在载体坐标系中 i' j' k ' di ' 则 i' x y z ( j ' z k ' y ) dt 1 0 0
Lecture 10 -- Algorithms for SINS
10
Outline
捷联惯导系统概述
姿态矩阵（DCM）微分方程的推导
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
11
3.1 方向余弦矩阵微分方程
G C G B C B B GB
C I
1 C I 2 2 ………..
许多类型的陀螺仪可用内部 硬件完成对角速率的积分， 从而直接输出角增量.
Lecture 10 -- Algorithms for SINS
14
3.6 地理坐标系的修正
G0 G0 CB (T ) CB (0)C
G0 G0 B0 CB C B0 CBT T
C C
R
也可用欧拉角、四元数等表示姿态
Lecture 10 -- Algorithms for SINS
4
1.3 SINS的示意框图
AX AY AZ
姿态信息 沿载体轴的 加速度输出 对加速度分量 进行坐标变换 沿地理坐标 系各轴的加 速度 导 航 计 算 位置 速度 显 示
GX GY GZ
C I I TG IG
0
N
GT
T
G0 1 G0 B0 GT CB ( C GT ) CB0 CBT T
Lecture 10 -- Algorithms for SINS 15
3.7 算例
某捷联惯导系统在 n 时刻, 其载体坐标系和惯性坐标系重合. 然 后从时刻 n 到时刻 n+1, 沿着载体三个轴的三个陀螺仪 X, Y, Z 的角增量输出分别为 0.002, 0.004 和 0.006 (rad); 请利用基于 Peano-Paker 解的一阶角增量算法计算时刻 n+1 载体和惯性坐 标系之间的方向余弦矩阵.
0.006 0.004 0.006 0.004 1 1 0 0 1 0.006 0.006 1 0 . 002 0 1 0 1 0 . 002 1 1 0.004 0.002 0 0 1 0.004 0.002
C(n) I3
0 z y z 0 x y 0.006 0.004 0 x 0.006 0 0 . 002 0 0 0.004 0.002
C(n 1) C(n)I 3
类似的
dj ' (k ' x i ' z ) dt
dk ' (i ' y j ' x ) dt
Lecture 10 -- Algorithms for SINS
9
2.3 方向余弦矩阵微分方程
则
i ( j ' z k ' y ) i (k ' x i ' z ) i (i ' y j ' x ) j ( j ' k ' ) j (k ' i ' ) j (i ' j ' ) C z y x z y x k ( j ' z k ' y ) k (k ' x i ' z ) k (i ' y j ' x )
C13 0 C 23 z C 33 y
x
y x C
0
C11 y C12 x C 21 y C 22 x C 31 y C 32 x
-- 斜
对称矩阵
C --- 其解取决于 C(0) 和载体的转动角速度的变化规律 C
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
2
1.1*捷联惯导系统: 特点
陀螺和加速度计被直接“捆绑 (strapped)”到载体上 没有物理的平台 对陀螺仪的角速度输出进行 积分，获取载体的姿态信息.
滚动轴
E
方位轴
俯仰轴
N
加速度计的输出需要变换到导航坐标系中. 对导航坐标系中的加速度分量进行补偿并积分，获取载体的速 度和位置信息.
19
6
2.1 方向余弦矩阵
z, 方位
C C
S2
x, 滚动 y, 俯仰
咋来的？ 设 S1 为导航坐标系，其单 位坐标矢量为 i, j 和 k S2 为载体坐标系, 其单位坐标矢量 为 i’, j’ 和 k’
N
E
R
S1
Lecture 10 -- Algorithms for SINS
7
2.1 方向余弦矩阵
12
3.2*毕-卡解
IB ( t ) dt G0 0 C (T ) C (0) e CB (0) e G0 B G0 B
B T
sin 0 1 cos 0 2 C (0) I 2 0 0 ------ 毕-卡形式的解
记初始时刻的地理坐标系为 G0 , 则有 G0 C G0 B C B B IB
记
B IB dt
0
T
其中
0 B IB z y z 0
0 z y
T 0
z 0 x
y x 0
Lecture 10 -- Algorithms for SINS
3
1.2 姿态变换
z, 方位轴
加速度需要变换:
fE f bx f f C N by f f bz
x, 滚动轴 y, 俯仰轴
e
N
E
C 包含姿态信息.
x
G0 B
y x
0
T
where i i dt
(角增量)
i x, y, z
and denote
B IB ( t ) dt
则
C (T ) C (0) e 0
G0 B
0 x y z
2 2
2
(向量的模)
Lecture 10 -- Algorithms for SINS
惯性元件
对地理坐 标系进行
修正 姿态
沿载体轴的 角速率输出
计算载体的 姿态信息 数学平台 计算机
Lecture 10 -- Algorithms for SINS
5
Outline
捷联惯导系统概述
姿态矩阵（DCM）微分方程的推导
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
G IG
VN R V E e cos R VE t an sin e R
B0
C
B0 BT
BT
C
G0 C G0 G C G G IG
G0 B0
ζ
GT CB T
C
G0 GT
C C
G0 G0
G0
E
G0 CG T
C 为从 S2 到 S1 的方向余弦 矩阵，即:
S2
k'
j'
AS1 CAS 2
其中 C11 C C 21 C 31
i'
C12 C 22 C 32 C13 i i' i j ' i k ' j i ' j j ' j k ' C 23 C 33 k i ' k j ' k k '
13
3.3*角增量算法
order S C
1 1 2 1
3
4
0
1 2
02 1 6 1 2
02 1 6 1 02 2 24
方程的解 各阶近似: