汪 菁,孫立海,鄒瑜光

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

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

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

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

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

1 回算機(jī)制

圖1 間接法對(duì)應(yīng)的卡爾曼濾波流程圖

(1)

(2)

(3)

(4)

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

(5)

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

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

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

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

圖2 正常情況下回算機(jī)制示意圖

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

圖3 異常情況下回算機(jī)制示意圖

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

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

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

表1 卡爾曼濾波過(guò)程所需乘法與加法次數(shù)

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

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

(6)

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

3 算法驗(yàn)證及分析

3.1 回算機(jī)制驗(yàn)證

結(jié)合某型低成本制導(dǎo)炸彈靶試，驗(yàn)證在實(shí)際應(yīng)用中回算機(jī)制對(duì)GPS時(shí)延的補(bǔ)償效果，其中MEMS和GPS基本性能指標(biāo)如表2所示。

表2 某型低成本制導(dǎo)炸彈導(dǎo)航儀器性能指標(biāo)

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

MEMS器件采樣頻率200 Hz，GPS輸出頻率1 Hz。經(jīng)前期測(cè)試，GPS時(shí)延不超過(guò)30 ms，因此回算機(jī)制中預(yù)設(shè)存儲(chǔ)器長(zhǎng)度為6。

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

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

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

圖4 飛行期間GPS時(shí)延

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

圖5 有無(wú)回算機(jī)制對(duì)北向定位誤差的影響

圖6 有無(wú)回算機(jī)制對(duì)東向定位誤差的影響

圖7 有無(wú)回算機(jī)制對(duì)高程定位誤差的影響

圖8 有無(wú)回算機(jī)制對(duì)空間定位誤差的影響

3.2 計(jì)算優(yōu)化驗(yàn)證

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

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

利用靶試遙測(cè)數(shù)據(jù)，驗(yàn)證本文所提出的計(jì)算優(yōu)化對(duì)導(dǎo)航定位精度的影響。圖9、圖10為采用計(jì)算優(yōu)化前后的定位誤差對(duì)比及兩者間偏差結(jié)果，由圖可知，取消回算過(guò)程中的Pk/k-1時(shí)間更新對(duì)導(dǎo)航定位精度無(wú)明顯影響，計(jì)算優(yōu)化前后定位偏差小于0.15 m，對(duì)于短時(shí)間內(nèi)的低成本組合導(dǎo)航來(lái)說(shuō)，該量級(jí)誤差可忽略。

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

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

4 結(jié)束語(yǔ)

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