基于弹载捷联惯性导航系统精确导航的双欧拉全姿态方法

吕维维，程向红，邱 伟

(1. 东南大学 微惯性仪表与先进导航技术教育部重点实验室，江苏 南京 210096； 2. 东南大学 仪器科学与工程学院， 江苏 南京 210096； 3. 上海宇航系统工程研究所，上海 201109)

0 引言

捷联惯性导航系统(SINS)是一种不依赖于任何外部信息，也不向外部辐射能量的自主导航系统。该系统具有隐蔽性好、导航精确的优点，在战术导弹、运载火箭、宇宙飞船、飞机、舰船上有着广泛应用[1-2]。作为SINS的核心，姿态更新算法在SINS精确导航中发挥着重要作用，这些算法主要有方向余弦法、四元数法、欧拉角法[3-5]等。其中：方向余弦法更新姿态矩阵需求解9个微分方程，求解的姿态矩阵可全姿态工作，但计算量较大[3]；四元数法计算量小、计算精度高，但每次求解到的姿态矩阵必须经过正交化的误差修正处理，在飞行过程中会出现有界参数误差出界的情况[6-8]；欧拉角法求解欧拉角只需求解3个微分方程，与其他算法相比，需要求解的方程个数最少，且求解到的姿态矩阵永远是正交阵，无需正交化处理，但欧拉角系统有1对奇异点，在奇异点附近会产生解算误差，使输出姿态角发生突变，无法准确定值[2, 9-10]。

为解决欧拉角奇异性的问题，黄雪樵[6]最早对双欧法进行了理论论证，认为双欧法能有效消除单欧法奇异点。SINGLA等[11]对序贯旋转法(MSR)进行了详细研究，并指出采取不同欧拉角组合、相互切换的方法比MSR方法更简洁。陈廷楠等[12]将双欧法与四元数法进行了理论对比，证明四元数法存在较大的累计计算误差，双欧法在克服欧拉角奇异性的问题上优于四元数法。以上学者主要进行理论研究，对双欧法的应用研究较少，对四元数法与双欧拉法解算精度的对比研究也较少。为适应现代战争的需要，目前战术导弹已逐渐采用大攻角控制方式，实现大机动飞行。对于垂直发射的地空导弹，为使导引头能成功捕获目标，要求对导弹姿态进行精确控制。传统方法使用四元数表示弹体姿态，并对姿态控制进行系统设计，然而四元数法在弹体俯仰角90°时只能进行姿态矩阵更新，无法正常输出姿态角，这给大机动地空导弹的姿态控制带来了困难。

本文针对垂直发射的地空导弹在大机动飞行时姿态解算容易出现奇异值的问题，将正、反欧拉角切换的双欧拉全姿态算法应用到姿态解算过程中，通过半物理试验验证了该方法的有效性。

1 正、反欧拉角方程

1.1 欧拉角法

动坐标系相对参考坐标系方位，由动坐标系依次绕3个不同轴转动的3个转角来确定[6]，如图1所示。令O-XbYbZb为动坐标系，O-ENU为参考坐标系，则航向角H、纵摇角P、横滚角R为1组欧拉角。

图1 弹体空间角位置的确定Fig.1 Determination of missile’s space position

两坐标系间的变换矩阵等于基本旋转确定变化矩阵的连乘，顺序根据基本旋转先后次序从右向左排列。图中，弹体空间角位置确定依据为

令导航坐标系为n，弹体坐标系为b，则姿态矩阵可表示为

(1)

(2)

1.2 正、反欧拉角微分方程

弹体的姿态变化可看作依次绕航向轴、俯仰轴、横滚轴进行基本旋转后的复合结果[1, 10]，即

(3)

(4)

(5)

将式(5)转换为欧拉角微分方程，具体表示为

(6)

(7)

由式(6)可知，欧拉角微分方程包含三角函数运算，给实时计算带来困难，当俯仰角P=90°时，欧拉角微分方程出现奇异点，使计算溢出。

(8)

(9)

(10)

式(8)～(10)中：P=±90°为方程组奇异点，但当P=0或P≈±180°时，方程解的精度会提高，此求解范围称为正欧拉方程解的精华区。

(11)

将式(11)转换为

(12)

(13)

(14)

(15)

从反欧拉方程来看，奇异性问题同样存在，即Rr=±90°是方程组的奇异点；当Rr=0或Rr=±180°时，方程解的精度会提高，此求解范围称为反欧拉角微分方程解的精华区。

2 正、反欧拉角的转换关系

任意2个坐标系之间的变换矩阵是唯一的，因此正、反欧拉角的变换矩阵对应项相等，分别表示为

(16)

L(Pr,Rr,Hr)=

(17)

由式(16)，(17)可得，cosRr=sinP/sinPr，将cosRr代入式(13)～(15)中，则反欧拉角微分方程可变换为

(18)

(19)

(20)

图2 正、反欧拉角微分方程的精华区与奇异区Fig.2 Essence areas and singular areas of positive and reverse Euler angle differential equations

