基于回算機制的GPS/INS時間延遲處理方法

汪 菁,孫立海,鄒瑜光

(神州五行航天科技有限公司，遼寧 大連 116020)

低成本制導炸彈作為一種精確對地攻擊彈藥，因其具有成本低、結構簡單、精度高、適合大量使用等特點，受到世界各國青睞[1-2]。為降低成本，低成本制導炸彈中常采用微機電系統(tǒng)[3](Micro Electromechanical Systems，MEMS)作為慣性測量器件進行慣性導航(Inertial Navigation System,INS)計算。但由于MEMS器件存在零偏漂移較大、零偏重復性差[4]等缺點，單獨使用會嚴重影響導航定位精度，因此低成本制導炸彈中常采用GPS接收機輸出定位和定速信息，通過卡爾曼濾波算法，對導航結果進行修正，即松耦合形式的GPS/INS組合導航。

采用卡爾曼濾波算法對來自不同器件的相關信息進行融合時，關鍵在于要保證融合的數據在時間點上的一致性[5-6]。由于GPS與INS工作頻率不同，且INS由于自身特性導致數據更新頻率也不是恒定值，因此采集數據不可能擁有同一時標。GPS接收機的輸出信息一般是經過接收機內部濾波算法處理后的結果，且由于數據轉換和串口傳輸速度等因素的影響，GPS與INS在進行數據融合時存在時間不同步的現(xiàn)象，往往只是同步到數據輸出時刻，而無法同步到GPS信息更新時刻，這將導致最終定位精度下降，使導航整體性能變差[7]。

為保證組合導航過程中數據融合的時間同步，國內外眾多學者提出了不同的處理方法，大體上可以歸結為硬件實現(xiàn)、軟件實現(xiàn)或軟硬件相結合的方法[8-10]。硬件上可采用秒脈沖(1 Pulse Per Second，1PPS)的方法控制采樣電路，或利用高精度計數器和INS脈沖實現(xiàn)GPS與INS的時間同步，但硬件實現(xiàn)方法通常需要增加額外的硬件，成本較高，且不適用于大多數已封裝好的INS測量單元，因此該方法的適用性較差。軟件上常結合卡爾曼濾波算法，在狀態(tài)變量中添加時間延時項[11-12]，利用插值、外推等算法對GPS輸出的速度、位置信息進行補償，采用補償后的信息進行數據融合，估計出時間延遲量[13]。這類方法由于增加了狀態(tài)變量個數，使運算量加大，導致計算時間延長，加大了處理器的負擔，對于低成本彈機處理器要求較高，容易造成輸出延遲。軟硬件相結合的方法則是利用多項式擬合[14-15]來描述GPS輸出，利用存儲的GPS信息和延遲時間計算多項式擬合系數，繼而獲得信息融合時刻真實的GPS輸出，從而與對應時刻的INS信息進行數據融合。理論上多項式次數越高，擬合精度越高，延遲補償效果越好，但同時也需要存儲更多的數據點。通常可根據載體運動的劇烈程度來選擇不同的擬合次數，但這在一定程度上增加了程序的復雜度。現(xiàn)有的軟件實現(xiàn)方法和軟硬件相結合的方法均不允許GPS數據存在丟失的情況，尤其是在載體運動狀態(tài)變化劇烈的時候，否則會引入較大的誤差，這對低成本彈機的性能提出了嚴格的要求。

不論利用何種方法，均不能完全消除時間延遲所帶來的影響。過度追求精密儀器和復雜的軟件補償方法，不僅對提高定位精度影響甚微，還會影響成本造價。因此從實際角度出發(fā)，相對簡易的補償方法更適用于低成本制導炸彈。本文在采用軟硬件相結合的方法的基礎上，提出一種回算機制，完成組合導航在GPS信息更新時刻的融合，在GPS數據輸出時刻輸出，減小時間不同步對系統(tǒng)定位精度的影響。同時，不額外添加卡爾曼濾波狀態(tài)變量，避免較大的計算量，在GPS信號丟失時仍可使用，不增加程序復雜度，適用性強，可滿足低成本制導炸彈及中低精度導航需求?？紤]到低成本處理器的能力限制，本文同時提出一種配合回算機制進行的工程計算優(yōu)化算法，在回算過程中取消卡爾曼濾波計算的時間更新操作，節(jié)約計算時間，緩解處理器在回算過程中的計算壓力，同時對定位精度不產生明顯影響，具有較強的工程實用性。

