基于快速正交搜索的車載導航方法

王志偉，秦俊奇，楊功流，石志勇，狄長春，王風杰

（1.軍械工程學院，石家莊 050003；2.北京航空航天大學，北京 100191；3.陜西華陰63870部隊，華陰 714200）

基于快速正交搜索的車載導航方法

王志偉1，秦俊奇1，楊功流2，石志勇1，狄長春1，王風杰3

（1.軍械工程學院，石家莊 050003；2.北京航空航天大學，北京 100191；3.陜西華陰63870部隊，華陰 714200）

以某型自行火炮炮載慣導系統為研究對象，針對系統剩余高階非線性誤差得不到有效補償的問題，結合卡爾曼濾波（KF），提出了基于快速正交搜索（FOS）的組合估計方法，既消除了線性誤差，也對系統的高階非線性誤差起到了良好的抑制作用。試驗結果表明，在沒有衛星信號的情況下，與單獨使用KF相比，FOS/KF可以有效提高導航精度，經過補償后的平均水平速度誤差僅為0.034 m/s，定位誤差可基本保持在10 m以內，實現了非線性條件下的高精度自主導航。

自行火炮；非線性誤差；快速正交搜索算法；捷聯慣性導航系統

在有衛星信號輔助的情況下，炮載導航可以達到很高的精度。但是在遮蔽的情況下衛星信號不穩定，并且在戰場環境下有多種手段和方法可以屏蔽衛星信號，甚至使衛星失靈，從而導致武器裝備不能進行有效導航[1-3]。針對以上問題，自主導航成為現階段研究的熱點之一，卡爾曼濾波（KF）作為一種有效的參數估計手段，廣泛應用到多種自主導航的過程中[4-5]。眾所周知，KF只能在線性環境下工作，一旦系統發生非線性，KF就會產生非線性估計誤差，導致導航誤差增大。而在實際導航過程中，很難保證系統模型線性[6-8]。為了避免建立模型帶來的估計誤差，各種神經網絡算法相繼被提出，但是神經網絡有一個致命的弱點，就是容易陷入局部極小點，并且需要依靠耗時的迭代算法來實現，不僅訓練時間長，而且算法精度有待提高[9-10]。本文利用快速正交搜索算法（Fast orthogonal search,FOS）有效避免了上述問題的發生。FOS算法是由美國學者Korenberg在1989年提出的[11]，該算法類似于有導師的神經網絡算法，需要已知的訓練數據，并通過最大限度地減小估計量和訓練量之間的均方誤差實現參數估計。但是相比神經網絡，FOS不需要頻繁的迭代過程，而是通過一次迭代就能確定出合適的系統模型項。相比傳統的最小二乘方法，通過FOS所建立的系統模型中包含更少的模型項，這就大大減小了噪聲的干擾，提高了估計精度[12-15]。目前，FOS算法在許多領域中已有應用，包括信號壓縮、磁共振成像、正電子發射斷層掃描、非線性系統控制以及基因識別技術等，但是FOS算法在導航領域的應用幾乎沒有。

本文將FOS算法與KF相結合，分析了炮載慣導應用環境，在有衛星信號時，利用衛星數據作為FOS的訓練數據，進行系統模型訓練，在沒有衛星信號時利用訓練所得的系統模型進行參數估計，實現了非線性條件下的高精度導航。

1 系統非線性誤差

式中：M為子午圈半徑；N為卯酉圈半徑；為加速度計輸出；為陀螺輸出；o(·)為相關誤差參數的高階項。由于KF只適用于線性系統，所以非線性系統在經過KF估計補償后仍然含有剩余高階非線性誤差項，即o(·)導致了剩余高階非線性誤差項的產生。

為了避免KF在使用過程中的局限性，提高導航精度，有必要對相關誤差參數的高階項o(·)進行補償。建立非線性模型如下：

所提方法具有以下特點：

1） 該方法可以估計主要的非線性誤差參數以及與其對應的權值系數；

2） 當GPS可用時，以GPS信息為訓練值，可以建立與當前系統高度匹配的非線性誤差模型；

3） 當GPS不可用時，可以利用2）中建立的誤差模型進行誤差估計；

4） 該方法具有很高的實時性。

2 快速正交搜索

2.1 系統模型

非線性系統可表示為如下形式：

式中：F為非線性函數；x(n-L)為系統輸入，y(n-K)為系統輸出，ε(n)為系統誤差，為計算過程的時間延遲，采樣次數

非線性系統(1)可被表示為

式中：i、j分別為系統輸入和輸出的維數。系統的均方誤差（MSE）可表示為：

