CN103017765B - 应用于微机械组合导航系统的偏航角修正方法和修正装置 - Google Patents

Abstract

本发明公开了一种应用于微机械组合导航系统的偏航角修正方法，包括以下步骤：采用微机械纯惯导进行导航解算；利用导航解算获得的载体速度、载体位置与卫星导航接收机的定位数据进行组合导航；确定停止对偏航角进行修正的非修正时刻；在非修正时刻之外，根据偏航角的修正值对偏航角进行限幅修正。同时本发明还公开了一种应用于微机械组合导航系统的偏航角修正装置，包括修正停止单元和限幅修正单元，修正停止单元根据下述条件确定非修正时刻并输出有效的修正停止信号；限幅修正单元对偏航角修正值进行限幅修正，并在修正停止信号有效期间，停止对所述偏航角修正值的修正。采用本发明对微机械组合导航产品的偏航角进行修正。

Description

应用于微机械组合导航系统的偏航角修正方法和修正装置

技术领域

本发明涉及一种应用于微机械组合导航系统的偏航角修正方法，同时还涉及一种应用于微机械组合导航系统的偏航角修正装置。

背景技术

采用基于微机械技术的惯性测量单元(IMU)和卫星接收机组成的组合导航系统结合了卫星导航的高精度定位，误差无积累及惯性导航的自主性、实时性等优点，使导航系统的成本下降，可靠性增加，精度得到明显提高。

但是，微机械惯性器件本身存在精度相对较低的问题，在传统的组合导航算法中，由于偏航角可观测性较差，偏航角会逐渐发散。

发明内容

本发明解决的技术问题是：针对现有技术的不足，本发明提供了一种应用于微机械组合导航系统的偏航角修正方法，同时还提供了一种应用于微机械组合导航系统的偏航角修正装置，采用本发明可以对微机械组合导航产品的偏航角进行修正，通过将偏航角的误差限制在一定范围内，提高了微机械惯性测量单元与卫星导航数据的融合精度，使微机械组合导航产品更适用于各种近程战术武器。

本发明的技术解决方案是：

本发明一方面公开了一种应用于微机械组合导航系统的偏航角修正方法，包括以下步骤：

采用微机械纯惯导进行导航解算；

利用导航解算获得的载体速度、载体位置与卫星导航接收机的定位数据进行组合导航，

进一步还包括对偏航角的修正步骤，具体为：

(1)确定停止对偏航角进行修正的非修正时刻：

若载体水平速度小于速度限定值，则停止对偏航角的修正；

若载体存在航向的机动，则停止对偏航角的修正；

若载体的横向过载大于横向过载限定值，则停止对偏航角的修正；

若载体高度高于高度限定值，则停止对偏航角的修正；

若俯仰角大于俯仰角限定值，则停止对偏航角的修正；

若载体偏航角与弹道偏角的夹角小于修正限定值，则停止对偏航角的修正；

(2)在所述非修正时刻之外，根据偏航角的修正值对偏航角进行限幅修正。

本发明另一方面公开了一种应用于微机械组合导航系统的偏航角修正装置，包括：修正停止单元和限幅修正单元，

所述修正停止单元根据下述条件确定非修正时刻并输出有效的修正停止信号：

若载体水平速度小于速度限定值，则停止对偏航角的修正；

若载体存在航向的机动，则停止对偏航角的修正；

若载体的横向过载大于横向过载限定值，则停止对偏航角的修正；

若载体高度高于高度限定值，则停止对偏航角的修正；

若俯仰角大于俯仰角限定值，则停止对偏航角的修正；

若载体偏航角与弹道偏角的夹角小于修正限定值，则停止对偏航角的修正；

所述限幅修正单元对偏航角修正值进行限幅修正，并在修正停止信号有效期间，停止对所述偏航角修正值的修正。

进一步的，上述限幅修正单元的限幅修正值≥偏航角修正值/n，其中n为在一个偏航角计算周期中，所述纯惯导解算输出的次数。

进一步的，上述限幅修正单元在每个纯惯导的解算周期进行一次限幅修正。

本发明与现有技术相比具有如下优点：

本发明针对低精度的微机械组合系统，在传统的组合算法对偏航角进行修正，可将偏航角误差限制在一定范围内，起到误差约束的目的，满足微机械组合导航系统长期稳定工作的需要。

