高軍強(qiáng)，湯霞清，張 環(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)點(diǎn)，被認(rèn)為是目前最理想的組合方式，廣泛應(yīng)用于精確制導(dǎo)武器、測繪、航空航天等領(lǐng)域[1-2]。然而，由于INS/GPS組合導(dǎo)航系統(tǒng)中的GPS信息一般滯后于INS信息，為了對同一量測時(shí)刻的INS、GPS信息進(jìn)行融合就需要組合導(dǎo)航系統(tǒng)信息融合時(shí)刻也必須滯后于量測時(shí)刻，從而對系統(tǒng)實(shí)時(shí)性造成了不良影響[3-4]。

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

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

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)的測量誤差進(jìn)行修正，并不需要進(jìn)行導(dǎo)航輸出。因此，將狀態(tài)變量分為導(dǎo)航狀態(tài)量x和慣性傳感器偏差狀態(tài)量α兩個(gè)部分，分別為：

(1)

(2)

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

組合導(dǎo)航系統(tǒng)輸出周期為T=ti+1-ti(i=0,1,2,…)，在ti時(shí)刻輸出的導(dǎo)航狀態(tài)量為xi，慣性傳感器偏差狀態(tài)量為αi，則截止到tk時(shí)刻組合導(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)的最大后驗(yàn)概率估計(jì)問題：

(6)

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

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

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

Gk=(Fk,Ωk,Ξk)

(7)

(8)

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

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

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

(9)

(10)

(11)

(12)

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

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

圖1 tk時(shí)刻INS/GPS因子圖模型

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

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

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

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

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

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

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

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

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

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

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

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

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

(13)

(14)

(15)

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

2.2 INS誤差修正方案

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

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

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

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

3 仿真實(shí)驗(yàn)與分析

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

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

圖4 運(yùn)動(dòng)軌跡仿真

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

表1 仿真實(shí)驗(yàn)參數(shù)

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

圖5 實(shí)驗(yàn)1、2、6位置RMSE曲線對比

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

圖6 實(shí)驗(yàn)1、3、4位置RMSE曲線對比

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

圖7 實(shí)驗(yàn)1、5位置RMSE曲線對比

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

圖8 實(shí)驗(yàn)1、7、8位置RMSE曲線對比

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

4 結(jié)語

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