陆用惯性导航系统非线性对准方法

王志伟，王风杰，狄长春，石志勇，杨功流

1.陆军工程大学 石家庄校区，石家庄 050003 2.63870部队，华阴 714200 3.北京航空航天大学 仪器与光电工程学院，北京 100083

初始对准的精度和实时性决定了惯性系统的导航效果[1-3]。随着对导航精度要求的不断提高，在小失准角条件下的线性系统模型不再适用[4-6]。近年来非线性滤波方法不断涌现，在滤波的过程中需要建立非线性误差模型，但是造成系统非线性的原因复杂多变，建模过程既不准确又浪费时间。文献[7-8]将非线性模型线性化，利用卡尔曼滤波(KF)直接处理，但是导致了滤波维数增加，不仅增大了计算量，而且使得各个状态变量之间产生耦合和约束。为此出现了基于概率分布的滤波方法，如粒子滤波(PF)、无迹卡尔曼滤波(UKF)等，但是PF自身存在一些无法避免的问题，如粒子退化、样本贫化以及计算量大等[9]，而UKF一般不适用于状态非高斯分布的系统模型，限制了其适用范围[10]。目前，各种神经网络算法被用于导航过程中，但是神经网络有一个致命的弱点，就是容易陷入局部极小点，并且需要依靠耗时的迭代算法来实现，不仅训练时间长，而且算法精度有待提高[11-12]。

另外，现阶段大部分初始对准方法需要依次进行粗对准和精对准，不仅耗时，而且要在静止的条件下进行，使载体的机动性大打折扣[13-15]。

快速正交搜索(Fast Orthogonal Search, FOS)算法有效避免了上述问题的发生，FOS算法是由美国学者Korenberg在1989年提出的[16]，该算法类似于有导师的神经网络算法，需要已知的训练数据，并通过最大限度地减小估计量和训练量之间的均方误差实现参数估计。相比神经网络算法，FOS算法不需要频繁的迭代过程，而是通过一次迭代就能确定出合适的系统模型项。相比传统的最小二乘方法，通过FOS算法所建立的系统模型中包含更少的模型项，这就大大减小了噪声的干扰，提高了估计精度[17-20]。目前，FOS算法在许多领域中已有应用，包括信号压缩、磁共振成像、正电子发射断层扫描、非线性系统控制以及基因识别技术等，但是FOS算法尚未在导航领域应用。

现阶段在有卫星信号辅助的情况下，炮载导航系统初始对准可以达到很高的精度。但是在遮蔽的情况下卫星信号不稳定，并且在战场环境下有多种手段和方法可以屏蔽卫星信号，使卫星信号在数个小时内不可用。为了使惯性导航系统在没有卫星信号情况下的初始对准能达到较高的精度，本文将结合KF和FOS两种算法，提出了FOS/ KF组合估计方法。考虑到卫星信号不可用的情况是在较短时间内，大部分时间段卫星信号是可用的，所以，在有卫星信号的情况下，利用FOS算法、卫星信息以及KF的预测值进行训练，建立起当前非线性模型。然后，当卫星信号因为种种原因不可用时，利用训练好的非线性模型进行初始对准和自主导航。整个过程可以在载体机动的情况下进行，简化了对准过程，有效提高了载体机动性。

1 大方位失准角误差模型

设水平失准角δp、δr为小角度，方位失准角δA为任意角度。

速度和姿态误差变为

(1)

(2)

由于捷联惯导系统在对准的过程中既包含了线性部分又包含了非线性部分，所以状态空间模型建立如下

(3)

(4)

在大失准角对准过程中，非线性状态变量仅为方位角，令经过KF估计过后的剩余非线性失准角为ΔA,航位推算所得的方位角为AINS,KF估计得到的方位失准角为δAKF,经过GPS量测所得的方位角为AGPS,由于卫星量测精度较高，这里认为AGPS为标准值。以上变量的关系为

ΔA=AINS-AGPS-δAKF

(5)

从式(5)可看出，ΔA中包含了几乎所有原因造成的系统非线性误差，若能对ΔA进行高精度估计，则可有效消除系统非线性带来的导航误差。为了对剩余非线性失准角进行有效估计，特提出了FOS/KF非线性参数估计方法。

2 快速正交搜索

2.1 系统模型

非线性系统可表示为

y(n)=

F[y(n-1),y(n-2),…,y(n-K),

x(n),x(n-1),…,x(n-L)]+ε(n)

(6)

式中：F为非线性函数；x(n-L)为系统输入；y(n-K)为系统输出；ε(n)为噪声；1