本发明以满足各种武器系统的导航需求为前提，以低成本的显著优势替代高精度、高成本的惯组，使得微机械组合导航系统扬长避短，达到特定应用环境下精确导航的要求。该方法便于工程应用，适于批量化生产。

附图说明

图1为纯惯导解算流程图；

图2为本发明的偏航角修正方法流程图；

图3为姿态角仿真曲线；

图4为计算误差角的流程图；

图5为偏航角修正量的弱化处理流程图；

图6为Vel_Yaw计算误差曲线图。

具体实施方式

下面就结合附图对本发明做进一步介绍。

微机械纯惯导的导航解算过程是一个积分计算的过程。利用当前得到的角速率数据对姿态角与捷联矩阵进行更新，再将当前的加速度数据在新矩阵下映射，得到东北天坐标系的投影，完成东、北、天方向速度与位置的更新，计算流程图见图1。其中姿态角的定义与旋转顺序有关，不同的飞行弹道应选择不同的坐标系，以减小导航参数表征误差。

组合导航计算主要是利用纯惯导计算的载体速度、载体位置与获得的卫星导航接收机的定位数据比较获得量测方程，通过数学推导可以获得状态方程，经过Kalman滤波方程后可以获得导航参数的误差项。卫星导航和纯惯导输出信号通过Kalman滤波器进行组合，滤波器的输出作为误差估计值对导航参数进行校正。

在上述利用微机械纯惯导和卫星的定位数据进行组合导航的过程中，本发明进一步引入了采用载体速度对偏航角的修正过程。对偏航角的修正需要进行严格的逻辑判断，保证修正时弹道偏角能够尽量逼近载体的航向角，从而将侧滑角控制在一个小范围内，即修正时残余的误差尽量小。此时利用东向速度与北向速度的矢量关系，计算弹道偏角。

(1)在对偏航角进行修正时，需要确定对偏航角的非修正时刻，在该非修正时刻需要停止对偏航角的修正。非修正时刻的确定如下：

a、根据载体速度确定非修正时刻

由东速和北速计算水平面内速度矢量的方法如下：

