高精度捷联式惯性导航系统算法研究大学论文.docx

1、高精度捷联式惯性导航系统算法研究大学论文高精度捷联式惯性导航系统算法研究1. 引言随着计算机技术的发展，捷联式惯性导航系统（strapdown Inertial Navigation System, SINS）的概念被提出，它取消了平台式惯性导航系统中复杂的机械平台装置，而将惯性传感器直接固联在载体上。SINS具有制造和维护成本低、体积小、重量轻以及可靠性高等优点，目前在高、中、低精度领域都得到了广泛使用。捷联算法的基本框图如图1所示。图1 捷联算法的基本框图在捷联惯性导航系统中，惯性传感器直接固联在载体上，因此对惯性传感器的性能提出了更高的要求。SINS中使用的陀螺所承受的动态范围较大，一般

2、能够达到100/s，与此同时，SINS中的陀螺和加速度计与载体一起进行角运动和线运动，这增加了导航计算机输出数据的难度和复杂性。姿态实时计算是捷联惯导的关键技术，也是影响捷联惯导系统导航精度的重要因素。载体的姿态和航向是载体坐标系和地理坐标系之间的方位关系，两坐标系之间的方位关系等效于力学中的刚体定点转动问题。在刚体定点转动理论中，描述动坐标系相对参考坐标系方位关系的方法有欧拉角法、四元数法、方向余弦法以及等效旋转矢量法。本报告对这四种姿态算法进行简单介绍，并结合研究对象对等效旋转矢量算法进行重点研究。针对角速率输入陀螺构成的捷联式惯性导航系统，本报告给出了一种改进的姿态算法，并在圆锥运动环境

3、下对该算法进行数学仿真，验证了该方法的可能性。2. 姿态算法介绍2.1 欧拉角法一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕三个不同轴转动三个角度进行确定。把载体坐标系oxbybzb作为动坐标系，导航坐标系oxnynzn（即地理坐标系）作为参考坐标系，导航系依次转过航向角H、俯仰角P、横摇角R可得到载体坐标系，通过求解欧拉角微分方程得到三个欧拉角，从而进一步可以得到捷联姿态矩阵。欧拉角微分方程如下所示： (1)式(1)即为欧拉角微分方程，求解方程可以得到三个欧拉角，也就是航向角、俯仰角以及横摇角，根据三个姿态角和姿态矩阵元素之间的关系即可以得到姿态矩阵。2.2 方向余弦法常用方向余

4、弦姿态矩阵微分方程的形式为 (1)式中为载体坐标系相对地理坐标系的转动角速度在载体坐标轴向的分量的反对称矩阵形式，具体表达式如式(2)。 (2)用毕卡逼近法求解矩阵微分方程，其解为 (3)式中2.3 四元数法四元数微分方程的形式为 (4)其中，Q(t)是姿态四元数，为b系相对n系的角速度。求解四元数微分方程一般用计算机实现，常用方法有毕卡逼近增量法和数值积分算法，本报告关于四元数求解姿态角算法采用的都是数值积分算法，因此此处仅对四元数四阶-龙格库塔算法进行简单介绍。对四元数微分方程 (5)式中：，为载体坐标系相对于导航坐标系的角速度矢量的四元数表达形式。用四阶龙格库塔法求解微分方程的过程如下：

5、 (6)其中：, , 其中h为姿态更新周期。这种方法的实质就是在(t, t+h)时间间隔内求取多个斜率值，加权求平均得到更精确的平均斜率。当采用四元数Q来表示转动时，若起始时刻四元数Q0取为单位四元数，则依据转动的四元数变换定理可知，Q应恒为单位四元数，即应满足下面的约束方程： (7)但由于计算误差的存在，破坏了上面的约束条件，因此必须对Q进行归一化处理。采用下式实现四元数的最佳归一化： (8)2.4 等效旋转矢量算法由于刚体运动的不可交换性误差，圆锥误差补偿算法是提高运载体姿态更新精度的一种有效途径。1971年Bortz和Jordon提出了等效旋转矢量概念，将载体的姿态四元数更新转换为姿态变

