高精度捷联式惯性导航系统算法研究1. 引言随着计算机技术的发展，捷联式惯性导航系统（strapdown Inertial Navigation System, SINS）的概念被提出，它取消了平台式惯性导航系统中复杂的机械平台装置，而将惯性传感器直接固联在载体上。

SINS具有制造和维护成本低、体积小、重量轻以及可靠性高等优点，目前在高、中、低精度领域都得到了广泛使用。

捷联算法的基本框图如图1所示。

图1 捷联算法的基本框图在捷联惯性导航系统中，惯性传感器直接固联在载体上，因此对惯性传感器的性能提出了更高的要求。

SINS中使用的陀螺所承受的动态范围较大，一般能够达到100 /s，与此同时，SINS中的陀螺和加速度计与载体一起进行角运动和线运动，这增加了导航计算机输出数据的难度和复杂性。

姿态实时计算是捷联惯导的关键技术，也是影响捷联惯导系统导航精度的重要因素。

载体的姿态和航向是载体坐标系和地理坐标系之间的方位关系，两坐标系之间的方位关系等效于力学中的刚体定点转动问题。

在刚体定点转动理论中，描述动坐标系相对参考坐标系方位关系的方法有欧拉角法、四元数法、方向余弦法以及等效旋转矢量法。

本报告对这四种姿态算法进行简单介绍，并结合研究对象对等效旋转矢量算法进行重点研究。

针对角速率输入陀螺构成的捷联式惯性导航系统，本报告给出了一种改进的姿态算法，并在圆锥运动环境下对该算法进行数学仿真，验证了该方法的可能性。

2. 姿态算法介绍2.1 欧拉角法一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕三个不同轴转动三个角度进行确定。

把载体坐标系ox b y b z b 作为动坐标系，导航坐标系ox n y n z n （即地理坐标系）作为参考坐标系，导航系依次转过航向角H 、俯仰角P 、横摇角R 可得到载体坐标系，通过求解欧拉角微分方程得到三个欧拉角，从而进一步可以得到捷联姿态矩阵。

欧拉角微分方程如下所示：cos cos 0sin cos 1sin sin cos cos sin cos sin 0cos bnbx b nby b nbz P P PR P R P R P P P P H R R ωωω⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥=-⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦⎣⎦(1) 式(1)即为欧拉角微分方程，求解方程可以得到三个欧拉角，也就是航向角、俯仰角以及横摇角，根据三个姿态角和姿态矩阵元素之间的关系即可以得到姿态矩阵n b C 。

2.2 方向余弦法常用方向余弦姿态矩阵微分方程的形式为b bk bn nb n =C C ω (1)式中bknb ω为载体坐标系相对地理坐标系的转动角速度在载体坐标轴向的分量的反对称矩阵形式，具体表达式如式(2)。

000b bnbznby bkb b nbnbznbxb b nby nbxωωωωωω⎡⎤-⎢⎥=-⎢⎥⎢⎥-⎣⎦ω (2)用毕卡逼近法求解矩阵微分方程，其解为200200sin 1cos ()()()b bk bk n nb nb t t t θθθθ⎡⎤∆-∆+∆=+∆+∆⎢⎥∆∆⎣⎦C C I θθ(3) 式中100n nbbnbznby t bkbk b b nbnb nbznbxt b b nbynbxdt θθθθθθ+⎡⎤-∆∆⎢⎥∆==∆-∆⎢⎥⎢⎥-∆∆⎣⎦⎰θω 0θ∆=2.3 四元数法四元数微分方程的形式为1()()2bnb t t =Q Q ω (4)其中，Q (t )是姿态四元数，Tbnbx y z ωωω⎡⎤=⎣⎦ω，为b 系相对n 系的角速度。

求解四元数微分方程一般用计算机实现，常用方法有毕卡逼近增量法和数值积分算法，本报告关于四元数求解姿态角算法采用的都是数值积分算法，因此此处仅对四元数四阶-龙格库塔算法进行简单介绍。

对四元数微分方程12bnb =⋅Q Q ω (5)式中：0b bx by bznb nb b nb b nb b ωωω=+++i j k ω，为载体坐标系相对于导航坐标系的角速度矢量的四元数表达形式。