式(9)中的直線表示取從n=N0到n=N的平均值。該算法的核心原則就是根據均方誤差的大小來確定系統模型項

2.2 詳細搜索過程

將式(1)中的系統進行Gram-schmidt（GS）正交化得到：

此時，均方誤差和正交權值系數變為：

完成搜索的條件有三個：

1） 當剩余均方誤差MSEm足夠小時完成搜索；

2） 當誤差模型項達到設定上限時完成搜索；

3） 當剩余候選函數不能使均方誤差產生足夠大的減小量時完成搜索。

當搜索完成后，利用αmr和gm計算出與所選模型項對應的權值系數am，過程如下：

綜上，FOS的基本原理可以定義為：利用GS正交化方法建立正交系數αmr，然后利用αmr計算權值系數gm，并根據對均方誤差影響最大的原則選出候選函數最后將對應的gm與系統輸出y(n)進行關聯，計算出正交化之前的權值系數am。

類比式(11)定義函數D(m,r)和C(m)，表示三者之間的關系如下：

將式αmr、gm和MSE的減小量Qm重新表示如下：

FOS簡要計算過程如下：

3） 重復步驟1）和2）直到滿足搜索完成的條件為止；

4） 計算正交化之前的權值系數am，構建系統模型。

相比數學方法，FOS更加直接地作用于誤差參數本身。在有衛星信號的時候，多種信息可以在線訓練FOS。同時，以均方誤差為標準，選取合適的候選函數，使得FOS能夠準確地感知出誤差項及其權重。這樣，盡管訓練時間很短，FOS依舊可以在沒有先驗信息的情況下較好地反映出當前誤差模型的主要特征。與其他方法不同的是，FOS無需模擬動態非線性系統，而是根據模型項的權重以數學模型的形式將其表示出來，并利用所構建的數學模型在線更新非線性系統模型。如此，通過FOS構建的系統模型可以保證在動態條件下的估計精度。另外，只要能保證訓練數據的準確性，無論失去衛星信號的時間長與短，FOS都可以提供高精度的估計結果。

3 FOS/KF組合估計方法

在導航解算過程中加入FOS算法，首先利用KF對系統的線性誤差進行估計，然后利用FOS估計出剩余系統非線性誤差，形成了 FOS/KF算法。式(1)～(3)中均包含o(δP)和o(δV)，表明非線性位置和速度誤差會影響導航結果，而位置誤差又源于速度誤差，所以這里利用FOS對速度誤差的非線性相關項進行估計。各個誤差項的關系如下：

式中：ΔV為剩余非線性速度誤差；VINS為導航解算得到的速度；VGPS為衛星測量得到的速度；δVKF為KF濾波得到的速度誤差。

整個過程分為兩個階段：第一個階段式模型訓練階段，在GPS可用時進行，目的是通過外部高精度信息對系統進行訓練，得到由多個候選函數組成的系統模型；第二個階段是估計階段，GPS不可用時進行，利用在模型訓練階段訓練好的系統模型進行參數估計。

3.1 模型訓練階段

當GPS信息可用時，將式(25)得出剩余非線性速度誤差ΔV作為輸出，將 KF預測值作為輸入，組成一組訓練值，進行模型訓練。訓練流程如圖1所示，其中，為導航解算得到的位置、速度、姿態信息，為GPS量測信息，為卡爾曼濾波的估計值。

為對應第3節中的各個變量，令輸入維數i=9，輸出維數j=3，采樣次數N=500，M=30，N0=5，候選函數總的個數約為80000。

圖1 模型訓練示意圖Fig.1 Model training sketch

3.2 預測階段

每當失去GPS信號時，系統不能直接求得精確的剩余非線性速度誤差ΔV，此時FOS按照4.1中的過程進行在線建模，結合 KF預測值對ΔV進行估計，補償速度輸出，并利用補償后的速度信息計算當前的位置信息，具體流程如圖2所示。

圖2 誤差預測示意圖Fig.2 Error prediction

4 實車試驗

為驗證FOS/KF方法的可行性，進行了實車試驗，試驗于2017年1月在北京進行，行車路線選取北京西北六環，載車為某型輪式自行火炮，試驗對象為該型自行火炮所搭載的慣性導航系統，慣組更新頻率為100 Hz，陀螺常值漂移為 0.05 (°)/h，量測范圍為 300(°)/s，隨機游走系數為加速度計零偏為150 μg，量測范圍為±10g，隨機噪聲為GPS水平定位精度為5 m，高程定位精度為10 m，測速噪聲為0.1 m/s。