6、化四元数的更新，为姿态更新的多子样算法提供了理论依据。1983年Miller探讨了锥运动条件下等效旋转矢量的三子样优化算法，优化指标是圆锥误差影响达到最小。在此基础上，Lee和Yoon研究了四子样算法，Jiang研究了利用本更新周期内的三子样及前更新周期内的角增量计算旋转矢量的优化算法。旋转矢量的微分方程是： (9)式中，(t)为旋转矢量，(t)为陀螺输出角速率。由式(9)可以推导求得旋转矢量的表达式，如二子样、三子样等都是工程中常用的算法。二子样算法表达式： (10)算法漂移率： (11)陀螺输出角速率时的二子样算法表达式：当陀螺输出为角速率时，旋转矢量法中的角增量可由以下公式提取 (12a

7、) (12b) (12c)算法漂移率： (13)三子样算法表达式： (14)算法漂移率： (15)当陀螺输出为角速率时，旋转矢量法中的角增量可由以下公式提取 (16a) (16b) (16c) (16d)算法漂移率： (17)式中，i，i=1，2，3为姿态更新周期内第i次陀螺采样值；a为圆锥运动半锥角；为圆锥运动角速率；h为姿态更新周期。3. 角速率输入的捷联惯导姿态算法研究与实现1971年，Bortz提出了旋转矢量微分方程，为一种全新的姿态算法提供了理论基础，有效地解决了算法求解过程中存在的不可交换性误差；在此基础上，Miller提出了二子样、三子样误差补偿算法，而Lee在此基础上又提出了四

8、子样算法；Jiang提出了利用当前周期与前一周期陀螺仪采样信号进行误差补偿新算法，能够提高算法精度；Savage对前人工作进行了总结，并就姿态更新算法的实现提出了一系列技巧，给出了完整的捷联惯导姿态算法编排和离散更新方法。这些算法的实现都是基于陀螺仪的角增量信息展开的，当陀螺仪输出为角速率信号时，利用数值积分方法将角速率信息转换为角增量信息，然后直接应用传统圆锥误差补偿算法，算法精度会降低2个数量级，不能满足载体高动态环境下对姿态解算精度的要求。因此有必要研究一种能够直接利用陀螺仪输出角速率信息进行姿态解算算法，从而进一步提高捷联惯导复杂工作环境下的姿态解算精度。旋转矢量的微分方程近似为： (

9、18)对于传统的二子样算法来说，在一个计算周期TT+h内需要三次陀螺采样值，采样时间h内，载体的角速度用抛物线拟合为：，0h (19)记角增量为 (20)则由式(19)和式(20)得 (21a) (21b) (21c) (21d) (22a) (22b) (22c) (22d) (22e)又由于姿态更新周期h一般为毫秒级的量，也可视为小量，这样有式(23)成立 (23)这样式(18)可写成 (24)对式(24)求各阶导数，并考虑到式(21d)和式(22e) (25a) (25b) (25c) (25d) (25e) (25f)用=0代入式(25)中，并应用式(21)和式(22)，得 (26)将

10、(h)用泰勒级数展开，得： (27)将式(26)代入式(27)得： (28)设陀螺在t=T，t=T+h/2，t=T+h的角速率分别为：1、2、3，可用陀螺角速率估计a、b、c的大小如下： (29)将式(29)代入式(28)并考虑陀螺的角增量输出，则旋转矢量可用下式估计： (30)式中X=1/60，Y=7/90。对捷联式惯性导航系统姿态更新来说，锥运动是最恶劣的工作环境条件，它会诱发数学平台的严重漂移，所以对旋转矢量算法作优化处理时常以锥运动作为环境条件。根据锥运动得到的准确四元数q(h)与旋转矢量(h)确定的计算四元数可以求得误差四元数，再把误差表达式进行泰勒级数展开，选择适当的X和Y使得误差