1 回算機制

圖1 間接法對應的卡爾曼濾波流程圖

(1)

(2)

(3)

(4)

Pk=(I-KkHk)Pk/k-1

(5)

在GPS/INS組合導航系統(tǒng)中，GPS信息尚未到來時，系統(tǒng)根據慣性器件采集的信息進行慣性導航計算，同時根據系統(tǒng)性能等參數完成時間更新操作；當GPS信息更新后，根據獲取的GPS量測信息對慣性導航結果進行修正，完成量測更新操作。由于GPS一幀數據較長，且串口傳輸速率有限，因此GPS數據并不是在接收瞬間完成輸出的，而是存在一定的時間延遲。根據GPS接收機設備的不同及處理器性能差異，通常一幀GPS數據時延為15～95 ms[17]。若GPS時延15 ms，載機掛彈以300 m/s速度飛行，則在進行量測更新操作時，GPS位置誤差在原有基礎上又增加4.5 m。持續(xù)的時延會對最終定位產生不利影響，因此需要對GPS時延進行補償。

通過對比可知，軟硬件相結合處理GPS時延的方法更適用于低成本制導炸彈，但現(xiàn)有的方法不允許GPS數據出現(xiàn)丟失，否則可能造成存儲數據量過大，導致存儲器壓力激增或插值擬合數據異常，從而影響導航計算結果。針對這種情況，結合卡爾曼濾波和GPS/INS組合導航的特點，本文提出一種回算機制，既可完成對GPS時延的補償，又適用于GPS數據短暫丟失的異常情況。

具體方法如下：依次調取MEMS模塊和GPS模塊，并采集數據。利用硬件對采集到的MEMS數據標記時間戳，GPS接收機在接收到新數據時，會提供1PPS，觸發(fā)中斷，此時標記GPS接收數據時間戳t1，并根據校驗位判斷是否已接收到完整的GPS數據。若未收到，則采樣下一時刻的MEMS數據；若已收到，則標記GPS輸出數據時間戳t2。

正常情況下的回算機制示意如圖2所示。未發(fā)現(xiàn)接收數據時間戳t1時，彈機利用MEMS數據完成慣性導航計算，同時利用式(2)完成時間更新。當發(fā)現(xiàn)t1后，正常完成慣性導航與時間更新計算，同時預設存儲器存儲當前導航結果和MEMS數據，直至t2到來，在此之前，按MEMS采樣頻率正常輸出慣性導航的計算結果。在t2時刻，GPS完成數據接收并輸出，此時調取存儲器中存儲的t1時刻MEMS數據，利用式(3)～式(5)進行量測更新，與GPS數據完成數據融合。在此基礎上依次調取存儲器中(t1,t2]區(qū)間內存儲的MEMS數據，完成慣性導航計算，輸出t2時刻(即當前時刻)導航結果。至此完成一輪完整的回算操作，清空存儲器后，采樣下一時刻的MEMS數據，進行慣性導航計算，并等待下一秒GPS接收數據時間戳的到來。

圖2 正常情況下回算機制示意圖

若GPS接收數據過程中出現(xiàn)異常導致數據丟失，或接收時間過長，則存儲器中存儲數據將過量。考慮到固定的GPS接收機，正常情況下GPS時延在某一范圍內小幅波動，因此可根據前期測試得到GPS時延的上限值，并根據GPS時延上限時長與MEMS采用周期設置存儲器最大存儲長度。如圖3所示，假設預設的存儲器最大存儲長度為6，則在t1時刻后第7拍存儲時會出現(xiàn)存儲器溢出的現(xiàn)象，此時清空存儲器，放棄GPS更新操作，組合導航自動退化為慣性導航，直至下一秒GPS接收數據時間戳的到來，再次進入回算機制。預設存儲器長度可以避免存儲器存儲過多的數據，減小對系統(tǒng)存儲空間的壓力，同時可以避免因某一次接收數據異常而對后續(xù)流程產生不利影響。若放棄某一次回算，則組合導航在該時刻自動退化為慣性導航，不影響輸出，短時間內不影響導航精度。

