董一平, 刘 宁,*, 苏 中, 王靖骁, 白宏阳

(1. 北京信息科技大学自动化学院, 北京 100192; 2. 北京信息科技大学高动态导航技术北京市重点实验室, 北京 100192; 3. 南京理工大学能源与动力工程学院, 南京 210094)

0 引 言

现代军事中,精确化打击已经成为主要的发展方向。高速自旋飞行体在飞行过程中自身高速旋转产生陀螺力矩,能够克服由推力偏心、质量偏心、气动偏心等引起的误差,具有较强的抗干扰能力。因此,受到世界上各军事强国的普遍重视。高速自旋是一种绕自身轴线的旋转速度为10 rev/s (1 rev=360°)以上的运动状态,广泛使用于航天、武器、民用等领域中。高速自旋飞行体主要包括:火箭弹类、导弹、弹箭类等,同时具有较高的打击能力,在增雨防雹、太空探索等民用领域也发挥重大作用,因此研究高速自旋飞行体的制导问题至关重要。

高速自旋飞行体在制导过程中,需要实时对飞行体的位置坐标、飞行姿态、飞行速度等参数进行解算,但存在着一定的困难。文献[9]中给出火箭弹的滚动角度较大,卫星在捕捉信号时难度较大;文献[10]中指出火箭弹飞行过程中横滚角过大,需要较大的动态测量范围,对陀螺仪的精度要求较高。高速自旋飞行体的滚转角速度可达上百转每秒,单纯使用捷联惯导系统(strapdown inertial navigation system,SINS),则陀螺仪具有较大的测量范围,但其分辨率会减低,导致测量误差增大。

针对上述问题,组合导航的方式可以将SINS与北斗卫星导航系统二者互补,同时具有精度高,抗干扰能力强等优点,更好发挥优势。文献[13]提出了全球定位系统(global positioning system, GPS)/惯性测量单元(inertial measurement unit, IMU)组合方式,采用扩展卡尔曼滤波方法(extended Kalman filter, EKF)进行滤波;文献[9]给出了INS/GPS超紧耦合的组合导航方法。但超紧组合方式对信息限制较多,使用不便捷;文献[14]给出了采用地磁辅助的微机械(micro electro mechanical system,MEMS)陀螺进行火箭弹姿态测量;文献[15]提出了基于MEMS的IMU和北斗卫星组合的方式,并通过无迹卡尔曼滤波算法对据进行融合,提高弹道参数测量的精度。

组合导航的滤波方式有多种,主要为卡尔曼滤波(Kalman filter, KF)算法和以其为基础的改进算法。EKF作为估计工具,在控制领域具有一定地位,但在非线系统,EKF可能出现偏差。传统的EKF算法,其系统噪声协方差矩阵和量测噪声协方差矩阵设定为常值,但实际系统噪声是随机的,所以滤波误差会增大。因此,本文基于自适应EKF(adaptive EKF, AEKF)算法进行数据融合,对量测噪声进行估计,改进AEKF算法。

本文基于北斗/SINS松组合模型,使用改进的AEKF算法对数据进行融合,根据北斗卫星是否更新判断进入量测。仿真结果表明,与未改进的相比,改进的AEKF算法系统滤波误差更小,系统更加稳定,同时具有轻量级的优点。

1 AEKF算法

1.1 AEKF算法实现

AEKF算法是在EKF算法的基础上演变而来,增加了遗忘因子,以对系统的噪声进行估计,达到减小误差的效果。EKF算法是解决非线性状态估计最为直接且性能较好的方法。由于噪声的不可知性,对EKF的算法造成影响。AEKF既有EKF适用于非线性系统的优点,又能够对未知的噪声进行估计,进而适应不断变化的噪声情况。因此,向EKF中引入自适应机制。

使用较为广泛的AEKF算法为Sage-Husa算法,该算法主要通过观测数据进行滤波的同时,使用噪声统计估计器,能够实时地估计和修正系统的系统噪声和量测噪声,具体引入带遗忘因子的噪声统计估计值。

设线性离散化系统方程和量测方程分别为

(1)

式中：是系统时刻的维估计状态;是系统时刻的维观测序列;是系统状态转移矩阵;是观测矩阵;-1,分别是系统噪声和量测噪声序列,相互独立且为正态白噪声。则Sage-Husa算法具体描述为

状态预测:

(2)

(3)

残差:

(4)

量测更新:

(5)

式中:为时刻EKF的增益矩阵。

(6)

=[-],-1

(7)

自适应估计器为

(8)

(9)

(10)

(11)

式中：=(1-)(1-+1)(0<<1)为遗忘因子,其作用为加强新数据对滤波的作用。

1.2 算法改进

本文在此基础上,对自适应器的量测噪声估计公式中添加系数,其中添加系数的目的是更准确地反映量测噪声的变化情况,具体为

(12)

(13)

2 组合导航系统

2.1 坐标系选取

本文采用SINS,使用导航坐标系北-东-地坐标系;组合导航系统借助卫星信号修正,卫星信号为WGS-84国际统一的大地坐标系经度、纬度和高度。为统一计算,将卫星信号从大地坐标系转换至中间地心直角坐标系,再转换至北-东-地(N-E-D)坐标系。

2.2 数学模型

传统的EKF建模方法,对于建模所需的变量易于选取,但在计算过程中需要每次对非线性数据采用雅各比矩阵的方式进行线性化,计算量大,对实时性有较大影响。因此本文采用对系统方程和量测方程的非线性函数作泰勒级数展开并仅保留线性项,获得线性模型,提高系统的实时性。

选取系统的误差量作为状态变量:位置误差Δ、速度误差Δ、姿态误差Δ、加速度计零偏误差Δ和陀螺零偏误差Δ,建立状态方程如下:

(14)

(15)

(16)

其次,建立量测方程+1=[+1,+1],具体如下式所示:

(17)

2.3 系统实现

本次组合导航系统采用松组合的方式,具有易实现和有冗余度的优点。SINS随时间增加造成误差积累,北斗卫星导航系统具有全天候、实时性高等优点,与SINS组合使用,二者能够互补。结构框图如图1所示。

图1 组合导航系统组成

首先需要对系统进行初始对准,是获取的高准确性和高实时性是高精度组合导航实现的前提。初始对准目的是获取载体的速度、位置和姿态信息。然后对SINS数据进行解算,得到飞行体的位置、速度和姿态信息,并把获取的北斗卫星数据进行坐标变换。由于SINS更新速度快于北斗卫星数据,需要判断北斗卫星是否进入量测更新,以保证修正数据的准确性。在更新时,用北斗卫星数据校正SINS数据,使用AEKF算法,对量测噪声进行估计;在非量测更新阶段,单独采用SINS对飞行体进行定位。

具体硬件系统包括惯性测量模块、弹载计算机模块和数据记录模块。惯性测量模块包括陀螺仪、三轴加速度计;弹载计算机采用Cortex-M7+FPGA的架构进行设计;数据记录仪用于数据存储。具体硬件实物如图2所示。

图2 硬件实物图

3 仿真分析

为验证本次改进算法在高速自旋飞行体上的可行性,利用Matlab软件对实际载体运动的数据进行仿真分析。

载体运动轨迹为抛物线,实地观测结果为飞行高度约为2 000 m,飞行距离约为15 000 m,飞行总时长为46.5 s,发射角为25°。SINS的更新速率为1 ms,北斗卫星的更新速率为100 ms。仿真参数的误差描述如表1所示。实际陀螺仪和加速度计的测量数据如图3和图4所示。

表1 仿真参数误差

图3 陀螺仪测量数据

图4 加速度计测量数据

本文采用组合导航,使用AEKF滤波方法对SINS和北斗的数据进行融合,对比未改进与改进的AEKF算法对滤波误差的影响。

3.1 AEKF算法对量测噪声估计

本文经过分析,采用AEKF算法对量测噪声协方差矩阵进行估计。组合导航轨迹东向-地向平面信息、位置误差和速度误差如图5～图7所示。遗忘因子取0.9。

图5 组合导航轨迹东向-地向平面信息

图6 算法改进前的位置误差

图7 算法改进前的速度误差

由图5可以看出,SINS信息没有被修正过来,在后期误差越来越大,与北斗的观测数据有一定偏差。从图6和图7也可以看出,误差越来越大,没有趋于稳定。位置误差在30 s时开始不断上升,地方向上最后下降,整体偏高;速度误差在20 s时整体上升。因此,以上方法的滤波效果不佳。

3.2 改进的AEKF算法

由上一组为改进的AEKF算法仿真结果可知,组合导航滤波出现明显偏差,结束点的位置与北斗的量测位置相距1 400 m。因此,采用改进的AEKF算法进行滤波。位置误差、速度误差如图8和图9所示。遗忘因子取0.9。

图8 b=0.9时的位置误差

图9 b=0.9时的速度误差

由图8可以看出,位置误差渐趋平稳,且均收敛,虽然在35 s时,有次波动,但很快调节,恢复收敛,与未改进AEKF算法对比明显,且误差大范围下降,最后北向位置误差保持在3 m 范围内,东向误差平均保持在6 m 范围内。由图9可知,速度误差也有较大改善,在北向、东向两方向上分别收敛于2 m/s 和14 m/s,效果明显。地向速度改动不明显。

再次调节遗忘因子,改善系统滤波能力。经过反复调试,取遗忘因子为0.3,得到位置、速度误差如图10和图11所示。

图10 b=0.3时的位置误差

图11 b=0.3时的速度误差

经过调节遗忘因子参数,由图11可知,速度误差进一步改善,东向误差再次下调位置误差改变不明显,但位置、速度误差保持稳定。经过此次滤波算法调节,组合导航运动轨迹如图12所示。

图12 组合导航运动轨迹图

组合导航轨迹表明,滤波算法有效,将SINS数据在量测更新阶段校正过来,避免误差干扰,与图5对比,有明显改善。组合导航轨迹跟随北斗量测的数据进行调整。由上述未改进AEKF算法与不同参数调整的改进AEKF算法,可以得出,位置误差由发散,最终北向位置误差为 2 m,东向位置误差为6 m,地向位置误差7 m。北向、东向速度误差从发散调整为收敛,最终北向速度误差为2 m/s 范围内,东向速度误差为 10 m/s。由此表明,本文改进的AEKF滤波算法行之有效,能够保证组合导航滤波的稳定性。

4 结 论

本文研究了基于AEKF算法的高速自旋飞行体组合导航方法,针对EKF算法中系统存在的噪声问题,引入了自适应器,添加遗忘因子对量测噪声进行估计,并对EKF的建模做了调整,每次计算过程中不必重复计算雅可比矩阵,提高系统的实时性,采用滤波算法控制,对高速自旋飞行体的组合导航控制中具有轻量级的优点。通过AEKF算法与改进的AEKF算法对比,改进的算法能够明显降低误差,并保证误差渐趋稳定,进入收敛。通过本次仿真,得到AEKF算法能够提高导航精度,但还能够进一步改善,后续工作继续调整算法,并且采用其他算法进行修正,如无迹卡尔曼算法等。本文仅对系统的量测噪声进行修正,能够自适应估计协方差矩阵,保证了北斗/SINS组合导航的定位精度,并且提高系统的稳定性。