基于因子圖算法的INS/GPS信息滯后處理方法

高軍強，湯霞清，張 環(huán)，郭理彬

(陸軍裝甲兵學(xué)院 兵器與控制系，北京 100072)(*通信作者電子郵箱gjqxkl8990@163.com)

0 引言

慣性導(dǎo)航系統(tǒng)/全球定位系統(tǒng)(Inertial Navigation System/Global Positioning System, INS/GPS)組合導(dǎo)航系統(tǒng)能夠充分發(fā)揮INS和GPS各自的優(yōu)點，被認為是目前最理想的組合方式，廣泛應(yīng)用于精確制導(dǎo)武器、測繪、航空航天等領(lǐng)域[1-2]。然而，由于INS/GPS組合導(dǎo)航系統(tǒng)中的GPS信息一般滯后于INS信息，為了對同一量測時刻的INS、GPS信息進行融合就需要組合導(dǎo)航系統(tǒng)信息融合時刻也必須滯后于量測時刻，從而對系統(tǒng)實時性造成了不良影響[3-4]。

目前，消除信息滯后對INS/GPS性能影響的主要方法可以分為兩種: 一種方法是進行時間配準(zhǔn)，即將不同信息源的量測信息統(tǒng)一到同一時刻，使組合導(dǎo)航系統(tǒng)信息融合時刻處理的是不同信息源在同一時刻的量測信息，代表方法有曲線擬合法、拉格朗日插值法、最小二乘虛擬法、內(nèi)插外推法等，然而經(jīng)時間配準(zhǔn)方法處理后的量測信息實際上都引入了新的誤差，在工程應(yīng)用中也存在一定的局限性和片面性[5-6]; 另一種方法是直接在濾波算法中融合各信息源在不同時刻的量測信息，如將信息滯后時間擴充為系統(tǒng)狀態(tài)進行估計的增廣狀態(tài)法[7]、基于新息重組理論建立最優(yōu)濾波器的方法[8]、考慮量測信息滯后的非等間隔濾波算法[9]等。針對采用卡爾曼濾波的INS/GPS組合導(dǎo)航系統(tǒng)，將基于INS機械編排的時間更新過程與利用GPS信息進行修正的量測更新過程分解為兩個獨立的任務(wù)，在接收到GPS信息之前只進行時間更新過程，待接收到GPS信息之后再進行量測更新過程，能夠避免由于GPS信息滯后帶來的等待問題，保證系統(tǒng)的實時性，但需要對卡爾曼濾波進行較大的調(diào)整，系統(tǒng)精度也受到了一定影響[10-11]。

因子圖算法將組合導(dǎo)航信息融合問題描述為在因子圖模型中連接因子節(jié)點，通過因子圖模型推理實現(xiàn)組合導(dǎo)航結(jié)果的求解。因子圖模型良好的擴展性和靈活性，使其可以高效、快速地融合異步量測信息，在多傳感器組合導(dǎo)航系統(tǒng)中逐漸受到越來越多的關(guān)注[12-13]。本文將因子圖算法應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)，研究了INS/GPS因子圖模型的構(gòu)建及推理方法，提出了在因子圖模型中處理信息滯后的方法與INS誤差修正方案，并進行了仿真實驗與分析。

1 基于因子圖的INS/GPS信息融合算法

1.1 INS/GPS組合導(dǎo)航數(shù)學(xué)模型

INS/GPS組合導(dǎo)航系統(tǒng)中的狀態(tài)變量一般是由位置、速度、姿態(tài)、慣性傳感器偏差構(gòu)成的15維向量[14]。由于慣性傳感器偏差是用于對INS中慣性測量單元(Inertial Measurement Unit, IMU)的測量誤差進行修正，并不需要進行導(dǎo)航輸出。因此，將狀態(tài)變量分為導(dǎo)航狀態(tài)量x和慣性傳感器偏差狀態(tài)量α兩個部分，分別為：

(1)

(2)

式中:λ、L、h分別表示經(jīng)度、緯度、高度；vE、vN、vU分別表示東向、北向、天向速度；θ、γ、ψ表示姿態(tài)角；εx、εy、εz表示陀螺儀偏差量；▽x、▽y、▽z表示加速度計偏差量。

組合導(dǎo)航系統(tǒng)輸出周期為T=ti+1-ti(i=0,1,2,…)，在ti時刻輸出的導(dǎo)航狀態(tài)量為xi，慣性傳感器偏差狀態(tài)量為αi，則截止到tk時刻組合導(dǎo)航系統(tǒng)輸出序列和慣性傳感器偏差狀態(tài)量序列可以分別表示為集合的形式：

Xk={x0,x1,…,xk-1,xk}

(3)

Αk={α0,α1,…,αk-1,αk}

(4)