$\left\{\begin{array}{cc}\mathrm{Ve}<0;& \mathrm{Vel}\_\mathrm{Yaw}=\arccos \left(\frac{\mathrm{Vn}}{\sqrt{V_n^2+V_e^2}}\right)\\ \mathrm{Ve}>0;& \mathrm{Vel}\_\mathrm{Yaw}=2\mathrm{\&pi;}-\arccos \left(\frac{\mathrm{Vn}}{\sqrt{V_n^2+V_e^2}}\right)\end{array}\right.$

其中，Vn、Ve分别为北向速度和东向速度，为载体的水平速度，Vel_Yaw为由速度计算得到弹道偏角。如果载体速度过小，弹道偏角会随着速度分量的比重变化而出现角度跳动，以载体静止时为例，载体速度为0，但速度实测值在0附近跳动，此时东速和北速正负跳变引起的Vel_Yaw角度跳变最为明显，所以修正时载体水平速度要足够大。因此将载体水平速度小于速度限定值的时刻或时间段，确定为非修正时刻，由上述公式可知，在水平速度恒定时，Vn在内取不同值，速度跳变引起的角度Vel_Yaw计算误差也随之变化。北向速度Vn跳动越小，水平速度值越大，则测量精度越高。设定速度跳动范围为0.2m/s，水平速度为200m/s时，随着Vn在0～200m/s内变化，Vel_Yaw计算误差曲线如图6所示。具体应用中可根据载体水平速度设定该限定值。

b、根据载体在航向的机动确定非修正时刻

如果载体存在航向的机动，即载体处于转弯过程中，则弹道偏角与载体偏航角存在较大的夹角(即此时侧滑角较大)，此时应停止对偏航角的修正，并将该时刻或时间段，确定为非修正时刻。

c、根据载体的横向过载确定非修正时刻

通过对横向过载的判断，确定在载机飞行过程中是否存在侧滑角。在飞行过程中，载体存在侧滑角时，气动力会引起横向过载，在一定范围内，侧滑角越大，横向过载越大。如果横向过载过大(此时侧滑角也较大)时，不适宜对偏航角进行修正。由于加速度计的输出存在白噪声，所以对横向过载的监测要通过多次过载输出取平均值来实现，当横向过载大于横向过载限定值时，将大于横向过载限定值的时刻或时间段确定为非修正时刻。载体横向过载限定值取值越小，则限制越严格，相应的。数值越低，限制越严格，修正次数也越少，但计算修正值的精度更高。本发明实施例中，载体横向过载限定值一般取值15mg～30mg。

d、根据载体高度确定非修正时刻

由于高空空气密度较小，在侧滑角保持恒定时，高空环境中气动力减小，横向过载敏感值减小，使得对横向过载的逻辑判断过于宽松，所以在高空时不进行偏航角的修正，将该时刻或时间段确定为非修正时刻。本发明实施例中载体高度限定值的确定需要结合弹体气动外形分析，一般取30km～40km。

e、根据载体俯仰角确定非修正时刻

在俯仰角不大时，水平面内速度的矢量方向与载体的航向近似，可以借助速度对角度进行修正；但俯仰角绝对值较大时，上述两者差异较大，无法使用速度辅助载体姿态角修正。因此，将俯仰角大于俯仰角限定值的时刻或时间段确定为非修正时刻，本发明实施例中俯仰角限定值确定为大于等于60度。

f、根据载体偏航角与弹道偏角的夹角确定非修正时刻

借助速度消除偏航角的误差在理论上是不可能实现的，两者的理论误差与弹体气动外形、风速等条件有关系，在具体型号中必须计算该误差阈值，本方法修正的目的是将偏航角的误差限制在该阈值内。所以在满足上述各种逻辑判断时，需要计算待修正量(载体偏航角与弹道偏角的夹角)是否超出修正阈值。只有误差角足够大时才进入修正，保证偏航角误差值限制在该阈值内。因此，当载体偏航角与弹道偏角的夹角小于修正限定值(即修正阈值)时，停止对偏航角的修正，确定为非修正时刻。

(2)当偏航角符合上述修正条件进行角度修正时，一步到位的修正会带来偏航角的跳变，由于姿态角之间的耦合关系，滚转角和俯仰角输出也会出现不同程度的跳动，过大的角度跳动将会给控制系统带来不必要的麻烦。因此在对偏航角进行修正时，对每次的修正值进行幅值限定，采取多次修正的限幅修正方式对修正量多次修正，从而避免上述的偏航角跳变问题。

对偏航角的修正值进行限幅修正时，限幅修正的幅度≥偏航角修正值/n，其中n为在一个偏航角计算周期中，所述纯惯导解算输出的次数。

完成对偏航角的一次修正后，继续重复上述(1)～(2)的步骤，在每个纯惯导解算周期中对偏航角进行一次重复修正。

微机械惯导与卫星导航数据的融合可以对导航参数进行修正，但由于偏航角误差观测性较差的问题，传统的组合滤波算法难以将偏航角修正到理想范围，偏航角精度会随着时间漂移慢慢发散。本发明基于微机械惯导自身的特性，将速度信息辅助偏航角的修正。需要指出的是，由于侧滑角的存在，本发明的偏航角修正方法并不能将偏航角修正到其真值，但可以选择侧滑角较小时对偏航角进行修正，从而将偏航角误差控制在一定范围内，使得低精度的惯导扬长避短，满足战术武器制导精度的要求。该设计方法一致性好，设计周期短，便于工程实现。

下面以应用于火箭弹的某型微机械组合导航装置产品为例对本发明实施例做进一步描述。其中微机械陀螺零位稳定性约为50°/h。

(1)建立惯导解算编排方程，在获取导航初值后，惯组的计算单元根据当前的角速度和加速度数据进行惯导解算。惯导解算是一个迭代计算的过程，每周期获取新的角速度和加速度数据后，根据上一周期的导航结果迭加，完成当次的解算。

惯导的解算涉及到导航初值的问题，导航初值包含3个姿态角、3维速度和3维位置。在产品上电时，组合系统处于静止状态，速度初值为0；位置初值由系统中卫星导航接收机提供；姿态角初值由参数装订得到。

为满足控制系统实时性要求，以5ms为惯导解算周期，导航系统向控制器以200Hz发送导航数据。

(2)卫星导航接收机单独完成卫星定位，通过内部接口的形式将定位数据发送给惯组的计算机单元。计算单元完成数据包的接收、校验、解包后，得到载体速度和位置值。卫星导航定位周期为200ms。

计算机单元根据惯导的工作状态与卫星导航接收机的工作状态，确定是否满足Kalman组合滤波的条件，如果满足条件则进行滤波，并利用滤波结果对导航数据进行修正，如果不满足滤波条件，则以惯导解算的数据为导航结果。正常状态下，组合滤波周期为200ms，期间包含40次纯惯导解算。

组合滤波器采用线性Kalman滤波，取其状态矢量为惯导系统各误差项。选取导航坐标系为北东地坐标系。取系统状态矢量为

X＝[φ δV δP ε_b ε_r]^T

式中：

φ＝[φ_N  φ_E φ_D]^T，分别代表北向、东向和地向平台误差角；

δV＝[δV_N δV_E δV_D]^T，分别代表北向、东向和地向速度误差；

分别代表纬度、经度和高度误差。

将上述3组修正量引入组合系统完成修正后，由于φ_D可观测性较差，偏航角修正效果并不理想，其他姿态角以及速度和位置均有较好的修正精度。

(3)通过在线判断每周期是否具备偏航角的修正条件，来确定是否进行偏航角的修正，具体的判断流程如下：

a、该火箭弹在发动机关机后至落地均为惯性飞行，水平速度均高于1倍音速，而发动机工作时间较短，发动机关机前无须对偏航角进行修正，根据关机后的水平速度下限值，将水平速度门限设定为300m/s以上。

b、对航向角速率Wy进行监测，在|Wy|＜1°/s时，进行偏航角修正。

c、对载体的横向过载Az进行观察，为避免白噪声对Az的影响，选取100ms中的Az数据进行平均，如果则对偏航角进行修正。

d、考虑大气密度对空气动力的影响，存在侧滑角时，弹体飞行高度在30公里以下所受到的横向过载足够明显，所以设定飞行高度h＜30Km时，对偏航角进行修正。

e、在俯仰角绝对值较大时，东向和北向的速度矢量和方向与载体的航向角差别较大，所以在修正时，必须考虑俯仰角的影响。导航计算中，弹体坐标系到导航坐标系的旋转顺序依次为偏航、俯仰、滚转，定义俯仰角变化范围为-90°～+90°；偏航角和滚转角变化范围为-180°～+180°。假设一种飞行模式进行数学仿真：初始点弹体静止，姿态角均为0，之后弹体向正北飞行，偏航角速率Wy和滚转角速率Wx保持不变，俯仰角速率以恒定值逐渐增加，从图3的仿真结果可知，在俯仰角接近90度时，偏航角    与滚转角γ的绝对值均出现0到180°的剧烈变化，但在俯仰角＜60°时，偏航角与滚转角基本为0。整个仿真过程中由东速和北速计算得到的弹道偏角均为0度(正北向)，由导航解算得到的偏航角在俯仰角较小时与弹道偏角基本能吻合，但随着俯仰角逐渐变大，两者差异越来越大，所以在俯仰角＜60°时，对偏航角进行修正。

f、由于本方法中偏航角的修正目的是将偏航角限制在一定范围内，所以在速度矢量与载体偏航角之间的夹角较小，并未到达修正阈值时，不会对偏航角进行修正。结合火箭弹的气动特性，使用本方法的判断逻辑后，由侧滑角和风扰动引起的误差不会大于5°，所以将误差的修正阈值定为5°。在火箭弹3个姿态角中，滚转角直接影响控制系统内回路中的弹体姿态稳定，不参与外回路的制导；而偏航角并不直接参与制导和控制，所以5度以内的偏航角误差即使对滚转角稍有耦合作用，但对控制系统影响很小，对后续惯导的解算影响也很小。

由于速度矢量角和偏航角幅值均为[0，360°)，所以在计算中存在0值附近数据跳变的问题，必须通过判断来确定夹角的真值，例如358°和2°的夹角是4°而不是356°。图4为计算流程图，Phi_Error为最终待修正的误差角。只有满足流程图中修正条件时才会对偏航角进行修正。

(4)如果进入了偏航角的修正流程，则Phi_Error≥5°，角度跳变5°以上很可能会使控制系统出现控制量跳动，不利于系统的稳定，因此，将修正量分多次通过限幅修正的方式进行误差校正。

例如，偏航角误差计算是在组合滤波后对偏航角进行计算得到，所以偏航角计算周期为200ms，期间包含了40次纯惯导解算结果的输出，误差量Phi_Error只要在40次输出内完成修正，即可满足时序要求。结合微机械陀螺的实际精度，在排除陀螺故障的前提下，Phi_Error不会超出40度的误差范围，将单步修正量设定为1度，既满足了控制系统平稳性的要求，又能保证时序上的及时修正。如果控制系统对角度跳动要求较高，可以分40次修正，每次修正量为Phi_Error/40。

在修正算法设计中，选取每次弱化后的修正量为1度，经过多次单步修正后，最终完成修正。图5为修正流程图，其中Phi_Single为单步修正的角度值，counter为计算得到的修正次数。

(5)最后需实际验证其可行性。半实物仿真验证是一种接近实际应用的验证方法，也是一种便于设置参量、观察参量的测试手段。分别将传统的组合导航算法和改进后的算法写入微机械组合导航硬件系统，并分别在两种状态下进行组合系统的半实物仿真，最后通过全程模飞中的偏航角精度来考核修正算法的作用和效果。针对该改进算法的多次试验，结果见表1。采用修正前的算法，在偏航角误差较小时，在改进后的算法中偏航角精度与改进前相当；但偏航角本身误差较大时，改进后的算法明显体现出了优势，其误差均未超出5度的范围，而传统的算法导致偏航角最大误差接近15度。

表1

本发明未详细说明部分属本领域技术人员公知常识。

Claims (6)

1.一种应用于微机械组合导航系统的偏航角修正方法，包括以下步骤：

采用微机械纯惯导进行导航解算；

利用导航解算获得的载体速度、载体位置与卫星导航接收机的定位数据进行组合导航，组合导航为利用当前得到的角速率数据对姿态角与捷联矩阵进行更新，再将当前的加速度数据在新矩阵下映射，得到东北天坐标系的投影，完成东、北、天方向速度与位置的更新，其中姿态角的定义与旋转顺序有关，不同的飞行弹道应选择不同的坐标系，以减小导航参数表征误差；利用纯惯导计算的载体速度、载体位置与获得的卫星导航接收机的定位数据比较获得量测方程，通过数学推导可以获得状态方程，经过Kalman滤波方程后可以获得导航参数的误差项；卫星导航和纯惯导输出信号通过Kalman滤波器进行组合，滤波器的输出作为误差估计值对导航参数进行校正的组合导航计算过程；

其特征在于，包括以下步骤：

(1)确定停止对偏航角进行修正的非修正时刻：

若载体水平速度小于速度限定值，则停止对偏航角的修正；

若载体存在航向的机动，则停止对偏航角的修正；

若载体的横向过载大于横向过载限定值，则停止对偏航角的修正；

若载体高度高于高度限定值，则停止对偏航角的修正；

若俯仰角大于俯仰角限定值，则停止对偏航角的修正；

若载体偏航角与弹道偏角的夹角小于修正限定值，则停止对偏航角的修正；

(2)在所述非修正时刻之外，根据偏航角的修正值对偏航角进行限幅修正。

2.如权利要求1所述方法，其特征在于，对偏航角的修正值进行限幅修正时，限幅修正值≥偏航角修正值/n，其中n为在一个偏航角计算周期中，所述纯惯导解算输出的次数。

3.如权利要求1或2所述方法，其特征在于，所述限幅修正在每个纯惯导的解算周期中对修正值进行一次修正。

4.一种应用于微机械组合导航系统的偏航角修正装置，其特征在于，包括：修正停止单元和限幅修正单元，

所述修正停止单元根据下述条件确定非修正时刻并输出有效的修正停止信号：

若载体水平速度小于速度限定值，则停止对偏航角的修正；

若载体存在航向的机动，则停止对偏航角的修正；

若载体的横向过载大于横向过载限定值，则停止对偏航角的修正；

若载体高度高于高度限定值，则停止对偏航角的修正；

若俯仰角大于俯仰角限定值，则停止对偏航角的修正；

若载体偏航角与弹道偏角的夹角小于修正限定值，则停止对偏航角的修正；

所述限幅修正单元对偏航角修正值进行限幅修正，并在修正停止信号有效期间，停止对所述偏航角修正值的修正。

5.如权利要求4所述装置，其特征在于：所述限幅修正单元的限幅修正值≥偏航角修正值/n，其中n为在一个偏航角计算周期中，纯惯导解算输出的次数。

6.如权利要求4或5所述装置，其特征在于：所述限幅修正单元在每个纯惯导的解算周期进行一次限幅修正。