非线性系统式(6)可被表示为

(7)

式中：P0(n)=1,Pm(n)为任意阶次的候选函数，由式(6)中系统右侧的输入、输出或者其向量积组成；am为各个候选函数的权值系数；M为模型中最终选出的候选函数个数(需要人工设定)。

Pm(n)=y(n-k1)y(n-k2)…y(n-ki)·

x(n-l1)x(n-l2)…x(n-li)

(8)

式中：m≥1；1≤k1≤K,1≤k2≤K, …, 1≤ki≤K;0≤l1≤L, 0≤l2≤L, …, 0≤lj≤L;i≥0、j≥0分别为系统输入和输出的维数；系统的均方误差MSE可表示为

(9)

式(9)中变量上面的直线表示取从n=N0到n=N的平均值。该算法的核心原则就是根据均方误差的大小来确定系统模型项Pm(n)。

2.2 详细搜索过程

将式(7)中的系统进行Gram-Schmidt(GS)正交化得到

(10)

式中：gm为wm(n)的权值系数；wm(n)为Pm(n)经过GS正交化得到的函数序列；正交化过程为

(11)

(12)

此时，均方误差和正交权值系数变为

(13)

(14)

最大的Qm所对应的候选函数Pm(n)被选为模型项，将所选模型项加入到模型中后，均方误差为MSEm=MSEm-1-Qm,称MSEm为剩余均方误差。显然，FOS筛选原则就是选出对均方误差影响最大的候选函数作为模型项。在挑选出每一个模型项后，将其从候选函数中去除，然后在剩余的候选函数中继续进行筛选，直到完成搜索。

完成搜索的条件有3个：

1) 当剩余均方误差MSEm足够小时完成搜索。

2) 当误差模型项达到设定上限时完成搜索。

3) 当剩余候选函数不能使均方误差产生足够大的减小量时完成搜索。

当搜索完成后，利用αm r和gm计算出与所选模型项对应的权值系数am,过程为

(15)

综上，FOS的基本原理可以定义为：利用GS正交化方法建立正交系数αm r,然后利用αm r计算权值系数gm,并根据对均方误差影响最大的原则选出候选函数Pm(n),最后将对应的gm与系统输出y(n)进行关联，计算出正交化之前的权值系数am。

如果利用GS正交化逐个计算wm(n),会耗费很大的存储空间和计算时间。为了避免上述问题的发生，FOS在计算过程中仅对正交系数αmr进行计算。

类比式(11)定义函数D(m,r)和C(m),如式(16)和式(19)所示，表示Pm(n)、wr(n)和y(n)三者之间的关系为

(16)

(17)

(18)

式中：m=1,2,…,M;r=1,2,…,m。

(19)

(20)

将αm r、gm和MSE的减小量Qm重新表示为

(21)

(22)

m=1,2,…,M;r=1,2,…,m

(23)

显然，FOS在不需要逐个计算正交函数wm(n)的情况下，也可以利用正交空间得出系统输出y(n)。

FOS简要计算过程如下：

1) 将候选函数序列Pm(n)正交化，得到正交序列wm(n)。

2) 计算所得正交序列wm(n)的权值系数gm、正交系数αm r和MSE的减小量Qm,对Qm进行筛选，选出最大的Qm所对应的候选函数Pm(n)作为模型项，并将其在候选函数序列中去除。

3) 重复1)和2)直到满足搜索完成的条件为止。

4) 计算正交化之前的权值系数am,构建系统模型。

相比数学方法，FOS更加直接地作用于误差参数本身。在有卫星信号的时候，多种信息可以在线训练FOS。同时，以均方误差为标准，选取合适的候选函数，使得FOS能够准确地感知出误差项及其权重系数。这样，尽管训练时间很短，FOS依旧可以在没有先验信息的情况下较好地反映出当前误差模型的主要特征。与其他方法不同的是，FOS无需模拟动态非线性系统，而是根据模型项的权重以数学模型的形式将其表示出来，并利用所构建的数学模型在线更新非线性系统模型。如此，通过FOS构建的系统模型可以保证在动态条件下的估计精度。另外，只要能保证训练数据的准确性，无论失去卫星信号的时间长与短，FOS都可以提供高精度的估计结果。

3 FOS/KF组合估计方法

在导航解算过程中加入FOS算法，首先利用KF对系统的线性误差进行估计，然后利用FOS估计出剩余系统非线性误差，形成了FOS/KF方法。通过第2节的分析可知，式(5)中的剩余非线性失准角ΔA中包含了几乎所有原因造成的系统非线性误差，所以这里利用FOS算法对ΔA进行估计。各个误差项的关系如式(5)所示。