Zk={z0,z1,…,zk-1,zk}

(5)

INS/GPS組合導(dǎo)航信息融合的任務(wù)便可以表示為求解聯(lián)合概率分布p(Xk|Zk)的最大后驗概率估計問題：

(6)

因子圖算法采用因子圖模型描述式(6)所示最大后驗概率估計問題，然后通過對因子圖模型進行推理，求出最大后驗估計值, 其算法核心包括因子圖模型構(gòu)建與因子圖模型推理兩部分。

1.2 INS/GPS因子圖模型構(gòu)建

因子圖模型是一種二分圖模型，由變量節(jié)點、因子節(jié)點以及表示變量節(jié)點與因子節(jié)點間依賴關(guān)系的邊構(gòu)成，記tk時刻的INS/GPS因子圖模型為：

Gk=(Fk,Ωk,Ξk)

(7)

(8)

由式(3)～(5)，Xk、Αk、Zk中的變量隨時間的推移越來越多，故因子圖模型中的變量節(jié)點、因子節(jié)點、邊也在不斷增多。下面以時刻由tk-1推移到tk，Gk相對于Gk-1的變化，說明INS/GPS因子圖模型的具體構(gòu)建方法。

由tk-1到tk，狀態(tài)變量中增加了xk和αk，故在因子圖中添加兩個變量節(jié)點，分別為Ωxk=xk，Ωαk=αk，為便于表示，在因子圖中Ωxk直接記作xk，Ωαk直接記作αk。

一般情況下，慣性傳感器偏差隨時間的傳播可以用一個非線性函數(shù)描述，這意味著可以由當(dāng)前時刻的偏差預(yù)測下一時刻的偏差。將αk-1對αk的預(yù)測結(jié)果g(αk-1)看作是對αk的量測信息，則得到偏差因子節(jié)點的表達式為：

(9)

(10)

(11)

(12)

1.3 INS/GPS因子圖模型推理方法

因子圖模型的推理就是利用聯(lián)合概率分布，在已知圖結(jié)構(gòu)和證據(jù)的情況下，回答查詢問題。對于組合導(dǎo)航系統(tǒng)因子圖模型需要解決的最大后驗概率查詢問題，目前主要有最大化-乘積消元算法、最大化-乘積團樹算法等[18]; 然而，傳統(tǒng)推理方法一般用于事后處理，在一次推理中默認對模型中的所有狀態(tài)變量進行求解，這對于隨時間增長而不斷增添狀態(tài)變量的組合導(dǎo)航系統(tǒng)因子圖模型而言，求解效率勢必會不斷下降，顯然無法滿足導(dǎo)航系統(tǒng)實時性要求。

圖1 tk時刻INS/GPS因子圖模型

本文INS/GPS因子圖模型推理方法以iSAM2算法為核心，并結(jié)合INS/GPS系統(tǒng)特點進行了兩方面的調(diào)整：一是將慣性導(dǎo)航解算結(jié)果作為迭代估計中變量的初值，由于INS具有很高的短時精度，采用組合導(dǎo)航結(jié)果能夠?qū)NS誤差積累進行有效修正，保證了每個時刻的慣性導(dǎo)航解算結(jié)果與最優(yōu)估計值相差較小，將其作為初值可以減少高斯牛頓迭代的次數(shù)，從而提高最優(yōu)估計值的求解速度；二是iSAM2算法中為了對歷史狀態(tài)量進行平滑得到更高的估計精度，設(shè)計了流動重新線性化過程，而考慮到導(dǎo)航系統(tǒng)更重要的是關(guān)注當(dāng)前時刻狀態(tài)量的估計，對歷史時刻狀態(tài)量的平滑沒有太大意義，故在推理時不進行重新線性化，從而進一步提高了因子圖模型推理效率。

從添加新的節(jié)點到完成一次最優(yōu)估計的步驟如下：

步驟1 增添新的變量節(jié)點和因子節(jié)點；

步驟2 利用慣性導(dǎo)航解算結(jié)果設(shè)置新增添狀態(tài)變量的初值；

步驟3 對新增添的因子節(jié)點進行線性化處理；

步驟4 查找原因子圖中受新增添因子節(jié)點影響的部分；

步驟5 用高斯牛頓迭代方法計算需要求解的狀態(tài)變量的更新量；

步驟6 初值+更新量，得到最優(yōu)估計值。

2 因子圖算法中信息滯后處理方法

進一步研究圖1所示INS/GPS因子圖模型，如果系統(tǒng)由INS/GPS變?yōu)閱我籌NS，那么只需要將GPS因子節(jié)點去掉便可得到INS因子圖模型，這正是因子圖模型良好靈活性和擴展性的體現(xiàn)。文獻[10-11]采用的GPS信息滯后處理方法，在因子圖算法中的實現(xiàn)非常簡便。

