張 靖，王向東，鄧志寶，張凌東，劉宇航

（1. 北京航天發(fā)射技術研究所，北京，100076；2. 火箭軍駐211廠軍事代表室，北京，100076）

0 引 言

單一慣性定位方法已無法滿足陸基武器發(fā)射系統(tǒng)在復雜工況中大范圍機動、隨機發(fā)射的定位需求，不具備良好的魯棒性與可靠性[1]，難以持續(xù)提供高精度的定位信息?；诙嘈畔⒃慈诤系慕M合定位算法實現了多種定位傳感器的優(yōu)勢互補，可在復雜工況中提供高精度的定位信息，提升導彈武器系統(tǒng)的生存能力。

按信號的輸出頻率特性劃分，目前定位信息源主要分為3類：a）信號以固定頻率發(fā)送的信息源，如慣導（Inertial Navigation System，INS）、里程計（Odometer，OD）、全球衛(wèi)星導航系統(tǒng)（Global Navigation Satellite System，GNSS）等；b）信號異步非等時間間隔輸出的信息源，如地磁匹配（Geomagnetic Matching Navigation System，GMNS）、地圖匹配（Map Matching Navigation System，MMNS）、景象匹配（Scene Matching Navigation System，SMNS）等；c）距離間隔較長、不等時間間隔信號輸出的信息源，如地標。組合定位算法在融合不同頻率特性的信息源時存在異步時間配準誤差，降低定位精度。

解決多信息源異步融合問題，主要有3種思路：a）采用GMNS、MMNS等信息源輸出的異步非等間隔定位信息直接校正INS的位置，該方式對匹配定位信息利用不充分。b）在數據融合前消除時間不同步問題。文獻[2]采用最小二乘法將高頻量測值融合到低頻量測時刻進行同步，文獻[3]、文獻[4]采用內插外推將高精度量測數據推算到低精度量測信息的時間點上；c）針對異步量測信息采用異步融合方法，可避免以上兩種方法均存在數據擬合誤差。文獻[5]采取各個子濾波器濾波周期的最大公約數和最小公倍數作為融合周期，但不適用于信號無固定頻率非等間隔輸出的信息源。

本文提出了一種基于因子圖的異步多源組合定位算法，以異步量測信息異步融合的思想，采取量測信息“即來即更新”的狀態(tài)估計方式，充分利用了信息源的量測信息，避免了外推法的擬合誤差，適用于多頻率特性傳感器的數據融合，可實現定位信息源的即插即用。

1 系統(tǒng)誤差模型

組合導航系統(tǒng)的離散化誤差模型為

式中 xk為系統(tǒng)的狀態(tài)變量； zk為系統(tǒng)的觀測變量；為轉移矩陣和觀測矩陣； wk， vk為噪聲項，設 Qk， Rk為噪聲項 wk，vk的協方差矩陣；i表示有i個觀測方程。采用表示INS/OD組合的觀測方程。表示 INS/GMNS組合的觀測方程。

2 因子圖與和積算法

2.1 因子圖

因子圖是一種概率圖模型，用來表示全局函數與局部函數的關系，可以將多變量全局函數因式分解后的多個局部函數進行圖形化表示。因子圖模型G=(F , X,ε)包含因子節(jié)點：fi∈F、變量節(jié)點：xj∈X和邊：eij∈ε3個元素[6]。因子圖可將待求解的問題轉化為邊界問題，在很多領域如編譯碼、信號處理、人工智能、神經網絡中的邊界問題都可用因子圖算法表示并求解，因此，因子圖作為一種分析問題的圖形工具已經得到了廣泛的應用[7]。

變量 xi以變量節(jié)點形式來呈現，局部函數 fj( Xj)以函數節(jié)點的形式來呈現。當變量 xi與某個局部函數fj( Xj)相關時，則在因子圖中用雙向邊將兩者對應的節(jié)點相連接。

基于上述分析，式（2）對應的因子圖模型見圖1。

圖1 對應因子圖模型Fig.1 Corresponding Factor Graph Model

2.2 和積算法

因子圖可以清晰直觀地表現函數與隨機變量間的關系，而對應函數節(jié)點與變量節(jié)點間的消息傳遞則需要和積算法來實現。和積算法作為一種消息傳遞算法，又稱置信傳播算法，用來計算因子圖中各節(jié)點沿著不同邊傳遞的信息，規(guī)則如下：