用四阶龙格库塔法求解微分方程的过程如下：()1234()()226ht h t +=+⋅+++Q Q k k k k (6)其中：11()()2bnb t t ⎡⎤=⋅⋅⎣⎦k Q ω, 121()222b nb h t h t ⎧⎫⎡⎤⎛⎫=⋅+⋅+⎨⎬ ⎪⎢⎥⎝⎭⎣⎦⎩⎭k k Q ω, 231()222b nb h t h t ⎧⎫⎡⎤⎛⎫=⋅+⋅+⎨⎬ ⎪⎢⎥⎝⎭⎣⎦⎩⎭k k Q ω, []{}431()()2bnb t h t h =⋅+⋅+k Q k ω 其中h 为姿态更新周期。

这种方法的实质就是在(t , t +h )时间间隔内求取多个斜率值，加权求平均得到更精确的平均斜率。

当采用四元数Q 来表示转动时，若起始时刻四元数Q 0取为单位四元数，则依据转动的四元数变换定理可知，Q 应恒为单位四元数，即应满足下面的约束方程：222201231q q q q +++=(7)但由于计算误差的存在，破坏了上面的约束条件，因此必须对Q 进行归一化处理。

采用下式实现四元数的最佳归一化：=Q (8)2.4 等效旋转矢量算法由于刚体运动的不可交换性误差，圆锥误差补偿算法是提高运载体姿态更新精度的一种有效途径。

1971年Bortz 和Jordon 提出了等效旋转矢量概念，将载体的姿态四元数更新转换为姿态变化四元数的更新，为姿态更新的多子样算法提供了理论依据。

1983年Miller 探讨了锥运动条件下等效旋转矢量的三子样优化算法，优化指标是圆锥误差影响达到最小。

在此基础上，Lee 和Yoon 研究了四子样算法，Jiang 研究了利用本更新周期内的三子样及前更新周期内的角增量计算旋转矢量的优化算法。

旋转矢量的微分方程是：()11()()()()()()()212t t t t t t t =+⨯+⨯⨯ΦΦΦΦωωω (9)式中，Φ(t )为旋转矢量，ω(t )为陀螺输出角速率。

由式(9)可以推导求得旋转矢量的表达式，如二子样、三子样等都是工程中常用的算法。

二子样算法表达式：12122=++3∆∆∆⨯∆θθθθΦ (10)算法漂移率： ()421960e a h φ=ΩΩ (11) 陀螺输出角速率时的二子样算法表达式：当陀螺输出为角速率时，旋转矢量法中的角增量可由以下公式提取1=()42T t t T ⎡⎤⎛⎫∆++⋅ ⎪⎢⎥⎝⎭⎣⎦θωω (12a )2()42T t t T T ⎡⎤⎛⎫∆=+++⋅ ⎪⎢⎥⎝⎭⎣⎦θωω (12b )()4()62T t t t T T ⎡⎤⎛⎫∆=+⋅+++⋅ ⎪⎢⎥⎝⎭⎣⎦θωωω (12c )算法漂移率： ()22132280e a h φ=ΩΩ (13) 三子样算法表达式：13231927=++()2040∆∆⨯∆∆⨯∆-∆θθθθθθΦ (14) 算法漂移率： ()621204120e a h φ=ΩΩ (15)当陀螺输出为角速率时，旋转矢量法中的角增量可由以下公式提取125()83633T T t t t T ⎡⎤⎛⎫⎛⎫∆=⋅+⋅+-+⋅ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎣⎦θωωω (16a )22()853633T T t t t T ⎡⎤⎛⎫⎛⎫∆=-+⋅++⋅+⋅ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎣⎦θωωω (16b )3285()3633T T t t t T T ⎡⎤⎛⎫⎛⎫∆=-++⋅++⋅+⋅ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎣⎦θωωω (16c ) 2()33()633T T t t t t T T ⎡⎤⎛⎫⎛⎫∆=+⋅++⋅+++⋅ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎣⎦θωωωω (16d ) 算法漂移率： ()4212592e a h φ=ΩΩ (17) 式中，Δθi ，i =1，2，3为姿态更新周期内第i 次陀螺采样值；a 为圆锥运动半锥角；Ω为圆锥运动角速率；h 为姿态更新周期。