2.1 GPS信息滯后時的因子圖模型及討論

圖2 存在GPS信息滯后時tk時刻INS/GPS因子圖模型

前文指出，圖1、圖2(a)、圖2(b)均是tk時刻的INS/GPS因子圖模型，3個模型之間存在差異的原因在于GPS信息滯后時間不同?，F(xiàn)在換一個角度考慮，將圖1、圖2(a)也視作當(dāng)T

(13)

(14)

(15)

總結(jié)以上對比，從精度的角度，圖1優(yōu)于圖2(a)優(yōu)于圖2(b)；從實時性的角度，圖2(b)優(yōu)于圖2(a)優(yōu)于圖1。組合導(dǎo)航系統(tǒng)設(shè)計在精度與實時性之間的權(quán)衡是亙古不變的命題，本文研究的出發(fā)點是在保證最高實時性的前提下，實現(xiàn)相對更高的精度。

2.2 INS誤差修正方案

INS誤差修正是指將組合導(dǎo)航結(jié)果作為慣性導(dǎo)航解算的初始值，從而消除INS中的誤差積累，這是保證組合導(dǎo)航系統(tǒng)精度的關(guān)鍵。當(dāng)不存在信息滯后時，毋庸置疑地采用上一時刻的實時導(dǎo)航狀態(tài)量作為慣性導(dǎo)航解算的初始值，對上一時刻到當(dāng)前時刻之間的IMU量測數(shù)據(jù)進行解算，能夠有效消除INS中的誤差積累; 然而，當(dāng)存在信息滯后時，為了保證系統(tǒng)實時性，需要在接收到GPS量測信息前將精度較低的預(yù)測結(jié)果作為實時導(dǎo)航狀態(tài)量進行輸出，而較早時刻的導(dǎo)航狀態(tài)量已經(jīng)是量測更新結(jié)果，精度較高。因此，在對INS誤差進行修正時便存在兩種方案可供選擇。

方案一 采用上一時刻的實時導(dǎo)航狀態(tài)量進行INS誤差修正，不考慮其是否進行了量測更新。該方案的優(yōu)點在于，每次只對一個系統(tǒng)輸出周期內(nèi)的IMU量測數(shù)據(jù)進行解算，從而保證了誤差積累最小，并且無需儲存歷史IMU量測數(shù)據(jù)。缺點在于，如果上一時刻的實時導(dǎo)航狀態(tài)量是預(yù)測結(jié)果，將有可能無法有效修正INS誤差。

方案二 采用上一時刻因子圖模型推理結(jié)果中剛剛完成量測更新的導(dǎo)航狀態(tài)量進行INS誤差修正，對該導(dǎo)航狀態(tài)量對應(yīng)時刻到當(dāng)前時刻之間的IMU量測數(shù)據(jù)進行解算。該方案的優(yōu)點在于，采用量測更新結(jié)果能夠保證有效修正INS誤差；缺點在于，需要儲存一段時間的IMU量測數(shù)據(jù)，并且一次INS解算的時間段越長，就會在解算過程中產(chǎn)生越大的誤差積累。

為了直觀對比兩種INS誤差修正方案，圖3給出了存在GPS信息滯后時因子圖算法的處理時序，并對兩種INS誤差修正方案進行了標(biāo)記。

3 仿真實驗與分析

設(shè)置INS/GPS組合導(dǎo)航系統(tǒng)輸出頻率為5 Hz；INS測量頻率為100 Hz，陀螺常值漂移為0.02 °/h、隨機漂移為0.01 °/h，加速度計的常值零偏為100 μg，隨機零偏為50 μg；GPS測量頻率為1 Hz，經(jīng)度、緯度測量噪聲均方差為10 m，高度測量噪聲均方差為15 m，3個方向速度測量噪聲均方差都為0.1 m/s；總仿真時長1 800 s，設(shè)計仿真軌跡如圖4所示。

圖3 GPS信息滯后時因子圖算法處理時序及INS誤差修正方案

圖4 運動軌跡仿真

為了分析存在信息滯后時因子圖組合導(dǎo)航算法的性能，比較兩種INS誤差修正方案在不同GPS信息滯后時間下的處理效果, 進行8組仿真實驗。每組仿真實驗中分別進行50次蒙特卡羅仿真，采用均方根誤差(Root Mean Square Error, RMSE)評價組合導(dǎo)航精度。各組仿真實驗中，GPS信息滯后時間及INS誤差修正方案設(shè)置如表1所示。其中，實驗1為不存在信息滯后的理想狀況，作為誤差參照基準(zhǔn)。由于不存在信息滯后時，上一時刻實時導(dǎo)航狀態(tài)量便是剛剛完成量測更新的導(dǎo)航狀態(tài)量，故兩種INS誤差修正方案是等價的。