整个过程分为2个阶段，第1个阶段是模型训练阶段，在GPS信息可用时进行，目的是通过外部高精度信息对系统进行训练，得到由多个候选函数pm(n)组成的系统模型；第2个阶段是估计阶段，GPS信息不可用时进行，利用在模型训练阶段训练好的系统模型进行相应的参数估计。

3.1 模型训练阶段

当GPS信息可用时，将式(5)得出的剩余非线性失准角ΔA作为输出，将KF预测值作为输入，组成一组训练值，进行模型训练，训练流程如图1所示。

图1中：PINS、VINS和θINS为航位推算得到的位置、速度、姿态信息;PGPS、VGPS和θGPS为GPS量测信息;δPKF、δVKF和δθKF为卡尔曼滤波的估计值。

为对应第2节中的各个变量，令输入维数i=9,输出维数j=3,采样次数N=500,M=30,N0=5。

候选函数总的个数约为80 000,最大候选函数个数为30，建模平均耗时0.418 7 s，建模最大耗时0.631 2 s。

图1 模型训练流程
Fig.1 Flowchart of model training

3.2 预测阶段

当失去GPS信号时，系统不能直接求得精确的剩余非线性失准角ΔA,此时以KF的估计值δPKF、δVKF和δθKF为输入，利用模型训练阶段训练好的系统模型输出剩余非线性失准角ΔA,并将剩余非线性失准角ΔA、航位推算得到的姿态AINS、KF滤波得到的姿态误差δAKF按照A=AINS-ΔA-δAKF的关系进行解算，最终得到当前准确姿态信息A,误差预测流程如图2所示。

图2 误差预测流程
Fig.2 Flowchart of error prediction

4 仿真分析

为了验证FOS/KF方法的正确性，分别用扩展卡尔曼滤波(EKF)和FOS/KF两种方法对大/小失准角进行滤波，在滤波阶段EKF采用卫星所提供的速度信息。由于水平失准角的收敛速度和精度都很高，所以这里只对方位失准角进行仿真对比。

4.1 静基座对准

在静基座条件下，设定仿真时间为600 s，大方位失准角为30°，小方位失准角为1°，在两种条件下分别进行3次仿真。仿真结果如图3和表1所示，其中图3(a)为小方位失准角情况下两种方法的第1次仿真结果，图3(b)为大方位失准角情况下两种方法的第1次仿真结果。

从图3中可看出，EKF的收敛时间为80 s左右，而FOS/KF方法的收敛时间可以保持在30 s以内，收敛速度和收敛精度都远优于EKF。

由表1中结果可看出，在小失准角的情况下，两种方法的误差都很小，都可以达到较高的对准精度。在大失准角的情况下，EKF的误差显著增大，已不能满足精对准的需求，而FOS/KF方法依然保持了较高的估计精度。

图3 静基座条件下方位失准角误差
Fig.3 Azimuth misalignment angle error under condition of stationary base

方法均方根误差方位失准角为30°方位失准角为1°第1次第2次第3次第1次第2次第3次EKF1.033419652.424837550.83054310.07387010.035442100.03427545FOS/KF0.018444750.012973950.00606380.00731440.002878550.00241010

4.2 动基座对准

在动基座条件下，仿真时间与静基座相同，依旧对大方位失准角30°和小方位失准角1°进行3次仿真。图4为模拟跑车路径，在路径中包含了静止、加速、匀速、减速、直线和曲线等运动方式。仿真结果如图5和表2所示，图5分为小方位失准角和大方位失准角时的失准角误差的第1次仿真结果。

图4 仿真路径
Fig.4 Simulation path

从图5可看出，EKF的收敛时间为200 s，而FOS/KF方法的收敛时间可以保持在80 s以内，FOS/KF方法的收敛速度和收敛精度都有较大优势，这与静基座条件下的仿真结果基本一致。但是仿真结果的收敛效果均不如静基座条件下的平滑，这是因为在行进间对准过程中许多参数是时变的，这对对准结果会造成影响。

由表2中的结果可看出，在小失准角的情况下，两种方法的误差都很小，都可以达到较高的对准精度。在大失准角的情况下，EKF的误差显著增大，与静基座条件下的仿真结果一致。