4.1 跑車條件

為了對所提方法進行充分驗證，整個試驗被分為11個階段，進行11次重復驗證。每個階段時長為800 s，前500 s所采集的數據為訓練數據，后300 s采集的數據為驗證數據，整個試驗過程時長12 329 s。在每個階段開始前需要停車關機不少于2 min，確保每個階段具有相同的初始物理狀態。

圖3為跑車的詳細數據，包括跑車路徑、車速以及跑車里程等信息。圖4為炮載慣性導航系統（采樣頻率為100 Hz）。圖5為自行火炮外部搭載的GPS天線（采樣頻率為1 Hz）和電臺天線。圖6為位于火炮內部的數據處理設備。從圖中速度數據可明顯看出，車輛每隔一段時間就會停止一次，停止過后進行下一次數據采集，一共11次。

圖3 跑車路徑及詳細信息Fig.3 Test path and detailed information

圖4 炮載慣導Fig.4 Gun-board SINS

圖5 GPS天線Fig.5 GPS antenna

圖6 數據處理設備Fig.6 Data processing equipment

4.2 試驗結論

表1 速度誤差對比Tab.1 Contra st on velocity errors

表1為每個階段三個方向的平均速度誤差。對比兩種方法的估計結果，當使用KF單獨估計時，三個方向的平均速度誤差分別為0.354 5 5 m/s、1.323 7 m/s、1.124 5 m/s。造成該誤差的主要原因是慣導自身的零偏和常值漂移，尤其是陀螺的常值漂移。盡管部分誤差已被KF估計出來，但是剩余的高階非線性誤差仍造成誤差隨著時間的推移快速增長。當使用FK/FOS 組合估計方法進行估計時，三個方向上的平均誤差減少到0.032 2 m/s、0.033 4 m/s、0.005 7 m/s。這說明FOS可以很好地對剩余高階非線性誤差進行估計和補償。

另外，對比每個方向11個階段的估計結果，在使用FK/FOS方法進行估計時，最大值僅為0.052 3 m/s、0.057 6 m/s、0.013 1 m/s。由于系統隨時間不斷變化，說明FOS估計方法可以很好地適應動態環境。

圖7～9分別為三個方向上速度誤差的對比曲線，可以看出，FK/FOS對實際系統的誤差抑制效果十分顯著。

圖7 平均東向速度誤差Fig.7 Average east velocity error

圖8 平均北向速度誤差Fig.8 Average north-velocity error

圖9 平均天向速度誤差Fig.9 Average up velocity error

表2中對比了兩種方法所得到的位置誤差，三個方向上的平均值分別為19.000 3 m、21.215 8 m、8.056 9 m和5.856 1 m、8.004 5 m、2.090 0 m。結果進一步說明了KF不能對剩余非線誤差進行有效估計，也說明了經過FOS估計和補償后，位置誤差得到了顯著改善。

圖 10～12分別為三個方向上位置誤差的對比曲線，由于位置信息是通過速度信息直接求取的，所以利用補償后的速度計算出的位置誤差有明顯減少。

圖10 平均東向位置誤差Fig.10 Average east position error

圖11 平均北向位置誤差Fig.11 Average north position error

圖12 平均天向位置誤差Fig.12 Average up position error

圖 13為載體運行過程中方向角的變化曲線。圖14為經過兩種方法估計補償后的方向角誤差對比圖，其中的各點處的數值對應 11個驗證過程中選取的各個過程中最大的方位角誤差值。顯然，方向角誤差的大小與剩余非線性誤差有著很大關系，這與式(2)一致。

圖13 方位角Fig.13 Azimuth

表2 位置誤差對比Tab.2 Contrast on position errors

圖14 每300 s中的最大方位誤差角Fig.14 Maximum azimuth in every 300 s

5 結 論

在分析慣導誤差的基礎上，提出將FOS算法加入到導航過程中，利用FOS對系統的剩余非線性誤差進行估計，與KF配合工作，形成了KF/FOS組合估計算法。對比分析兩種方法在實際跑車過程中的結果，說明KF/FOS算法可以有效估計系統剩余非線性高階誤差，并且通過對速度誤差的補償可以有效降低定位和測姿誤差，為動態條件下的高精度導航提供了方法和手段。

（References）：

[1]房建成, 申功勛.車載DR系統自適應擴展卡爾曼濾波模型的建立及仿真研究[J].中國慣性技術學報, 1998,6(3): 24-28.Fang J C, Shen G X.Modeling and simulation of the self-adaptive filtering in landing vehicle DR system[J].Journal of Chinese Inertial Technology, 1998, 6(3): 24-28.