3. 角速率输入的捷联惯导姿态算法研究与实现1971年，Bortz 提出了旋转矢量微分方程，为一种全新的姿态算法提供了理论基础，有效地解决了算法求解过程中存在的不可交换性误差；在此基础上，Miller 提出了二子样、三子样误差补偿算法，而Lee 在此基础上又提出了四子样算法；Jiang 提出了利用当前周期与前一周期陀螺仪采样信号进行误差补偿新算法，能够提高算法精度；Savage 对前人工作进行了总结，并就姿态更新算法的实现提出了一系列技巧，给出了完整的捷联惯导姿态算法编排和离散更新方法。

这些算法的实现都是基于陀螺仪的角增量信息展开的，当陀螺仪输出为角速率信号时，利用数值积分方法将角速率信息转换为角增量信息，然后直接应用传统圆锥误差补偿算法，算法精度会降低2个数量级，不能满足载体高动态环境下对姿态解算精度的要求。

因此有必要研究一种能够直接利用陀螺仪输出角速率信息进行姿态解算算法，从而进一步提高捷联惯导复杂工作环境下的姿态解算精度。

旋转矢量的微分方程近似为：12=+⨯ΦΦωω (18)对于传统的二子样算法来说，在一个计算周期T ~T +h 内需要三次陀螺采样值，采样时间h 内，载体的角速度用抛物线拟合为：2()23T τττ+=++a b c ω，0≤τ≤h (19)记角增量为()=()T d ττττ∆+⎰θω (20)则由式(19)和式(20)得0()()T T ττ==+=a ωω (21a ) 0()()2T T ττ==+=b ωω (21b ) 0()()6T T ττ==+=c ωω (21c )()()0()()3,4,5i i T T i ττ==+==⋅⋅⋅0, ωω (21d ) 0(0)()ττ=∆=∆=0θθ (22a )00(0)()(+)=T ττττ==∆=∆=a θθω (22b ) 00(0)()(+)=2T ττττ==∆=∆=b θθω (22c ) 00(0)()(+)=6T ττττ==∆=∆=c θθω (22d )()()(1)0(0)()(+)=,4,5,6i i i T i ττττ-==∆=∆==⋅⋅⋅0θθω (22e )又由于姿态更新周期h 一般为毫秒级的量，Φ也可视为小量，这样有式(23)成立()()ττ≈∆θΦ (23)这样式(18)可写成1()()()()2T T ττττ=++∆⨯+ωθωΦ (24)对式(24)求各阶导数，并考虑到式(21d )和式(22e )11()()()()()()22T T T ττττττ=++∆⨯++∆⨯+ωθωθωΦ (25a )11()()()()()()()()22T T T T ττττττττ=++∆⨯++∆⨯++∆⨯+ωθωθωθωΦ (25b )(4)133()=()()()()()()222T T T τττττττ∆⨯++∆⨯++∆⨯+θωθωθωΦ (25c )(5)()=2()()3()()T T τττττ∆⨯++∆⨯+θωθωΦ (25d )(6)()=5()()T τττ∆⨯+θωΦ (25e ) ()()=,7,8,9,i i τ=⋅⋅⋅0Φ (25f )用τ=0代入式(25)中，并应用式(21)和式(22)，得(4)(5)(6)()(0)1(0)2+=221(0)62+2=62133(0)6+22+6=6222(0)262+326=12(0)566=(0),7,8,9,i i ==⨯=+⋅⨯⨯+⨯=⋅⨯⋅⨯⋅⨯⨯=⋅⨯⋅⨯⨯=⋅⨯==⋅⋅⋅00ΦΦΦΦΦΦΦab a a bc b a a b c a bc a b b a c a c c b b c b c c c (26)将Φ(h )用泰勒级数展开，得：()()()()()()()234(5)(4)(5)00000+02624120h h h h h h =+++++⋅⋅⋅ΦΦΦΦΦΦΦ (27)将式(26)代入式(27)得：23345111()6410h h h h h h h =+++⨯+⨯+⨯Φa b c a b a c b c (28)设陀螺在t =T ，t =T +h /2，t =T +h 的角速率分别为：ω1、ω2、ω3，可用陀螺角速率估计a 、b 、c 的大小如下：()()112312321342223h h ⎧⎪⎪⎪⎨⎪⎪⎪⎩==-+-=-+a b c ωωωωωωω (29) 将式(29)代入式(28)并考虑陀螺的角增量输出，则旋转矢量可用下式估计：()2213231Xh Yh =∆+⨯+⨯-Φθωωωωω (30)式中X =1/60，Y =7/90。