对比表1和表2中结果，在进行大失准角对准时，由于行进间对准的过程中基座是运动的，以及在仿真过程中加入了加大的干扰项，所以失准角误差普遍比静基座条件下的结果偏大；在进行小失准角对准时，失准角误差基本与静基座条件下的仿真结果在同一数量级上，说明基座的运动对大失准角对准的影响大，对小失准角对准的影响小。另外，无论基座运动与否，EKF在进行大失准角对准时已不能满足精对准的需求，而FOS/KF方法依然保持了较高的估计精度，说明在大失准角的情况下进行初始对准，FOS/KF方法仍然适用，并且不需要事先进行粗对准。

图5 动基座条件下方位失准角误差
Fig.5 Azimuth misalignment angle error under condition of moving base

方法均方根误差方位失准角为30°方位失准角为1°第1次第2次第3次第1次第2次第3次EKF1.624192982.83486712.54428790.05324720.03902470.0778219FOS/KF0.035057720.06385430.01657120.00134250.00732520.0057634

5 实车试验

为了对所提方法进行充分验证，整个试验被分为11个阶段，进行11次重复验证，每个阶段时长为800 s，前500 s所采集的数据为训练数据，后300 s采集的数据为验证数据，整个试验过程时长12 329 s。

图6为跑车的详细数据，包括跑车路径、车速以及跑车里程等信息，图7为炮载惯性导航系统(采样频率为100 Hz)，图8为自行火炮外部搭载的GPS天线(采样频率为1 Hz)和电台天线，图9为位于火炮内部的数据处理设备。

从图6中速度数据可明显看出车辆每隔一段时间就会停止一次，停止过后进行下一次数据采集，一共11次。

图10为事后从北斗双天线测姿系统中提取的真实方位角数据，用来计算11个阶段的方位角误差，计算结果如图11、图12和表3所示。

由于每个阶段跑车环境是不断变化的，所以跑车过程中系统非线性的程度也在不断变化，故从图11和图12可看出EKF的估计误差随着系统非线性程度的变化而变化，而FOS/KF方法的估计误差比较平稳，基本不随系统非线性程度变化，说明FOS/KF方法受外界环境的影响小，针对线性系统和非线性系统有同等的估计效力。

从表3中的数据可看出，在整个试验中EKF最大的估计误差达到14.99°，最小值为2.51°，平均误差为4.89°，不仅受环境的影响较大，而且其估计精度远不能达到初始对准的要求。FOS/KF在各种工况下可以将方位误差角控制在0.8°以内，并且总体变化不大，能较好地适应各种跑车环境。

试验结果表明，在误差参数和非线性程度时变的情况下， FOS/KF可以始终保持较高的对准精度，对导航和对准精度的提高有很大帮助。为验证FOS/KF方法对长时间的自主导航仍旧有较大帮助，下一步将会以优化训练耗时、探索参数变化对训练效果的影响为研究方向，完善FOS/KF非线性滤波方法在实际应用中的各项机理研究。

图7 炮载惯性导航系统
Fig.7 Gun-board SINS

图8 GPS天线和电台电线
Fig.8 GPS antenna and radio antenna

图9 数据处理设备
Fig.9 Data processing equipment

图10 真实方位角
Fig.10 Real azimuth angle

图11 方位角误差的最大值
Fig.11 Maximum azimuth angle error

图12 方位角误差的均方根
Fig.12 RMS of azimuth angle error

序号方位角误差/(°)EKFFOS/KFMaxRMSMaxRMS114.9910.930.760.46211.956.260.960.2235.674.600.280.1442.511.690.300.1552.642.110.430.3165.444.190.520.30710.305.090.640.3884.162.191.060.5797.504.940.600.311013.406.231.920.47116.875.561.190.80平均值7.764.890.7872730.37

6 结 论

1) 在进行小失准角对准时，失准角误差基本与静基座条件下的仿真结果在同一数量级上，说明基座的运动对大失准角对准的影响大，对小失准角对准的影响小。

2) 无论基座的运动与否，EKF在进行大失准角对准时已不能满足精对准的需求，而FOS/KF方法依然保持了较高的估计精度，说明在大失准角的情况下进行初始对准，FOS/KF方法仍然适用，并且不需要事先进行粗对准。

3) 在误差参数和非线性程度时变的情况下，FOS/KF方法可以始终保持较高的对准精度，不随系统非线性程度的变化而变化。

[1] SAVAGE P G. Strapdown analytics[M]. Minn: Strapdown Associates, 2000: 331-332.

[2] GAIFFE T. From R&D brass board to navigation grade FOG-based INS: The experience of Photonetics/Ixsea[C]∥Optical Fiber Sensors Conference Technical Digest. Piscataway, NJ: IEEE Press, 2012: 1-4.