[2]吳文啟, 何曉峰, 胡小平.導航技術發展研究[R].控制科學與工程科學發展報告, 2014: 1378-1381.Wu W Q, He X F, Hu X P.Highlights in navigation research[R].Development of Control Science and Engineering Science, 2014: 1378-1381.

[3]周永余, 陳永冰, 李文魁.21世紀初艦船組合導航系統的發展趨勢[J].海軍工程大學學報, 2002, 14(6): 46-51.Zhou Y Y, Chen Y B, Li W K.Development trend of the combined ship navigation system in the early part of the 21st century[J].Journal of Naval University of Engineering, 2002, 14(6): 46-51.

[4]Dissanayake G, Sukkarieh S, Nebot E, et al.The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[J].IEEE Transactions on Robotics and Automation, 2001,17(5): 731-747.

[5]Niu X, El-Sheimy N.Development of a low-cost MEMS IMU/GPS navigation system for land vehicles using auxiliary velocity updates in the body frame[C]//Proceedings of ION GNSS.CA, 2005: 2003-2012.

[6]Gustafsson F.Particle filter theory and practice with positioning applications[J].IEEE Aerospace and Electronic Systems Magazine, 2015, 25(7): 53-82.

[7]Regulski P, Terzija V.Estimation of frequency and fundamental power components using an unscented Kalman filter[J].IEEE Transactions on Instrumentation and Measurement, 2015, 61(4): 952-962.

[8]Pasha S A, Hoang D T, Vo B N.Nonlinear baysian filtering using the unscented linear fractional transformation model[J].IEEE Transactions on Signal Processing, 2013, 58(2): 477-489.

[9]Wang X Y, Huang Y.Convergence study in extended Kalman filter-based training of recurrent neural networks[J].IEEE Trans on Neural Networks, 2015, 22(4): 588-600.[10]Gan X S, Jing D M, Wei C.Wavelet neural network based on SSUKF and its applications in aerodynamic force modeling for flight vehicle[C]//International Conference on Measuring Technology and Mechatronics Automation.2013: 1087-1090.

[11]Korenberg M J.A robust orthogonal algorithm for system identification and time series analysis[J].Biological Cybernetics, 1989, 60(4): 267-276

[12]Korenberg M J.Identification nonlinear difference equation and functional expansion representations: the fast orthogonal algorithm[J].Annals of Biomedical Engineering,1988, 16, 123-142.

[13]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.

[14]Eklund J M, Korenberg M J, McLellan P J.Nonlinear system identification and control of chemical processes using fast orthogonal search[J].Journal of Process Control,2007, 17(9): 742-754.

[15]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.

[16]王志偉, 秦俊奇, 狄長春, 等.基于北斗差分定位的炮載慣導外場標定[J].中國慣性技術學報, 2016, 24(3):314-319.Wang Z W, Qin J Q, D C C, et al.Inertial navigation system’s field calibration based on Beidou-II differential positioning[J].Journal of Chinese Inertial Technology,2016, 24(3): 314-319.

The shaking error compensation for gun-board SINS

WANG Zhi-wei1, QIN Jun-qi1, YANG Gong-liu2, SHI Zhi-yong1, DI Chang-chun1, WANG Feng-jie3
(1.Ordnance engineering college, Shijiazhuang 050003, China;2.Beihang University, Beijing 100191, China; 3.Unit 63870 of PLA, Huayin 714200, China)

The gun-board strapdown inertial navigation system is taken as the research object.Because of the residual high order nonlinear error of the system can not be compensated effectively, a combination estimation method based on fast orthogonal search algorithm (FOS) is proposed, combined with Kalman filter (KF), which not only eliminates the linearity error, but also plays a good role in restraining the residual high-order nonlinear error of the system.The results show that, when GPS is outage, the navigation accuracy can be improved more effectively by FOS/KF compared with by KF-only.After the compensation, the average horizontal velocity error is only 0.034 m/s, the positioning error can be basically kept within 10 m, and the high precision autonomous navigation under nonlinear condition is realized.

self-propelled gun; nonlinear error; fast orthogonal search algorithm; SINS

V249.3

A

1005-6734(2017)05-0592-07

10.13695/j.cnki.12-1222/o3.2017.05.007

2017-05-20；

2017-08-28

國防預研基金（9140A09040112JB34111）

王志偉（1990—），男，博士，研究方向為慣性導航。E-mail: wzw505869351@126.com

聯 系 人：秦俊奇（1961—），男，教授，博士生導師。E-mail: Qjq1961@yeah.net