在因子圖中，某個節(jié)點x傳遞到另一個節(jié)點f的信息可以從其他所有與節(jié)點x相關聯的節(jié)點傳遞到x節(jié)點的消息乘積而得。

變量節(jié)點x傳遞到函數節(jié)點f的消息：

變量節(jié)點f傳遞到函數節(jié)點x的消息：

式中 X=n(f)表示節(jié)點f的所有相鄰節(jié)點即函數f包含的所有變量組成的集合；～x表示除去節(jié)點x的所有變量集。

3 異步非等間隔數據融合算法

在線性高斯系統(tǒng)（Linear Gaussian，LG）中，運動方程、觀測方程是線性的，且兩個噪聲項服從零均值的高斯分布，記高斯分布的概率密度函數為

式中 m和2σ為均值和方差，且服從以下計算：

其中，

同樣地，可以得到：

組合導航是由觀測量估計狀態(tài)量的過程，系統(tǒng)的狀態(tài)變量為：x1, x2,…xk，系統(tǒng)的觀測量為，狀態(tài)變量估計的條件概率密度可寫為，假設模型的一階馬爾可夫性，根據貝葉斯法則得到：

圖2 慣導/里程計/地磁匹配組合定位狀態(tài)估計因子圖示意Fig.2 State Estimation Factor Graph Representation for Inertial Navigation/Odometer/Geomagnetic Matching Integrated Navigation

最小方差估計等于量測為某具體實現條件下的條件均值[9]：

式中kx的條件概率密度函數為時刻k的邊緣函數：

式（12）的計算需用到和積算法，并用積分作為求和操作，見式（4）、式（5）。再將式（10）代入式（12），得到：

采用式（9）的計算方法整理得到，為推導過程簡潔、直觀，以下采用向量、矩陣采用一維狀態(tài)形式進行推導：

其中，

再令：

其中，

迭代進行計算，得到：

令：

同樣地，通過式（12）可以得到：

其中，

異步多源組合定位算法流程如圖3所示，INS/OD的信號同頻且高頻輸出，GMNS的信號非等間隔輸出。若無 GMNS信號更新，進行一次時間更新，一次INS/OD組合的量測更新，若有GMNS信號更新，進行一次時間更新，進行兩次量測迭代更新，見式（19）、式（20）。

圖3 異步多源組合定位算法流程Fig.3 Asynchronous Multi-source Integrated Navigation Algorithm Flow Diagram

4 仿真分析

選取INS、OD和GMNS 3種定位傳感器進行仿真分析，INS、OD的信號以100 Hz的頻率輸出，GMNS的信號在1～10 s之間非等間隔輸出。采用異步多源組合定位算法、GMNS定位結果直接校正慣導位置這兩種組合定位方法進行仿真，對結果進行比較分析。

4.1 行車路徑與狀態(tài)

以 WGS84坐標系統(tǒng)設置起始點為：經度116.40675°E，緯度39.80047°N，高度H為70 m。

行車狀態(tài)如表1所示，全程共計用時4095 s，里程約為80 km。行駛路徑和行駛方向如圖4所示。

表1 行車狀態(tài)Tab.1 Driving Status

圖4 仿真路徑Fig.4 Simulation Path Chart

4.2 誤差參數設置

以90TM型二頻機抖激光陀螺儀為參考，取陀螺隨機常值零偏為 0.003 （°）/h，隨機游走系數為0.0005 （°）/h；加速度計零偏為20 μg；地磁匹配水平誤差為10 m（1σ），高程誤差10 m（1σ）；里程計刻度系數誤差為 0.01；里程計桿臂誤差為[0.0149 m，0.1756 m，-1.0232 m]；地磁匹配桿臂誤差為[0.0149 m，0.1756 m，1.0232 m]。

4.3 仿真結果

依據第4.1節(jié)路徑與第4.2節(jié)誤差參數的設置生成了8組行車數據，分別采用本文提出的異步多源組合定位算法、GMNS結果直接修正慣導位置的組合導航算法進行仿真分析，隨機選取一組仿真結果的北向、東向與高程定位誤差對比分別如圖5～7所示。圖5a、圖6a、圖7a為采用異步融合算法的組合導航定位誤差，精度為：北向10.2 m（3σ）、東向10.5 m（3σ）、高程4.9 m（3σ）；圖5b、圖6b、圖7b為采用GMNS結果直接校正INS位置的組合導航定位誤差，精度為：北向29.9 m（3σ）、東向30.1 m（3σ）、高程15.1 m（3σ）。從仿真結果可以看出，采用異步非等間隔融合算法的水平與高程定位誤差分別減少到采用匹配點直接校正慣導位置方法的三分之一。