圖3 異常情況下回算機制示意圖

采用上述回算機制處理GPS時延，無須增添狀態(tài)變量，不增加卡爾曼濾波算法的計算量。該回算機制僅需存儲一步慣性導航結果、有限步MEMS數據，可避免GPS數據異常對存儲空間和計算精度帶來的不利影響。除了判斷時延長度是否超過預設值外，程序執(zhí)行上僅需調用原有程序模塊，無新增處理環(huán)節(jié)，不增加程序的復雜度，在工程實踐中具有較好的實用性。

2 工程實踐中的計算優(yōu)化

為不干擾正常導航計算與輸出，采用回算機制處理GPS時延時需保證處理效率，并及時輸出。但在實際操作中常出現(xiàn)回算過程占用較長的處理時間，導致后續(xù)控制延遲明顯，繼而影響了下一時刻的MEMS數據采樣，造成數據整體延遲。在現(xiàn)有低成本處理器的能力內，考慮采取一定的計算優(yōu)化，以達到節(jié)約回算時間、避免數據延遲的目的。

表1 卡爾曼濾波過程所需乘法與加法次數

由于量測更新階段的計算更新頻率低，且必不可少，因此考慮從更新頻率相對較高的時間更新計算上進行優(yōu)化。由式(2)可知，在時間更新環(huán)節(jié)中，一步預測均方誤差陣Pk/k-1依照估計均方誤差陣Pk、一步轉移陣Φk,k-1、系統(tǒng)噪聲陣Qk-1等參數調整。對于固定的MEMS器件，系統(tǒng)噪聲特性基本穩(wěn)定，不存在大范圍的波動。而一步轉移陣計算公式如式(6)所示(其中系統(tǒng)矩陣F參考文獻[15]中式(14.63))，由于采樣時間Δt通常較小，在短時間內Φk,k-1調整量較小。

Φk,k-1≈I+F·Δt+F·(Δt)2/2

(6)

由此可看出，一步預測均方誤差陣Pk/k-1在短時間內與估計均方誤差陣Pk差別較小。考慮到回算之前，已計算過t1至t2時間段內的一步預測均方誤差陣Pk/k-1，若在回算中再次計算Pk/k-1，不僅數值上前后差別不大，且會占用較多計算時間。因此從實際工程角度出發(fā)，可取消回算過程中時間更新計算環(huán)節(jié)。該項計算優(yōu)化算法在理論仿真和實際應用中均得到驗證，可節(jié)約計算時間，不影響定位精度，適用于低成本彈機處理器，工程應用效果較好。

3 算法驗證及分析

3.1 回算機制驗證

結合某型低成本制導炸彈靶試，驗證在實際應用中回算機制對GPS時延的補償效果，其中MEMS和GPS基本性能指標如表2所示。

表2 某型低成本制導炸彈導航儀器性能指標

試驗條件：載機掛載制導炸彈以速度為Ma=0.6、海拔高度為5500 m飛行，繞靶場平穩(wěn)掛飛后投放。初始位置和速度均由GPS提供，初始姿態(tài)角由載機提供，姿態(tài)角誤差±2°。

MEMS器件采樣頻率200 Hz，GPS輸出頻率1 Hz。經前期測試，GPS時延不超過30 ms，因此回算機制中預設存儲器長度為6。

狀態(tài)估計均方誤差陣初值P0、系統(tǒng)噪聲陣初值Q0、量測噪聲陣初值R0設置如下：