由式(18)～(20)可见，反欧拉角微分方程与正欧拉角微分方程相反，其精华区在P=±90°附近，奇异区在P=0或P=±180°附近。若以±45°和±135°为界进行划分，则正、反欧拉角微分方程的精华区与奇异区如图2所示。由图可知：正、反欧拉角微分方程的精华区与奇异区正好呈倒置关系。充分利用该关系实现两者的无缝切换：当|P|≤45°或|P|>135°时，求解正欧拉角微分方程，发挥正欧拉角精华区解算精确的优点；当|P|>45°且|P|≤135°时，求解反欧拉角微分方程，利用反欧拉角精华区进行姿态解算，有效避免俯仰角在±90°附近姿态角突变的问题。

由于正、反欧拉角变换矩阵的一致性，因此正欧拉角变换矩阵与反欧拉角变换矩阵的对应项相等，其定义为

由式(16)～(17)得到正欧拉角到反欧拉角的变换关系式，即

(21)

式中：Rr≠±π/2。反之，反欧拉角到正欧拉角的变换关系式为

(22)

式中：P≠±π/2。针对地空导弹的大机动运动，利用双欧拉全姿态算法，交替采用正、反欧拉角来描述姿态。地空导弹正、反欧拉角微分方程的全姿态解算流程如图3所示。由图可知，其解算步骤为：1)初始化系统，输入惯性测量单元(IMU)信息。2)对正欧拉角俯仰角P的绝对值进行判断，当|P|≤45°或|P|>135°时，符号标志位Flag=1，此时求解正欧拉角微分方程；当|P|>45°且|P|≤135°时，Flag=-1，此时求解反欧拉角微分方程。整个姿态解算的持续时间设为T，当导航时间达到T时解算结束。

图3 地空导弹正、反欧拉角微分方程全姿态解算流程Fig.3 Flow chart of positive and reverse Euler angle differential equations’ whole attitude solution

3 试验验证

3.1 仿真验证

为验证地空导弹双欧拉全姿态算法的非奇异性，进行了仿真验证。仿真参数设置情况见表1。姿态解算周期为0.01 s，仿真经度为118.8° E，纬度为32.06° N。在仿真过程中，IMU航向角H=0°，横滚角R=0°，俯仰角周期性摇摆，正弦函数为A·sin(2πf·t+φ0)+ψ0。其中：A为IMU的摇摆幅值，A=90°；f为摇摆频率，f=(1/256)Hz；φ0为摇摆初始相位，φ0=0°；ψ0为摇摆中心，ψ0=0°。仿真时间为2 500 s。

表1 传感器参数设置

整个仿真过程中，IMU俯仰角在±90°附近周期性摇摆，姿态解算采用四元数算法。经过2 500 s的仿真，IMU实际解算姿态角与理论姿态角的对比如图4～6所示。

图4 四元数法航向角的理论值与实际值比较Fig.4 Comparison of heading angle’s theoretical value and actual value by using quaternion method

图5 四元数法俯仰角的理论值与实际值比较Fig.5 Comparison of pitching angle’s theoretical value and actual value by using quaternion method

图6 四元数法横滚角理论值与实际值比较Fig.6 Comparison of rolling angle’s theoretical value and actual value by using quaternion method

由图4～6可知：当姿态解算采用四元数算法时，由于IMU俯仰角为±90°时只能进行姿态更新，无法正常输出姿态角，因此IMU输出的实际航向角和横滚角均发生突变，姿态角出现奇异现象。

为进行对比，在同样条件下采用双欧拉全姿态算法进行姿态解算，IMU实际解算的姿态角与理论姿态角的对比情况如图7～9所示。

图7 双欧拉法航向角理论值与实际值比较Fig.7 Comparison of heading angle’s theoretical value and actual value by using dual-Euler method

图8 双欧拉法俯仰角理论值与实际值比较Fig.8 Comparison of pitching angle’s theoretical value and actual value by using dual-Euler method

图9 双欧拉法横滚角理论值与实际值比较Fig.9 Comparison of rolling angle’s theoretical value and actual value by using dual-Euler method

由图7～9可知：当采用双欧拉全姿态算法后，IMU实际解算的航向角、横滚角与理论值基本重合，未出现突变为-180°或180°的现象。理论俯仰角与实际俯仰角基本重合，在±90°附近能正常输出姿态，未出现突变现象。双欧拉全姿态解算的实际姿态、理论姿态的误差曲线分别如图10～12所示。

图10 双欧拉法航向角误差Fig.10 Error curve of heading angle by using dual-Euler method

图11 双欧拉法俯仰角误差Fig.11 Error curve of pitching angle by using dual-Euler method

图12 双欧拉法横滚角误差Fig.12 Error curve of rolling angle by using dual-Euler method

由图10～12可知：在解算过程中，航向角误差缓慢变大，但误差绝对值在0.5′以内。俯仰角误差因IMU周期性摇摆而上下波动，但误差绝对值在1′以内。横滚角误差随时间增长而缓慢变大，但误差绝对值在0.6′以内。当运用正欧拉角更新时，Flag=1；当运用反欧拉角更新时，Flag=-1。俯仰角在±90°摇摆时Flag的变换情况如图13所示。