表1 仿真實驗參數(shù)

圖5所示為實驗1、2、6位置RMSE曲線對比，當(dāng)GPS信息滯后時間為(0, 0.2]時，采用方案一和方案二均得到了與不存在GPS信息滯后時相當(dāng)?shù)慕M合導(dǎo)航精度，說明因子圖算法憑借其可以在一個信息融合時刻處理各信息源不同時刻量測信息的特點，非常成功地處理了信息滯后帶來的影響。

圖5 實驗1、2、6位置RMSE曲線對比

圖6所示為實驗1、3、4位置RMSE曲線對比，采用方案一進行INS誤差修正時，隨著GPS信息滯后時間的變長，高度誤差幾乎不受影響，而經(jīng)度誤差和緯度誤差出現(xiàn)了明顯增大，組合導(dǎo)航結(jié)果精度越來越差。這是由于GPS信息滯后時間越長，組合導(dǎo)航結(jié)果就是時間跨度越長的預(yù)測結(jié)果，其精度受INS誤差積累的影響越大，對INS誤差修正作用也就越小。

圖6 實驗1、3、4位置RMSE曲線對比

圖7為實驗1、5位置RMSE曲線對比，當(dāng)GPS信息滯后時間達到(1.6, 1.8]時，經(jīng)度誤差和緯度誤差隨時間發(fā)散，當(dāng)載體高度方向發(fā)生位移時，高度誤差也明顯增大，此時方案一已經(jīng)無法有效修正INS誤差。以上分析表明：方案一只能在較小的GPS信息滯后情況下，對INS誤差起到有限的修正作用，當(dāng)GPS信息滯后時間變長，該方案的導(dǎo)航誤差會越來越大，甚至發(fā)散，不具備長航時導(dǎo)航的能力。

圖7 實驗1、5位置RMSE曲線對比

圖8所示實驗1、7、8位置RMSE曲線對比，采用方案二進行INS誤差修正時，即便GPS信息滯后時間達到(1.6, 1.8]也能得到與不存在GPS信息滯后時相當(dāng)?shù)膶?dǎo)航精度。說明，INS誤差積累會在每一次修正中得到有效消除，保證了組合導(dǎo)航精度的長期穩(wěn)定。

圖8 實驗1、7、8位置RMSE曲線對比

以上分析表明，當(dāng)GPS信息滯后時間不超過組合導(dǎo)航系統(tǒng)輸出周期時，直接采用實時導(dǎo)航狀態(tài)量對INS誤差進行修正，計算量小且不需要額外的存儲空間便可以實現(xiàn)與沒有滯后時非常接近的導(dǎo)航精度；但是當(dāng)GPS信息滯后時間超過組合導(dǎo)航系統(tǒng)輸出周期時，采用實時導(dǎo)航狀態(tài)量對INS誤差修正效果變差，甚至有可能導(dǎo)致誤差發(fā)散。這種情況下，如果采用上一時刻因子圖模型推理結(jié)果中剛剛完成量測更新的導(dǎo)航狀態(tài)量對INS誤差進行修正，則可以取得更好的效果?；贗NS/GPS因子圖組合導(dǎo)航算法的GPS信息滯后處理方法很好地解決了信息滯后帶來的不良影響，對存在信息滯后的組合導(dǎo)航系統(tǒng)設(shè)計具有一定參考意義。

4 結(jié)語

采用因子圖算法實現(xiàn)INS/GPS組合導(dǎo)航系統(tǒng)信息融合，避免了系統(tǒng)實時性因GPS信息滯后而變差，同時又能保證很高的系統(tǒng)精度。因子圖算法利用接收到的量測信息建立因子節(jié)點，將其連接到因子圖模型中對應(yīng)的變量節(jié)點上進行因子圖模型推理便可求得信息融合結(jié)果。因子圖算法可以在一個信息融合時刻處理不同時刻的量測信息，使其對于受異步、滯后等因素影響的多傳感器信息融合問題有很強的適應(yīng)性。因子圖算法對子系統(tǒng)進行反饋校正時，由于不同時刻可用的量測信息不同，導(dǎo)致因子圖模型推理結(jié)果中不同時刻狀態(tài)量的估計精度差別較大，因此需要盡量采用估計精度較高的狀態(tài)量進行反饋，以改善組合導(dǎo)航系統(tǒng)的精度。本文采用完成量測更新的導(dǎo)航狀態(tài)量對INS誤差進行修正時需要儲存一段時間的IMU量測數(shù)據(jù)，當(dāng)GPS信息滯后時間太長時，求解效率有所下降，故因子圖模型實時推理方法以及在多傳感器信息融合中的應(yīng)用有待進一步研究。