P0=diag{2°,2°,2°,0.1 m/s,0.1 m/s,0.1 m/s,6.298×10-5°,8.062×10-5°,15 m,20×10-3g,20×10-3g,20×10-3g,30°/h,30°/h,30°/h}2

Q0=diag{30°/h,30°/h,30°/h,20×10-3g,20×10-3g,20×10-3g,0,0,0,20×10-3g,20×10-3g,20×10-3g,30°/h,30°/h,30°/h}2

R0=diag{6.298×10-5°,8.062×10-5°,15 m,0.1 m/s,0.1 m/s,0.1 m/s}2

利用彈機硬件記錄GPS接收和輸出時間戳，并遙測下發(fā)。根據遙測數據記錄，靶試飛行期間GPS數據時延如圖4所示，其中縱坐標為每秒接收到的GPS輸出時間戳與接收時間戳的差值。實測結果表明在制導炸彈飛行期間，GPS時延基本在15～25 ms區(qū)間內，與前期測試相符。

圖4 飛行期間GPS時延

按遙測數據進行再現(xiàn)，對比使用回算機制及未使用回算機制時導航定位與GPS間誤差，具體結果如圖5～圖8所示，其中圖5～圖7依次為有無回算機制下定位誤差在導航系各方向(北向、東向、天向)上的對比，圖8為空間定位誤差對比，藍色曲線表示有回算機制時導航與GPS間的定位誤差曲線，橙色表示無回算機制時的定位誤差。由對比結果可知，除高程定位差別較小(平穩(wěn)掛飛，高程變化較小)外，使用回算機制時導航與GPS間定位誤差總體上小于未使用回算機制時的定位誤差。該誤差會影響靶試最終精度，使用回算機制時最終定位精度約為3.83 m，而未使用回算機制時最終定位精度約為12.15 m。

圖5 有無回算機制對北向定位誤差的影響

圖6 有無回算機制對東向定位誤差的影響

圖7 有無回算機制對高程定位誤差的影響

圖8 有無回算機制對空間定位誤差的影響

3.2 計算優(yōu)化驗證

在相同的條件下對比使用計算優(yōu)化算法前后的計算效率及其對導航定位精度的影響。

以臺式計算機(Intel?CoreTMi5-3470，CPU 3.20 GHz，12 GB RAM，64位操作系統(tǒng))為例，在C語言運行環(huán)境下，運行A、B兩套含回算機制的導航仿真程序，其中A程序未采用計算優(yōu)化，B程序采用計算優(yōu)化。人為設定GPS時延15 ms，A程序完成一次回算過程耗時約0.1 ms，B程序完成一次回算過程耗時約0.047 ms，采用優(yōu)化算法在單次回算過程中可節(jié)省約一半的計算時間。使用低成本彈載計算機運行A、B兩套程序，A程序在回算過程中運行時間超過MEMS采樣周期，導致后續(xù)控制延遲明顯，而B程序可保證回算過程運行時間小于MEMS采樣周期，不增加延遲，實際應用效果較好。

利用靶試遙測數據，驗證本文所提出的計算優(yōu)化對導航定位精度的影響。圖9、圖10為采用計算優(yōu)化前后的定位誤差對比及兩者間偏差結果，由圖可知，取消回算過程中的Pk/k-1時間更新對導航定位精度無明顯影響，計算優(yōu)化前后定位偏差小于0.15 m，對于短時間內的低成本組合導航來說，該量級誤差可忽略。

圖9 有無計算優(yōu)化對定位誤差的影響

圖10 有無計算優(yōu)化定位偏差

4 結束語

針對組合導航算法中數據融合的時間不同步現(xiàn)象，本文提出了一種適用于低成本彈機處理器的回算機制，對GPS輸出時延進行補償，可適用于GPS數據丟失或異常等情況，且不增加程序復雜度，不給系統(tǒng)存儲空間帶來過大的壓力。同時提出一種工程實踐中的計算優(yōu)化，取消回算過程中Pk/k-1的時間更新操作，不影響導航定位精度，節(jié)省計算時間，對于低成本制導炸彈具有較好的工程實用價值。