[3] NAPOLITANO F, GAIFFE T, COTTREAU Y, et al. PHINS: The first high performances inertial navigation system based on fiber optic gyroscopes[C]∥International Conference on Integrated Navigation Systems. Piscataway, NJ: IEEE Press, 2002: 296-304.

[4] EI-SHEIMY N, NASSAR S, NOURELDIN A. Wavelet de-noising for IMU alignment[J]. IEEE Aerospace and Electronic Systems Magazine, 2004, 19(10): 32-39.

[5] ARASA I, HAYKIN S. A numerical-integration perspective on Gaussian filters[J]. IEEE Trans on Automatic Control, 2009, 54(8): 1254-1269.

[6] WAN E A, VAN D, MERWE R. The unscented kalman filter for nonlinear estimation[C]∥Adaptive System for Signal Processing Communication and Control, Piscataway, NJ: IEEE Press, 2000: 1389-1392.

[7] YU M J, PARK H W, JEON C B. Equivalent nonlinear error models of strapdown inertial navigation system[C]∥ ION GPS/GNSS 2003. Manassas, VA: ION Publications, 2003: 188-198．

[8] 李东明. 捷联式惯导系统初始对准方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2006: 85-88.

LI D M. Study on initial alignment methods of strapdown inertial navigation system[D]. Harbin: Harbin Engineering University, 2006: 85-88 (in Chinese).

[9] 王志伟, 石志勇, 全振中. 惯性导航在线标定滤波技术[J]. 火力与指挥控制, 2014, 39(12): 2061-2065.

WANG Z W, SHI Z Y, QUAN Z Z. A survey of inertial navigation on-line calibration filtering technique[J]. Fire Control & Command Control, 2014, 39(12): 2061-2065 (in Chinese).

[10] HASSIBI B, SAYED A H, KAILATH T. Linear estim- ation in Krein spaces—part2: Application[J]. IEEE Transactions on Automatic Control, 2016, 41(1): 34-49.

[11] WANG X Y, HUANG Y. Convergence study in extended Kalman filter-based training of recurrent neural networks[J]. IEEE Transactions on Neural Networks, 2016, 22(4): 588-600.

[12] GAN X S, DUANMU J S, CONG W. Wavelet neural network based on ssukf and its applications in aerodynamic force modeling for flight vehicle[C]∥Proceeding of the 2000 National Technical Meeting of the Institute of Navigation. Anaheim, CA: Institute of Navigation, 2000: 574-582.

[13] 王巍. 惯性技术研究现状及发展趋势[J]. 自动化学报,2013, 39(6): 723-728．

WANG W. Status and development trend of inertial technology[J]. Acta Automatic Sinica, 2013, 39(6): 723-728 (in Chinese).

[14] WU Y X, WU M P, HU X P, et al. Self-calibration for land navigation using inertial sensors and odometer: Observability analysis[C]∥AIAA Conference of Guidance, Navigation and Control. Reston, VA: AIAA, 2013: 1-10.

[15] WU Y X, GOODALL C, EI-SHEIMY N. Self-calibration for IMU/Odometer land navigation: Simulation and test results[C]∥Proceedings of ION/ITM. Manassas, VA: ION Publications, 2015: 839-849.

[16] KORENBERG M J. A Robust orthogonal algorithm for system identification and time series analysis[J]. Biological Cybernetics, 1989, 60(4): 267-276．

[17] KORENBERG M J, PAARMANN L D. Applications of fast orthogonal search: Time-series analysis and resolution of signals in noise[J]. Annals of Biomedical Engineering, 1989, 17(3): 219-231．

[18] KORENBERG M J. Identification nonlinear difference equation and functional expansion representations: The fast orthogonal algorithm[J]. Annals of Biomedical Engineering, 1988, 16(4): 123-142.

[19] KORENBERG M J, PAARMANN L D. Orthogonal approaches to time-series analysis and system identification[J]. IEEE Signal Processing Magazine, 1991, 8(3): 29-43．

[20] MIKAEL E J, KORENBERG M J, JAMES M P. Nonlinear system identification and control of chemical processes using fast orthogonal search[J]. Journal of Process Control, 2017, 17(9): 742-754．

[21] 高伟. 捷联惯性导航系统初始对准技术[M]. 北京： 国防工业出版社, 2014: 105-106.

GAO W. Initial alignment for strapdown inertial navigation system[M]. Beijing： National Defense Industry Press, 2014: 105-106 (in Chinese).