图13 双欧拉法正、反欧拉角微分方程切换标志Fig.13 Switch flag of positive and reverse Euler angle differential equations by using dual-Euler method

3.2 半物理验证

为进一步验证地空导弹双欧拉全姿态算法的有效性，在三轴转台上进行了半物理试验。转台试验实物如图14所示，该转台的转动速率精度为±0.000 5(°)/s，角度测量精度为±0.000 1°，可认为转台输出是没有误差的理论姿态。在半物理试验中，转台外框、中框、内框分别控制IMU的航向角、俯仰角、横滚角转动。试验采用某型挠性惯组作为IMU，其中三轴陀螺仪的随机常值漂移为0.025 (°)/h，随机噪声为0.01(°)/√h，三轴加速度计的随机常值偏置为0.1 mg，随机噪声为0.05 mg/√Hz。IMU的姿态更新周期为0.01 s。试验中，转台外框和内框始终保持静止(H=0，R=0)，转台中框先静止一段时间，然后以约0.015 (°)/s的固定转动速率在±90°之间周期性转动。

图14 转台试验实物Fig.14 Actual turntable test

试验总共进行2 500 s，导航计算机实时记录转台输出姿态、IMU输出角速度、加速度信息。导航计算机采用双欧拉全姿态算法对陀螺仪和加速度计输出进行捷联解算。给出俯仰角姿态解算结果，解算得到的实际俯仰角与转台输出理论俯仰角的比较如图15所示。

图15 双欧拉法俯仰角理论值与实际值比较Fig.15 Comparison of pitching angle’s theoretical value and actual value by using dual-Euler method

图16 双欧拉法俯仰角误差曲线Fig.16 Error curve of pitching angle by using dual-Euler method

由图15可知：双欧拉全姿态算法得到的IMU俯仰角曲线与转台输出俯仰角曲线几乎重合。即使转台输出角速率因人为操作而出现小的波动，IMU解算的姿态角也能很好跟踪。当俯仰角接近±90°时，导航计算机能正常输出俯仰角，未出现突变为-180°或180°的现象。双欧拉全姿态算法解算的俯仰角与理论俯仰角的误差曲线如图16所示，解算出的俯仰角与理论俯仰角的误差大致在±2′之间波动，误差值保持稳定。

半物理试验正、反欧拉角微分方程切换时Flag的变化情况如图17所示。由图可见：Flag的变化与IMU俯仰角大小的变化保持一致。

图17 双欧拉法正、反欧拉角微分方程切换标志Fig.17 Switch flag of positive and reverse Euler angle differential equations by using dual-Euler method

3.3 双欧拉全姿态方法与四元数法的精度比较

垂直发射的地空导弹为了使导引头能够成功捕获目标，对导弹的姿态精度和导航精度提出了很高的要求。但目前，对双欧拉方法进行理论研究的文献很少有双欧拉方法与四元数法导航精度比较的报道。为比较本文提出的地空导弹双欧拉全姿态方法与传统四元数方法的导航精度，进行了半物理纯惯性导航试验。试验进行了2 500 s，分别记录不同时刻采用双欧拉法和四元数法所得的纯惯性导航位置误差，对比情况见表2～4。

表2 纯惯性导航东向位置误差

表3 纯惯性导航北向位置误差

表4 纯惯性导航位置误差

由表2～4可知：在不同时刻采用双欧法所得的纯惯性导航东向位置误差、北向位置误差、位置误差均小于采用四元数法所得的误差。采用四元数法每次求解到的姿态矩阵必须经过正交化的误差修正，不可避免存在近似误差，且随着计算时间的增加，误差逐渐变大。由于双欧拉法无需正交化处理，因此无近似误差的累积。由表4可知：当时间为500 s时，双欧拉法的位置误差比四元数法的位置误差减小了9.81%；当时间为2 500 s时，双欧拉法的位置误差比四元数法的位置误差减小了13.14%。随着时间的增加，双欧拉法的导航精度比四元数法的导航精度更高。

4 结束语

地空导弹在大机动飞行时，其俯仰角在±90°附近会因奇异性而造成姿态角输出突变。为解决该问题，本文提出了双欧拉全姿态解算方法。通过推导正、反欧拉角微分方程，证明正、反欧拉角微分方程的精华区与奇异区呈倒置关系。根据俯仰角将正、反欧拉角微分方程进行切换，避免微分方程在奇异区内求解所产生的奇异性。详细分析了正、反欧拉角的内在联系，建立了两者的转换关系，实现两者的无缝切换。为验证地空导弹双欧拉全姿态算法的有效性，进行了仿真试验与半物理试验,结果表明：由双欧拉全姿态算法得到的弹体俯仰角，在±90°附近与理论俯仰角一致，未出现姿态突变现象。将双欧拉全姿态解算方法与四元数法的导航精度进行试验对比，结果表明：前者导航精度更高，未出现误差累积现象。本文方法能保证地空导弹在大机动飞行时姿态角正常解算和输出，对于弹载SINS在大机动飞行时的全姿态解算具有较高的工程应用价值。后续将对大机动飞行实际系统中的导弹进行进一步测试，以验证该方法的稳定性和精确性。