通過對比兩種組合導航算法的定位誤差圖可以看出，直接運用GMNS結果校正慣導位置時，導航過程中定位誤差有突跳的狀態(tài)，如圖 5～7的箭頭所示；采用異步融合算法會將定位誤差的幅值壓縮、平滑，定位精度更高。

行駛過程中北向與東向的陀螺零偏狀態(tài)估計對比如圖8、圖9所示。從圖8、圖9可知，采用異步融合算法進行導航的過程中，2000 s之后東向陀螺零偏估計值穩(wěn)定在0.003 （°）/h左右，3000 s之后北向陀螺零偏估計值穩(wěn)定在0.003 （°）/h左右。這是由于GMNS提供了較為準確的位置觀測信息，使東向陀螺零偏得到較好的估計。

圖5 北向誤差對比Fig.5 Northbound Error Comparison Chart

圖6 東向誤差對比Fig.6 Eastward Error Comparison Chart

圖7 高度誤差對比Fig.7 Height Error Comparison Chart

采用GMNS結果直接校正慣導位置，導航過程中由于缺少一種較為準確的觀測量，東、北向陀螺零偏的收斂在 0.0025 （°）/h左右，較初始設置偏差了0.0005 （°）/h。

水平軸陀螺零偏與位置誤差在一定時長內呈大致線性關系[10]，式（26）為作用在東向軸線的有效角速率零偏（gEδB）引起的位置誤差。

式中0R為地球半徑；sω為舒勒振蕩頻率。采用異步融合算法可對陀螺零偏進行更準確的實時校正，更加有效的抑制了位置誤差發(fā)散，提高定位精度。

圖8 北向陀螺零偏估計對比Fig.8 Northbound Gyro Bias Estimation Comparison Chart

圖9 東向陀螺零偏估計對比Fig.9 Eastward Gyro Bias Estimation Comparison Chart

5 實車試驗

采用 SINS、GNSS、里程計和高程計構建了一套組合定位系統(tǒng)，單機具體性能見表2。行車中約1000 s時開啟GMNS信號，約1500 s時關閉信號，約3500 s時再次開啟，直到終點。比較開啟與關閉GMNS信號期間的定位精度，驗證即插即用功能。

表2 實車試驗設備條件Tab.2 Test Conditions

將該組合定位系統(tǒng)安裝在獵豹試驗車上，行車路線如圖10所示，全程約44 km，行駛過程中以差分GPS的定位結果為基準，每3～4 km記錄定位結果，進行精度檢測。

圖10 跑車行駛軌跡Fig.10 Vehicle Trajectory Chart

行車中采集SINS、GMNS和里程計的原始數據，進行離線解算。

定位誤差統(tǒng)計如表3所示。表3中1點位的測量位于1000 s之前，此時未開啟GMNS信號，以INS/OD組合的方式進行定位，此間定位誤差逐漸增大；2、3點位為1000～1500 s之間的定位點，此時開啟GMNS信號，可觀察到定位誤差減?。?～8點位無GMNS信號，定位誤差逐漸增大，高程定位誤差變化最為明顯；9～12點位開啟GMNS信號，定位誤差再一次收斂。

表3 定位誤差統(tǒng)計Tab.3 Positioning Error Statistics

續(xù)表3

實車試驗過程中 12個定位點的高程定位誤差如圖 11所示。從圖 11可見，采用異步融合算法融合GMNS信號期間可明顯提升組合定位精度。且使用過程中無誤差突跳，定位精度可信度良好。

圖11 實車試驗高程定位誤差Fig.11 Height Error Chart

異步多源組合定位算法有效融合了GMNS的非等間隔信號，行車中隨機開啟與關閉GMNS信號，定位功能不受影響，識別出了GMNS信號，利用了GMNS的有效信息，實現了定位信息源的“即插即用”。

6 結 論

提出的基于因子圖的異步非等間隔組合定位算法避免了外推法帶來的誤差，利用了GMNS等信息源定位信息，更加準確地估計了慣導的水平軸陀螺零偏，抑制了定位誤差的發(fā)散，提高了定位精度。

異步融合算法中，量測信息“即來即更新”，實現了定位信息源的即插即用，進一步增強了定位系統(tǒng)的可靠性，提高了陸基機動武器系統(tǒng)的實戰(zhàn)化水平。