第六章 捷联惯导1
捷联式惯性导航系统培训讲学
1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。
于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。
捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。
因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。
现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。
惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。
在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。
它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。
所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。
惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。
捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。
平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。
捷联惯导算法与组合导航原理讲义(20170220)
捷联惯导算法与组合导航原理讲义严恭敏，翁浚编著西北工业大学2016-9前言近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。
相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。
惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。
希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。
本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议，深表谢意：西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技有限公司：刘兴华；东南大学：童金武；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军；网友：Zoro；山东科技大学：王云鹏。
书中缺点和错误在所难免，望读者不吝批评指正。
作者2016年9月目录第1章概述 (6)1.1捷联惯导算法简介 (6)1.2 Kalman滤波与组合导航原理简介 (7)第2章捷联惯导姿态解算基础 (10)2.1反对称阵及其矩阵指数函数 (10)2.1.1 反对称阵 (10)2.1.2 反对称阵的矩阵指数函数 (12)2.2方向余弦阵与等效旋转矢量 (13)2.2.1 方向余弦阵 (13)2.2.2 等效旋转矢量 (14)2.3方向余弦阵微分方程及其求解 (17)2.3.1 方向余弦阵微分方程 (17)2.3.2 方向余弦阵微分方程的求解 (17)2.4姿态更新的四元数表示 (20)2.4.1 四元数的基本概念 (20)2.4.2 四元数微分方程 (23)2.4.3 四元数微分方程的求解 (25)2.5等效旋转矢量微分方程及其泰勒级数解 (26)2.5.1 等效旋转矢量微分方程 (26)2.5.2 等效旋转矢量微分方程的泰勒级数解 (29)2.6圆锥运动条件下的等效旋转矢量算法 (31)2.6.1 圆锥运动的描述 (31)2.6.2 圆锥误差补偿算法 (33)第3章地球形状与重力场基础 (40)3.1地球的形状描述 (40)3.2地球的正常重力场 (46)3.3地球重力场的球谐函数模型 (50)3.3.1 球谐函数的基本概念 (50)3.3.2 地球引力位函数 (58)3.3.3 重力位及重力计算 (63)第4章捷联惯导更新算法及误差分析 (69)4.1捷联惯导数值更新算法 (69)4.1.1 姿态更新算法 (69)4.1.2 速度更新算法 (70)4.1.3 位置更新算法 (76)4.2捷联惯导误差方程 (76)4.2.1惯性传感器测量误差 (76)4.2.2姿态误差方程 (78)4.2.3速度误差方程 (79)4.2.4位置误差方程 (79)4.2.5误差方程的整理 (80)4.3静基座误差特性分析 (82)4.3.1 静基座误差方程 (82)4.3.2 高度通道 (83)4.3.3 水平通道 (83)4.3.4 水平通道的简化 (88)4.3.5 水平通道误差方程的仿真 (90)第5章卡尔曼滤波基本理论 (92)5.1递推最小二乘法 (92)5.2 Kalman滤波方程的推导 (94)5.3连续时间随机系统的离散化与连续时间Kalman滤波 (101)5.4噪声相关条件下的Kalman滤波 (107)5.5序贯滤波 (111)5.6信息滤波与信息融合 (114)5.7平方根滤波 (116)5.8遗忘滤波 (124)5.9 Sage-Husa自适应滤波 (125)5.10最优平滑算法 (127)5.11非线性系统的EKF滤波、二阶滤波与迭代滤波 (130)5.12间接滤波与滤波校正 (136)5.13联邦滤波（待完善） (136)5.14滤波的稳定性与可观测度分析 (141)第6章初始对准及组合导航技术 (147)6.1捷联惯导粗对准 (147)6.1.1矢量定姿原理 (147)6.1.2解析粗对准方法 (149)6.1.3间接粗对准方法 (152)6.2捷联惯导精对准 (153)6.3惯性/卫星组合导航 (157)6.3.1空间杆臂误差 (157)6.3.2时间不同步误差 (158)6.3.3状态空间模型 (159)6.4车载惯性/里程仪组合导航 (159)6.4.1航位推算算法 (159)6.4.2航位推算误差分析 (161)6.4.3惯性/里程仪组合 (164)6.5低成本姿态航向参考系统（AHRS） (167)6.5.1简化的惯导算法及误差方程 (168)6.5.2地磁场测量及误差方程 (169)6.5.3低成本组合导航系统模型 (170)6.5.4低成本惯导的姿态初始化 (171)6.5.5捷联式地平仪的工作原理 (173)第7章捷联惯导与组合导航仿真 (176)7.1飞行轨迹和惯性器件信息仿真 (176)7.1.1飞行轨迹设计 (176)7.1.2 捷联惯导反演算法 (177)7.1.3 仿真 (178)7.2捷联惯导仿真 (180)7.2.1 Matlab子函数 (180)7.2.2捷联惯导仿真主程序 (185)7.3惯导/卫星组合导航仿真 (186)7.3.1Matlab子函数 (186)7.3.2组合导航仿真主程序 (187)附录 (190)A一些重要的三维矢量运算关系 (190)B 运载体姿态的欧拉角描述 (192)C 姿态更新的毕卡算法、龙格—库塔算法及精确数值解法 (199)D 从非直角坐标系到直角坐标系的矩阵变换 (207)E 线性系统基本理论 (211)F 加权最小二乘估计 (216)G 矩阵求逆引理 (217)H 几种矩阵分解方法（QR、Cholesky与UD） (219)I 二阶滤波中的引理证明 (223)J 方差阵上界的证明 (225)K 三阶非奇异方阵的奇异值分解 (226)L Matlab仿真程序 (231)M 练习题 (237)参考文献 (241)第1章概述第1章概述 (6)1.1捷联惯导算法简介 (6)1.2 Kalman滤波与组合导航原理简介 (7)1.1捷联惯导算法简介在捷联惯导系统（SINS）中惯性测量器件（陀螺和加速度计）直接与运载体固联，通过导航计算机采集惯性器件的输出信息并进行数值积分求解运载体的姿态、速度和位置等导航参数，这三组参数的求解过程即所谓的姿态更新算法、速度更新算法和位置更新算法。
捷联惯性导航系统的解算方法
R.
Yb'' Yb
Yb
H'
N
cos Psin
sin P cos P cos
R R
欧拉角微分方程
Zb Z b'' U ' PZ b
b nb
——表示载体坐标
系相对地理坐标系的角
速度矢量在载体坐标系
轴向的分量构成的列矩
阵。
H. R.
O
2010-03-19
P.
HPR
ER X'
X b'' b X
nbbx b nby
k
b b
b:载体坐标系 n:地理坐标系
b
同一个矢量r，如果写成地理坐标系轴向分量形式：
r X nin Yn jn Zn k r nT 由于r是同一个矢n量n ，故
X Y
n
rn
n
Zn
in
n
jn
kn
r bT b r nT n
b
C
b n
n
由于正交阵，故
rb
C
b n
r
n
r bT r
b n
ib (ib in )in (ib jn ) jn (ib kn )kn cos(ib in )
jb ( jb in )in ( jb jn ) jn ( jb kn )kn
kb (kb in )in (kb jn ) jn (kb kn )kn
2010-03-19
kn k
in
b
kb
n
kn
n kn
n ijn
kn
n
ib kn
j
bn
kb kn
捷联惯导系统
作业思考题
1、简要说明捷联惯导系统的基本组成和原理。 2、什么是数学平台？它有什么作用？
惯性导航系统
第四十四讲 捷联惯导系统 力学编排方程（一）
捷联式惯导系统（SINS）
加速度计
fb
数学平台
姿态矩阵 Cbp
f p 导航 速度、位置
计算机 姿态、航向
姿态矩阵计算
陀螺
ibb
pbb
b ip
姿态航向
-
C11 C21 C31
Cep 1 Cep T
C12 C13 1 C11 C21
C22
C23
C12
C22
C32 C33 C13 C23
C11 C22C33 C23C32 C21 C13C32 C12C33 C31 C12C23 C22C13
C31
C32
C33
位置矩阵微分方程组
Cep 0 f 0,0,0
1
p p epx epy
g g egx egy
R VeggxVeggy
VeppxVeppy
三、位置速率方程
11
p p epx epy
g g egx egy
RN RE
捷联惯导的发展
1、1950年起，德雷珀实验室捷联系统得到成熟的探索； 2、1969年，在“阿波罗-13”宇宙飞船，备份捷联惯导系统； 3、20世纪80~90年代，波音757/767、A310民机以及F-20战 斗机上使用激光陀螺惯导系统，精度达到1.85km/h的量级； 4、20世纪90年代，美国军用捷联式惯导系统已占有90% 。光 纤陀螺的捷联航姿系统已用于战斗机的机载武器系统中及波 音777飞机上。 5、国内由90年代挠性捷联惯导到现在激光捷联惯导、光纤陀 螺捷联航姿系统。
捷联惯导作业
一、原理分析：捷联式惯导系统是将惯性器件（陀螺仪和加速度计）直接固连在载体上的系统。
图1为捷联式惯导系统的原理图，陀螺仪和加速度计输出分别送入姿态矩阵计算和由载体坐标系至平台坐标系的方向余弦矩阵的计算。
有了姿态矩阵，其一可以实现把载体坐标系轴向加速度信息变换到导航坐标系轴，进而可以进行所需的导航参数计算，其二利用姿态矩阵的元素，提取方位和姿态信息。
图1. 捷联式惯导系统的原理图 姿态速率微分方程为:12b tbωΛ=Λ (1)其中；()b b b t ttb ib t ie et C ωωωω=-- (2)bib ω为陀螺仪测量经补偿后的值；0cos sin t iex tt ieiey ie t ie iez L L ωωωωωω⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦，为地球自转角速率； tan tety t yt etx t tt etx etety xtt etz tetxxt V R V R V L R ωωωω⎡⎤-⎢⎥⎢⎥⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎢⎥⎢⎥⎣⎦，为地理坐标系相对地球坐标系的转动角速率； 导航坐标系到载体坐标系的姿态矩阵为：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 tt C ψϕψθϕψϕψθϕθϕψθψθθψϕψθϕψϕψθϕθϕ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+-⎣⎦(3)对应的四元素初值为:0123cos cos cos sin sin sin 222222cos sin cos sin cos sin 222222cos cos sin sin sin cos 222222cossinsinsincoscos222222ψθϕψθϕλψθϕψθϕλψθϕψθϕλψθϕψθϕλ⎧=-⎪⎪⎪=-⎪⎨⎪=+⎪⎪⎪=+⎩(4)四元素姿态矩阵为：2222012312031302222212030123230122221302230101232()2()2()2()2()2()bt C λλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλ⎡⎤+++++⎢⎥=--+-+⎢⎥⎢⎥----+⎣⎦ (5)将姿态速率微分方程展开成矩阵形式：0112233001020bbbtbxtbytbx b bb tbx tbz tby b b btby tbztbx b bbtbz tbytbxλλωωωλλωωωωωωλλωωωλλ⎡⎤⎡⎤⎡⎤---⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦(6)该微分方程的解可用比卡逼近法求解，解得[]000s i n 2()c o s (0)2t I λλλλλλ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭ (7)其中：22202((()))b b b tbx tby tbz ωωωλ+∆+=，220I λλ∆=-∆将0cos2λ∆和00sin2λλ∆∆展成级数并提取有限项，得到姿态矩阵的算法为：[]2420001(1)(1)()()8384248n I n λλλλλλ⎧⎫∆∆∆+=-++-∆⎨⎬⎩⎭ (8)根据比力方程比力方程：f a G =-(2)t t tttti e e tV f V gωω=-+⨯+(9)写为分量形式：0(2)(2)0(2)0(2)0(2)(2)0t t tt t t tx xx iez etz iey ety t t t t t tt y y iez etz iex etx y t t t t t t t iey ety iex etx z z z V f V V f V g f V V ωωωωωωωωωωωω⎡⎤⎡⎤⎡⎤⎡⎤ -+ + ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=- +-++⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-+ +-⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦ (10)从而有：(2s i n )(2c o s )(2s i n )(2c o s )ttt tt txx x x ie yiezxtxt tt yt tt txy y ie xzxt ytttyt t ttxz x iex y xtytV V V f L tgL V L V R R V V V f L tgL V V R R V V V f L V V gR R ωωωω⎧=++-+⎪⎪⎪⎪=-++⎨⎪⎪⎪=+++-⎪⎩(11)其中：22Re(1sin ())Re(1(23sin ()))Rxt e L Ryt e L =+=-- (12)天向速度为零，高度通道保持不变，上式可写为：2(s i n )t a n2(s i n )t a nt tx y t t tx x ie y xt t tt t t x xy y ie x ytV V V f LV L R V V V f L V L R ωω⎧=++⎪⎪⎨⎪=--⎪⎩(13)将加速度对时间积分可以得到X 、Y 两个方向的速度：00y 0(2sin tan )(2sin tan )tt t t tx xie x x xttt tttxy ie y xtV V L L V dt V R V V L L V dt V R ωω=++=++⎰⎰(14)X 、Y 两个方向的速度除以对应的地球曲率半径，再次积分得到载体的经纬度位置：(0)(0)c o s tt yyttt xxt V L dt L R V dt R Lλλ=+=+⎰⎰(15)二、程序流程图：三、导航结果：不考虑天向速度及飞行高度的导航结果如下：1.位置坐标曲线：2.系统东向速度曲线：3.系统北向速度曲线：4.系统终点值：四、小结：1、对捷联式惯导系统有了一定的了解。
§3.9~3.10捷联式惯导系统
§3.9捷联式惯导系统概论一、概述“捷联”（strap down）这一术语的英文原意就是“捆绑”的意思，因此，所谓捷联系统就是将惯性测量装置的敏感器(陀螺仪与加速度计)直接捆绑在运载体上，从而可实现运动对象的自主导航目的。
平台式惯性导航系统虽然已经达到很高水平，但其造价高、使用十分昂贵。
计算机虽为数字式，但框架伺服系统一般仅采用模拟线路，所以相对来讲，可靠性差一些。
就在平台式惯性导航系统迅速发展的同时，捷联式惯性导航系统也处于研制过程中。
捷联式惯导方案是1956年提出的，当时由于没有满足捷联式系统历要求的惯性元件和计算机，因而没有被采用。
而平台式系统则不断改进、不断完善，达到了相当高的精度，满足了大多数任务的要求。
但是在可靠性和成本方面平台式系统都暴露出一系列严重问题。
与此同时计算技术取得了惊人的进展，克服了捷联式系统发展的一个主要障碍。
捷联式系统的高可靠性和低成本促使人们进—步对它进行新的技术探索。
上世纪六十年代初，美国联合飞机公司首先研制成功了第一个捷联式系统，于1969年成功地应用在阿波罗登月任务中。
捷联式惯性导航系统是将惯性敏感器(陀螺和加速度计)直接安装在运载体上，不再需要物理实现稳定平台的惯性导航系统。
陀螺仪作为角速率传感器而不是作为角位移传感器；加速度计的输入轴不是保持在已知确定方向上，加速度计测量值是运载体瞬时运动方向的加速度值。
通过计算机内的姿态矩阵实时计算而得到一个“数学解析平台”，它同样可以起到机电结合的稳定平台所提供的在惯性空间始终保持所要求的姿态作用。
捷联式惯性导航系统有以下几个主要优点：(1) 惯性敏感器便于安装、维修和更换。
(2) 惯性敏感器可以直接给出载体坐标系轴向的线加速度、线速度、供给载体稳定控制系统。
(3) 便于将惯性敏感器重复布置，从而易在惯性敏感器的级别上实现冗余技术，这对提高系统的性能和可靠性十分有利。
(4) 由于去掉了物理实现的平台，一则消除了稳定平台稳定过程中的各种误差；二则由于不存在机电结合的平台装置，使整个系统可以做得小而轻，并易于维护。
捷联式惯性导航系统
1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。
于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。
捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。
因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。
现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。
惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。
在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。
它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。
所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。
惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。
捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。
平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。
捷联惯导系统原理框图
2 Φ (h) θ1 θ2 θ1 θ2 3
• 算法思路不同； 等效旋转矢量法思路：
n n ( m) n b ( m) qb ( m) qn ( m1) qb ( m1) qb ( m1)
n in
b ib
捷联惯导系统
2.4 几种姿态算法的比较
4q1 q 0 T32 T23 4q 2 q 0 T13 T31 4 q q T T 21 12 3 0
sign(q1 ) sign(q 0 )[sign(T32 T23 )] sign(q 2 ) sign(q 0 )[sign(T13 T31 )] sign(q ) sign(q )[sign(T T )] 3 0 21 12
sin cos sin cos cos cos 0 sin 0 1 0
1
b nbx b nby b nbz
sin cos cos sin tan
数字递推形式：
n (l ) n(l 1) Cen(l ) Cn C (l 1) e
n n n (l ) en F (t )V n (t ) l en dt F R n C n ( l 1)
sin C en sin L cos cos L cos
L arcsin P33
0 sin L sin cos L cos L sin sin L
主 arctg
P32 P31
cos
捷联惯导系统
4. 捷联惯导系统误差方差