11、表达式各项系数为零，从而得出：X=1/180，Y=7/90。对于光纤陀螺输出为角速率时，式(30)中第一项的角增量需要通过数值积分算法估计其值大小，通常采用Simpson公式求解： (31)算法漂移率： (32)4. 算法仿真验证4.1 圆锥运动介绍圆锥运动是刚体运动的一种几何现象，刚体受到环境振动影响或本身具有的角运动，如果刚体在两个正交轴方向存在频率相同的角振动速率时，第三个正交轴在空间将绕其平衡位置作锥面或近似锥面的运动，称为刚体的圆锥运动。图2所示为经典圆锥运动示意图。图2 经典圆锥运动示意图下面对经典圆锥运动作简要介绍。假设载体坐标系为o-xyz，参考坐标系为o-XYZ，在某一时刻t

12、有公共原点o。参考坐标系和载体坐标系的关系可通过绕YZ平面内的oL轴旋转a角而得到，并且oL在YoZ平面内以角速度旋转，则o-xyz坐标系偏离开o-XYZ坐标系，且ox轴位于顶点为o半锥角为a的圆锥面上，y轴和z轴将分别绕Y轴和Z轴作振荡运动。在这种条件下，载体坐标系和参考坐标系之间的变换四元数为： (33)式中，为等效轴方向的单位矢量，a为动坐标系绕转动的角度，由图2得： (34)可得载体坐标系和参考坐标系之间的变化四元数为： (35)由四元数微分方程可以计算得经典圆锥运动的角速度矢量为： (36)4.2圆锥误差分析假设载体俯仰轴和横滚轴作同频不同相的角振动，即： (37)根据欧拉角微分方程

13、可知，与、的关系为： (38)对于微幅振动，有： (39)式(39)说明，当载体沿俯仰轴和横滚轴作同频不同相的角振动时，在方位轴上会整流出直流分量，且当相位相差90时，常值诱导漂移率最大： (40)此诱导漂移角速率称为圆锥误差。4.3 算法漂移率仿真为了验证改进的圆锥误差补偿算法有效性，对传统等效旋转矢量算法及旋转矢量改进算法在标准圆锥运动条件下进行仿真。I. 仿真环境设置圆锥仿真环境下，旋转矢量方程为：(1) 取半锥角a=1.0，陀螺采样周期为0.005s，考察旋转矢量算法漂移率与圆锥运动角频率的关系，仿真结果如图3所示。(2) 取半锥角a=1.0，圆锥运动的角速率为=2.0，考察旋转矢量算

14、法漂移率与算法迭代频率的关系，仿真结果如图4所示。II. 仿真结果图3 算法漂移率与圆锥运动角频率的关系图4 算法漂移率与姿态更新频率的关系图3可以看出，传统二子样算法与改进二子样算法的漂移率都随圆锥运动角频率的增大而增大，但在相同的角频率下，改进算法误差漂移率比同子样传统算法要小35个数量级；图4可以看出，改进算法与传统算法相比，算法精度都随着姿态更新频率的增大而提高，在相同姿态更新频率下，改进算法的精度显然高于传统算法。4.4 典型圆锥运动环境姿态解算仿真分析仿真流程如图5所示。图5算法仿真流程图I. 仿真环境设置圆锥运动的参数设置为：半锥角a=1.0，锥运动角频率f=1.0Hz，采样周期

15、为0.005s，仿真时间为30s。(1) 传统二子样旋转矢量算法在角增量输入情况下的姿态误差曲线如图6所示；当只有角速率输入时，通过Simpson积分法转化后得到的姿态误差曲线如图7所示。(2) 改进二子样旋转矢量算法在角速率输入时的姿态误差曲线如图8所示。II. 仿真结果图6 圆锥环境角增量输入下的二子样旋转矢量算法的姿态误差曲线图7 圆锥环境角速率输入下的二子样旋转矢量算法的姿态误差曲线图8 圆锥环境角速率输入下的二子样改进旋转矢量算法姿态误差曲线标准圆锥运动环境下圆锥误差主要体现在俯仰角上，且与时间呈线性增长关系，由图6和图7可以看出，使用积分算法对于需要准确角增量输入的算法的精度影响较大，航向角和姿态角解算精度将会降低，30s仿真时间内，俯仰角误差由0.01增加到0.0