徐昊瑋， 廉保旺， 劉尚波,2

(1.西北工業(yè)大學(xué) 電子信息學(xué)院, 陜西 西安 710072； 2.中國科學(xué)院 西安光學(xué)精密機械研究所, 陜西 西安 710119)

0 引言

通過內(nèi)部子導(dǎo)航系統(tǒng)間共享信息和性能互補，配備不同類型導(dǎo)航源的多源組合導(dǎo)航系統(tǒng)，能夠在復(fù)雜環(huán)境中為各種平臺提供連續(xù)且準(zhǔn)確的導(dǎo)航解決方案。信息融合是多源組合導(dǎo)航系統(tǒng)中的最基本問題，近年來已有許多研究集中于各種導(dǎo)航源的相互融合，如全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)、慣性導(dǎo)航系統(tǒng)(INS)、激光測距(LiDAR)系統(tǒng)、磁力計、計算機視覺或超寬帶(UWB)定位系統(tǒng)等[1-3]。在許多基于INS的多源組合導(dǎo)航系統(tǒng)中[4-6]，集中式卡爾曼濾波算法經(jīng)常被用來處理所有觀測量，并提供全局最優(yōu)估計。盡管集中式卡爾曼濾波可以避免觀測信息的丟失，但隨著導(dǎo)航源數(shù)量的增多，急劇增加的計算負擔(dān)和較差的系統(tǒng)容錯性會使得整個融合系統(tǒng)的性能下降。

針對上述問題，聯(lián)邦卡爾曼濾波器[7-8]作為一種分散化的卡爾曼濾波算法，逐漸受到關(guān)注。聯(lián)邦卡爾曼濾波器由眾多子濾波器和一個主濾波器組成，它是一種具有兩階段數(shù)據(jù)處理的算法。在進行多源組合導(dǎo)航數(shù)據(jù)融合的過程中，子濾波器將子導(dǎo)航源輸出的導(dǎo)航觀測量與參考導(dǎo)航源輸出的導(dǎo)航解進行融合，從而得到局部導(dǎo)航狀態(tài)估計值，而局部導(dǎo)航狀態(tài)估計值和子系統(tǒng)協(xié)方差矩陣被輸入到主濾波器中，計算全局最優(yōu)估計并更新全局協(xié)方差矩陣。然而，由于不同子導(dǎo)航源之間異步的數(shù)據(jù)更新以及在線和離線傳感器之間的切換，在主濾波器融合過程中各個局部導(dǎo)航觀測量的時刻并不統(tǒng)一，導(dǎo)致引入全局估計誤差。一些研究通過引入內(nèi)插或遞推的算法，將子系統(tǒng)的時刻統(tǒng)一，但也因此會引入人為誤差，增加了計算負擔(dān)[9]。

為了解決多源組合導(dǎo)航融合應(yīng)用中存在的上述問題，文獻[10-12]提出了一種基于因子圖的多源組合導(dǎo)航信息融合算法。因子圖算法(簡稱FG算法)是一種基于圖的貝葉斯推理平滑算法[13]，它為即插即用型傳感器提供了靈活的應(yīng)用基礎(chǔ)。在FG算法中，傳感器網(wǎng)絡(luò)中的每個觀測量被編碼成一個因子，所有傳感器的觀測因子通過節(jié)點互連的方式組成框架圖，通過對這些連接在框架上的因子使用貝葉斯推理完成數(shù)據(jù)融合和參數(shù)估計。由于在多源組合導(dǎo)航系統(tǒng)中，各導(dǎo)航源更新觀測量的速率不一致，因此采用更新速率最高的INS作為主導(dǎo)航源，當(dāng)在某時刻從任意子導(dǎo)航源獲取測量結(jié)果時，首先利用INS中的慣性測量單元(IMU)輸出數(shù)據(jù)進行預(yù)積分和定位解算，來計算未知觀測節(jié)點的預(yù)測值；然后將觀測量作為后驗信息，通過迭代得到各節(jié)點狀態(tài)量的全局最大后驗估計。文獻[14]在此基礎(chǔ)上進一步將滑動窗的概念引入多源組合導(dǎo)航因子圖算法中?；瑒哟笆沟靡蜃訄D的全局最優(yōu)化過程僅發(fā)生在最新生成的若干個節(jié)點間，它的引入不僅提高了融合算法的估計速度，而且減少了算法的計算量。

多源組合導(dǎo)航因子圖融合算法的最優(yōu)化過程是通過最小化觀測量與預(yù)測量之間的馬氏距離[15]實現(xiàn)的，馬氏距離與子系統(tǒng)先驗誤差分布的均值向量和協(xié)方差矩陣相關(guān)，在上述FG算法[10-14]中，觀測量的誤差分布一旦完成初始化后將保持不變。然而，由于各種環(huán)境因素的影響，非慣性導(dǎo)航源的觀測量是時變的，當(dāng)融合算法中觀測量誤差的先驗估計與實際得到的觀測量存在差異時，就會導(dǎo)致估計結(jié)果相對于真實值存在較大偏差，因此需要引入算法對觀測量誤差進行實時的估計?；谝陨显?，本文提出了一種基于高斯模型下的子系統(tǒng)觀測量均值向量和協(xié)方差矩陣的估計算法，該算法能夠通過迭代選權(quán)的方式對觀測量誤差分布狀態(tài)進行準(zhǔn)確、快速地估計，進而提高FG算法對導(dǎo)航狀態(tài)估計的精度。

1 基于滑動窗的因子圖組合導(dǎo)航系統(tǒng)模型

圖1 多源組合導(dǎo)航FG算法的原理示意圖Fig.1 Schematic diagram of factor graph-based multi-source integrated navigation data fusion algorithm

在FG算法中，應(yīng)用貝葉斯推斷進行狀態(tài)量估計的范圍可以是整個因子圖網(wǎng)絡(luò)，但是遍歷估計的方法使得算法在運行過程中的計算量不斷增大，而且最新得到的觀測值對早期變量估計準(zhǔn)確性的提高也并不顯著。因此通過對因子圖的優(yōu)化范圍加窗，即限定參與優(yōu)化的因子節(jié)點范圍，能夠有效減少算法計算量、提高算法的效率。

滑動窗因子圖意味著滑動窗口內(nèi)僅包含全部因子節(jié)點中的一部分，它類似于固定滯后平滑算法。然而，與傳統(tǒng)的線性平滑器不同，滑動窗因子圖可以通過應(yīng)用增量平滑算法，在每次產(chǎn)生新觀測量時無需重新計算整個雅克比矩陣，僅需要對經(jīng)過正交三角分解(QR)的雅克比矩陣進行增量更新即可?；瑒哟癋G算法中加窗的范圍即是應(yīng)用貝葉斯推斷進行狀態(tài)量估計的范圍，滑動窗的滑動規(guī)則包括定步長、變步長和定窗長、變窗長[15]?；瑒右?guī)則的選擇一般與應(yīng)用場景相關(guān)，如在動態(tài)性變化較大的應(yīng)用場景中，一般采用定步長、變窗長的滑動規(guī)則，從而保證算法能夠根據(jù)載體動態(tài)性自適應(yīng)地調(diào)整估計值輸出速率。而在本文的應(yīng)用場景下，由于信號和載體運動狀態(tài)的動態(tài)性變化相對較小，選取定步長、定窗長的滑動規(guī)則。本文中加窗的范圍是通過設(shè)定觀測量數(shù)目而確定的，即當(dāng)觀測節(jié)點達到設(shè)定上限時，則產(chǎn)生新的窗。

2 因子圖組合導(dǎo)航融合算法

2.1 FG算法

為了實現(xiàn)導(dǎo)航系統(tǒng)中的數(shù)據(jù)融合，首先建立k時刻導(dǎo)航坐標(biāo)系下的狀態(tài)變量Xk為

(1)

需要說明的是，之所以選用導(dǎo)航參數(shù)而非導(dǎo)航參數(shù)誤差作為系統(tǒng)的狀態(tài)量，主要是基于兩方面的考慮：1)將導(dǎo)航參數(shù)作為系統(tǒng)狀態(tài)變量，可以通過系統(tǒng)模型直接描述系統(tǒng)導(dǎo)航參數(shù)的動態(tài)過程，相比于根據(jù)1階近似推導(dǎo)得到的基于導(dǎo)航參數(shù)誤差狀態(tài)量，導(dǎo)航參數(shù)能夠更加準(zhǔn)確地反映真實狀態(tài)的演變[16]；2)對更新的IMU數(shù)據(jù)進行預(yù)積分和解算后，無需等待子系統(tǒng)觀測量更新，可以直接建立因子節(jié)點并加入因子圖中[7,18]。導(dǎo)航參數(shù)的微分方程為

(2)

(3)

(4)

pi=pi-1+vi-1Δt，

(5)

(6)

(7)

(8)

該后驗概率密度函數(shù)似然概率密度f(Zi|Xi)和先驗概率f(Xi|Xi-1)呈比例，即

(9)

(10)

(11)

(12)

(13)

上述因子圖編碼的優(yōu)化問題一般由高斯-牛頓迭代算法求解，(14)式所示為高斯-牛頓迭代算法的迭代規(guī)則[10]：

(14)

式中：ΔX=[ΔX1，ΔX2，…，ΔXk]；J()為包含滑動窗內(nèi)所有節(jié)點的全局雅克比矩陣，

J(

(15)

r()=[r1,r1,…,rk]為當(dāng)前估計值下的觀測量殘差，

(16)

在高斯-牛頓迭代求解過程中，需要在每個迭代周期內(nèi)應(yīng)用最小二乘法計算ΔX的值，一旦計算得到ΔX，則將+ΔX的值作為下次迭代的初始值，若ΔX足夠小或者超出迭代次數(shù)則取當(dāng)前值作為最終的狀態(tài)估計量。(15)式中JZ和JX的導(dǎo)數(shù)可以分別通過(17)式～(19)式計算得到：

(17)

(18)

(19)

2.2 增量平滑算法

隨著系統(tǒng)工作時間的逐漸增加，F(xiàn)G算法中產(chǎn)生的因子節(jié)點逐漸增多，而每次因子節(jié)點的產(chǎn)生都需要重新求解已有各個因子節(jié)點中狀態(tài)量的最優(yōu)估計值，但每次最優(yōu)化過程都需要重新產(chǎn)生J矩陣，并對J矩陣進行QR分解，得到平方根信息矩陣R，以完成矩陣求逆和最小二乘解的計算，上述過程會帶來較大的計算負擔(dān)，也對系統(tǒng)的實時性造成了影響。

假設(shè)(20)式所示為通過QR算法計算得到的當(dāng)前因子圖的平方根信息矩陣R[10]：

(20)

式中：×表示矩陣中的非0元素。

當(dāng)有新的因子節(jié)點產(chǎn)生時，對應(yīng)的平方根信息矩陣R′為

(21)

2.3 時變導(dǎo)航子系統(tǒng)觀測量狀態(tài)對FG算法的影響

最大似然估計法是傳統(tǒng)的μ和Σ的估計算法[18]，假設(shè)p為Xi的維數(shù)，m為樣本數(shù)量，最大似然估計算法只有滿足m?p，才能得到較為準(zhǔn)確的估計值。然而在多源組合導(dǎo)航的應(yīng)用中，各導(dǎo)航源的觀測量更新速率不同，且測量精度變化較快，如果應(yīng)用最大似然類估計算法則會造成估計滯后或估計誤差大的問題。最大后驗估計在最大似然估計的基礎(chǔ)上又增加了先驗概率的輔助，而先驗概率的加入使得估計在較小的數(shù)據(jù)集中能夠保持有良好的泛化性能[19]。基于以上原因，本文在傳統(tǒng)最大后驗估計算法的基礎(chǔ)上提出了一種應(yīng)用于因子圖的迭代型均值向量和協(xié)方差矩陣的最大后驗估計算法。

3 均值向量和協(xié)方差矩陣的最大后驗估計

3.1 導(dǎo)航狀態(tài)分布參量的迭代最大后驗估計

最大后驗估計融入了待估計量的先驗分布，因此最大后驗估計可以看作規(guī)則化的最大似然估計。以觀測誤差EZ為例，為了在樣本數(shù)量遠低于估計量維度數(shù)量的情況下得到較為準(zhǔn)確的μZ和ΣZ估計值，首先假設(shè)μZ和ΣZ服從Gaussian-Wishart分布，即(μZ,ΣZ)～NIW(μZ0,k0,v0,Ψ)，其中μZ0為μZ的均值，Ψ為ΣZ的先驗值，v0為ΣZ的自由度，k0為μZ協(xié)方差的尺度因子，μZ與ΣZ的聯(lián)合概率密度[20]可以表示為

(22)

式中：

(23)

d為數(shù)據(jù)的維度數(shù)；μ0為所有導(dǎo)航源的均值向量。

(24)

(25)

(26)

(27)

(28)

(29)

(30)

(31)

另外，在傳統(tǒng)基于滑動窗的FG算法中，由于在優(yōu)化過程中對狀態(tài)量和觀測量的分布不進行更新，當(dāng)優(yōu)化窗向前滑動時，無需進行特殊的操作，直接應(yīng)用先驗分布進行優(yōu)化操作。但是在本文提出的基于迭代最大后驗估計的因子圖IMAPFG算法中，當(dāng)窗向前滑動時，需要將上一時刻估計得到的狀態(tài)量和觀測量分布作為新窗中各個狀態(tài)的先驗分布，即μZ0和Ψ.

最后需要說明的是，對協(xié)方差矩陣的動態(tài)估計，實際上是一種動態(tài)加權(quán)的過程，即當(dāng)某一時刻測量值的殘差較大時，估計得到的協(xié)方差矩陣也會相應(yīng)增大，因此在下一次迭代周期中起到了降低權(quán)值的作用。隨著迭代的不斷進行，對于該觀測值的信任會不斷下降，該過程有助于提高融合算法對于野值點干擾的魯棒性。而算法中對于均值向量的估計一方面能夠提高協(xié)方差矩陣估計的準(zhǔn)確性，另一方面，則能很好地解決由觀測量偏置而引起的融合算法異常的問題。

綜上所述，本文所提出的IMAPFG算法步驟為：

1) 設(shè)置各導(dǎo)航系統(tǒng)與因子圖初始參數(shù)，并定義狀態(tài)空間向量，確定滑動窗范圍。

2) 當(dāng)子導(dǎo)航系統(tǒng)輸出測量值時，首先產(chǎn)生新的狀態(tài)節(jié)點加入因子圖中，并應(yīng)用(5)式～(7)式根據(jù)IMU系統(tǒng)的觀測量計算該狀態(tài)節(jié)點的初始值，同時將產(chǎn)生觀測節(jié)點連接到該狀態(tài)節(jié)點上。

3) 判斷是否滿足滑動窗口范圍：當(dāng)超出窗口范圍時，向前滑動窗或產(chǎn)生新的窗，并將上一窗內(nèi)估計得到的狀態(tài)量和觀測量分布作為當(dāng)前窗的先驗估計；當(dāng)滿足滑動窗口大小時，則直接進行步驟4.

4)應(yīng)用(14)式對窗口范圍內(nèi)的因子圖進行最優(yōu)化融合，并根據(jù)(27)式～(31)式對各系統(tǒng)的協(xié)方差矩陣和均值向量進行更新，當(dāng)達到迭代次數(shù)上限或者殘差足夠小時退出迭代。

5) 輸出滑動窗內(nèi)各狀態(tài)節(jié)點的狀態(tài)估計值，將各系統(tǒng)的協(xié)方差矩陣和均值向量的最終估計值作為下一個滑動窗內(nèi)各參量的初始值，滑動窗向后移動l個節(jié)點，返回步驟2.

3.2 迭代更新對最優(yōu)化系統(tǒng)收斂性的影響

在FG算法中，非線性算子F為多個因子節(jié)點根據(jù)(5)式～(7)式進行狀態(tài)更新的集合，對F關(guān)于各個因子節(jié)點狀態(tài)求導(dǎo)，即可得到J矩陣，因此F可導(dǎo)。在3.1節(jié)中提出的最優(yōu)化算法中，J矩陣在每個迭代周期內(nèi)都根據(jù)觀測值進行調(diào)整，分析(15)式與(17)式～(19)式可知，新算法對J矩陣的調(diào)節(jié)相當(dāng)于對矩陣中的各行元素分別乘以一個常數(shù)，如(32)式所示：

(32)

式中：Ji為矩陣J的第i行；ai表示新算法對矩陣J中第i行的調(diào)節(jié)系數(shù)。根據(jù)矩陣行列式的性質(zhì)可知J′的行列式滿足：

(33)

4 測試實驗設(shè)置

為了驗證本文提出的IMAPFG算法的性能，分別進行了仿真測試與實驗測試。在仿真測試中產(chǎn)生了不同種類的傳感器測量變化，并將融合結(jié)果與真實參考值比較，可以定量地評價各種算法的優(yōu)劣；而在實驗測試中，則應(yīng)用了真實的傳感器數(shù)據(jù)驗證融合算法的性能。

圖2 載體運動軌跡圖Fig.2 Trajectory diagram of platform

在仿真測試中，共產(chǎn)生了1 312 s的載體軌跡，軌跡中包括加速、減速、上升、下降、轉(zhuǎn)向等多種載體運動方式，如圖2所示為所產(chǎn)生的載體三維軌跡圖和東向、北向、天向軌跡曲線。根據(jù)載體軌跡分別模擬產(chǎn)生了INS輸出、GNSS輸出、LiDAR導(dǎo)航系統(tǒng)輸出和磁力計導(dǎo)航系統(tǒng)輸出，各模擬導(dǎo)航源的詳細規(guī)格如表1所示。為了驗證各算法對導(dǎo)航源變化的處理性能，需要在各個子導(dǎo)航源中模擬觀測量狀態(tài)變化過程：對于LiDAR系統(tǒng)而言，軌跡中載體的上升與下降過程會導(dǎo)致LiDAR系統(tǒng)中用于地圖匹配的特征點數(shù)量、匹配準(zhǔn)確度下降，從而影響輸出的載體位置精度[3]；對于GNSS與磁力計則人為地分別在特定時間段加入了偏置與噪聲。為了更好地驗證算法在不同子系統(tǒng)狀態(tài)變化情況下的性能，在仿真實驗中分別設(shè)置了兩種測試方案：在方案1中各個時刻內(nèi)有且只有1種子導(dǎo)航源狀態(tài)發(fā)生變化，且變化方式均為突變；方案2中的子導(dǎo)航源狀態(tài)變化形式更加復(fù)雜，在同一時段內(nèi)最多有3種導(dǎo)航源同時發(fā)生變化，且變化形式也包含突變和漸變兩種。表2展示了仿真實驗中兩種方案下各導(dǎo)航源的變化情況。在仿真測試中，分別應(yīng)用本文所提出的IMAPFG算法、普通FG算法[10]、基于最大似然估計的因子圖(MLEFG)算法[22]和基于傳統(tǒng)最大后驗估計的因子圖(MAPFG)算法[19]對各個導(dǎo)航源輸出數(shù)據(jù)進行融合，并比較融合結(jié)果。由于最大似然估計需要較大的樣本空間才能得到較好的估計結(jié)果，對于MLEFG算法，每100個樣本值才對各導(dǎo)航源的分布狀態(tài)進行更新。在最大后驗估計中，由于加入了先驗信息，無需大量樣本也能得到準(zhǔn)確的估計值。在MAPFG算法中，每10個樣本值就對導(dǎo)航源的方差和均值進行更新。IMAPFG算法的均值和方差是隨著窗的滑動進行的，在仿真中3種算法的窗的大小都被設(shè)定為10，即每當(dāng)產(chǎn)生10個觀測值后進行因子圖數(shù)據(jù)融合。另外，為了更符合實際情況，初始化的各子導(dǎo)航源的協(xié)方差矩陣都與仿真的數(shù)據(jù)有所偏差。

表1 仿真測試中各導(dǎo)航源參數(shù)Tab.1 Specifications of navigation sources for simulation test

表2 仿真測試中導(dǎo)航源觀測值變化設(shè)置Tab.2 Setting of observation values

實驗測試中所用的載體平臺如圖3(a)所示，圖中手推車用于搭載Novatel GNSS天線，慣性測量單元 (IMU)和UWB無線電單元，所有傳感器都通過脈沖秒(PPS)信號與GPS時間同步。 UWB網(wǎng)絡(luò)由6個節(jié)點組成，UWB測量結(jié)果與GNSS測量結(jié)果由筆記本電腦記錄。實驗中UWB系統(tǒng)的輸出頻率為10 Hz， GPS系統(tǒng)的輸出頻率為2 Hz，磁力計輸出頻率為10 Hz，表3列出了實驗中所用IMU的具體規(guī)格參數(shù)。在實驗測試中，手推車在36 m×5 m的區(qū)域內(nèi)進行折返運動，實驗時間約為1 500 s. 手推車的運動軌跡如圖3(b)中所示，實驗場景被選在了植被覆蓋的邊緣區(qū)域，在此區(qū)域內(nèi)，IMU、UWB和磁力計的導(dǎo)航輸出保持穩(wěn)定，但植被的遮擋會造成GPS系統(tǒng)的定位精度下降，或出現(xiàn)固定位置誤差偏置等問題。為了量化各融合算法對于處理子系統(tǒng)觀測狀態(tài)變化的性能，在實驗環(huán)境中，IMU、UWB與磁力計基本不受到外界因素的影響，因此觀測量有較高的準(zhǔn)確度，故采用以IMU/UWB/磁力計3種導(dǎo)航源的標(biāo)準(zhǔn)因子圖估計軌跡作為導(dǎo)航狀態(tài)參考真實值，相比于應(yīng)用實驗中標(biāo)記的載體軌跡作為參考真實值，這樣的設(shè)置可以增加速度與姿態(tài)的定量對比，同時也避免實驗過程中由于人為操作帶來的誤差。之后分別應(yīng)用不同融合算法計算得到IMU、UWB、GPS、磁力計4種導(dǎo)航源組合的估計軌跡，并與真實軌跡進行比較。在實驗測試中，用于比較的融合算法種類與仿真實驗中一致。

圖3 實驗場景Fig.3 Experimental scene

在仿真與實驗測試中分別用到了GNSS、LiDAR、UWB和磁力計4種子導(dǎo)航源。由于各子導(dǎo)航源的工作原理不同，輸出的導(dǎo)航觀測量與對應(yīng)的量測方程也有所不同。以下分別介紹4種子導(dǎo)航源對應(yīng)的量測方程。

對于本文中仿真和實驗所用的GNSS、LiDAR、UWB導(dǎo)航系統(tǒng)，輸出的觀測量都為載體的位置參數(shù)，因此這3種子導(dǎo)航源量測方程可分別表示為

表3 實驗測試中MEMs和IMU系統(tǒng)參數(shù)Tab.3 Specifications of MEMs and IMU for experimental test

(34)

(35)

(36)

磁力計可以通過粗對準(zhǔn)、坐標(biāo)轉(zhuǎn)換、磁航向計算、四元數(shù)更新和循環(huán)計算磁航向等步驟輸出連續(xù)的載體姿態(tài)觀測信息。需要說明的是，磁力計在粗對準(zhǔn)過程中需要利用加速度計測量值來簡單計算橫滾角以及俯仰角，之后對磁數(shù)據(jù)完成坐標(biāo)轉(zhuǎn)換，并計算航向角；而在循環(huán)計算磁航向步驟中，磁力計仍需要從慣導(dǎo)系統(tǒng)中取得橫滾角和俯仰角才能完成磁航向的計算。雖然磁力計實際上只提供了航向角的觀測信息，但將測量航向角、橫滾角、俯仰角轉(zhuǎn)化為四元數(shù)后，則視為產(chǎn)生了四元數(shù)觀測值。

基于上述分析，磁力計的量測方程可表示為

(37)

5 測試結(jié)果和分析

在融合算法中協(xié)方差矩陣的跡可以用來表示觀測量的準(zhǔn)確性，跡的值越小說明觀測量誤差越小，越可以信賴，反之則誤差越大，越不可信。圖4展示了在仿真測試方案1中，當(dāng)分別應(yīng)用MLEFG、MAPFG和IMAPFG 3種融合算法時，LiDAR導(dǎo)航系統(tǒng)協(xié)方差矩陣跡的變化曲線，圖中用A、B、C、D 4個區(qū)域標(biāo)記了表2中定義的子導(dǎo)航源變化區(qū)間。從圖4中可以看出，MLEFG算法由于需要的觀測樣本大，對于LiDAR觀測量的估計與真實情況有較大延遲。而IMAPFG算法與MAPFG算法則能夠較為快速地對觀測量變化做出反應(yīng)，且IMAPFG的協(xié)方差估計值更為穩(wěn)定，曲線更加平滑，估計性能更好。特別地，當(dāng)進入?yún)^(qū)域B時，受到GPS偏置的影響，3種算法對協(xié)方差矩陣的估計都出現(xiàn)跳變，但是由于IMAPFG算法能夠在迭代過程中實時更新均值向量，IMAPFG算法的協(xié)方差矩陣估計值受到均值變化的影響也較小，相應(yīng)地，IMAPFG算法對GPS均值向量的估計也更加快速、準(zhǔn)確。

圖4 LiDAR導(dǎo)航源觀測誤差協(xié)方差矩陣的跡的變化曲線Fig.4 Trace curves of covariance matrix of LiDAR navigation observation error

圖5 仿真測試方案1中各算法的載體位置、速度、航向角的估計誤差變化曲線Fig.5 Estimation error of the position, velocity and heading angle of algorithms in the simulation test of Scenario 1

如圖5所示為仿真測試方案1中應(yīng)用不同算法得到的載體位置、速度、航向角的誤差變化曲線。從圖5中可以看出，在開始階段，各導(dǎo)航源相對穩(wěn)定時，4種導(dǎo)航融合算法的性能差別較小。但是，由于在各個子導(dǎo)航源的初始方差中人為地加入了誤差，相比于傳統(tǒng)的FG算法，具有方差估計功能的其他3種融合算法對載體位置、速度、航向角的估計更加精確。隨著導(dǎo)航源狀態(tài)的變化，所有融合算法都受到一定程度的影響，各導(dǎo)航狀態(tài)估計值的精度均有所下降。其中FG算法受到的影響最大，且在各狀態(tài)各區(qū)域內(nèi)，子導(dǎo)航源狀態(tài)變化所帶來影響并沒有隨著時間推移而減弱。相比于FG算法，其余3種算法都能逐漸降低子導(dǎo)航源變化帶來的影響。以區(qū)域A為例，當(dāng)LiDAR導(dǎo)航源輸出的載體位置精度降低時，3種算法通過不同的方式逐漸提高LiDAR導(dǎo)航源的協(xié)方差值，從而在融合過程中減小對LiDAR導(dǎo)航源輸出數(shù)據(jù)的依賴。由于基于最大后驗估計的MAPFG算法與IMAPFG算法能夠更加迅速地對子導(dǎo)航源的變化作出準(zhǔn)確反應(yīng)，對導(dǎo)航狀態(tài)量的估計更加準(zhǔn)確、受到子系統(tǒng)影響更小。進一步，由于在IMAPFG算法中，協(xié)方差矩陣和均值向量的最大后驗估計過程是在迭代過程中完成的，在每次導(dǎo)航狀態(tài)的估計中，殘差與子系統(tǒng)狀態(tài)信息相互影響，保證了估計值的最優(yōu)性，從而使得每次對協(xié)方差矩陣、均值向量相比于MAPFG算法都更加準(zhǔn)確。從圖5中也能看出，相比于MAPFG算法，IMAPFG算法對于子系統(tǒng)變化的反應(yīng)更快，對導(dǎo)航狀態(tài)量的估計更加準(zhǔn)確。

表4中定量地展示了4種算法在方案1中各個區(qū)域以及全部軌跡中導(dǎo)航狀態(tài)量的估計誤差情況。從表4中可以看出，相比于另外3種算法，由于缺少對各子系統(tǒng)觀測量狀態(tài)的更新，F(xiàn)G算法在A、B、C、D 4個區(qū)域內(nèi)輸出的位置、速度和航向角的估計精度都明顯低于其他3種算法，而在整個仿真軌跡中，F(xiàn)G算法的平均位置誤差為0.17 m，平均速度誤差為0.57 cm/s，平均航向角誤差為，均為4種算法中最大。而IMAPFG算法，由于能夠?qū)ψ酉到y(tǒng)狀態(tài)實時、準(zhǔn)確地更新，從而能夠更好地應(yīng)對子系統(tǒng)觀測狀態(tài)的變化或惡化，其平均位置估計誤差僅為0.05 m，相比于MAPFG算法、MLEFG算法、FG算法，IMAPFG算法的定位精度分別提高了28%、50%和71%. 除了定位精度外，IMAPFG算法的定速精度與定姿精度也有顯著提高。

表4 仿真實驗中各區(qū)域以及全部軌跡中導(dǎo)航 狀態(tài)量的估計誤差Tab.4 Estimation errors of navigation state in different regions and whole trajectories in simulation experiments

圖6 仿真測試方案2中各算法的載體位置、速度、航向角的估Fig.6 Estimated errors of position, velocity and heading angle of algorithms in Scenario 2

圖6中展示了仿真測試方案2中應(yīng)用不同算法得到的載體位置、速度、航向角的誤差變化曲線。橫向?qū)Ρ葓D5中各個算法的導(dǎo)航誤差結(jié)果可知，圖6中更為復(fù)雜且持續(xù)時間更長的子系統(tǒng)時變狀態(tài)導(dǎo)致各算法的最終導(dǎo)航精度都惡化較多，特別是在有多個子導(dǎo)航源同時產(chǎn)生變化的H區(qū)域內(nèi)時。但縱向?qū)Ρ雀魉惴ㄖg的性能可以看出，由于對仿真中GNSS子系統(tǒng)均值向量和協(xié)方差矩陣更加準(zhǔn)確地估計，IMAPFG算法在各種導(dǎo)航狀態(tài)的估計都更加準(zhǔn)確。在方案2中，IMAPFG算法輸出的位置三維誤差均值、速度三維誤差均值和航向角無誤差均值分別為0.13 m、0.4 cm/s和4.8×10-5rad. 在方案2中，相比于MAPFG算法、MLEFG算法、FG算法，IMAPFG算法的定位精度分別提高了31%、63%和86%.

在實驗測試中，如圖7(a)中所示為實驗測試中根據(jù)IMU、UWB、磁力計3種導(dǎo)航源估計得到的手推車運動軌跡，在實驗中將該軌跡視為真實軌跡。手推車在實驗人員的操作下在開闊區(qū)域與植被遮擋區(qū)域間做折返運動，遮擋區(qū)域如圖7(a)中陰影部分所示，圖7(a)中J、K紅色矩形框標(biāo)記區(qū)域分別為開闊區(qū)域下軌跡和遮擋區(qū)域下軌跡，圖7(b)與圖7(c)分別為矩形框J、K標(biāo)記區(qū)域的放大圖，圖中藍色圓點表示GPS的載體位置觀測值。從圖7中可以看出：在J區(qū)域下，GPS的測量值與軌跡基本保持一致，且抖動較小；而在K區(qū)域下，受到植被遮擋的影響，GPS輸出的位置觀測值受到較大影響，相比于區(qū)域J，可以明顯看出區(qū)域K內(nèi)GPS的觀測值抖動更大且伴有一定的偏置。

表5中定量地展示了應(yīng)用不同算法計算得到的IMU/UWB/GPS/磁力計多源組合導(dǎo)航系統(tǒng)的導(dǎo)航狀態(tài)量的估計誤差。從表5中可以看出，在開闊區(qū)域內(nèi)，由于能快速、準(zhǔn)確地估計子導(dǎo)航源的觀測精度，IMAPFG算法擁有最小位置、速度、航向角估計精度。而相比于開闊區(qū)域，4種算法在經(jīng)過遮擋區(qū)域時，導(dǎo)航精度均有所下降，這主要是由于GPS觀測值在遮擋區(qū)域內(nèi)惡化而引起的。但是，相比于其他3種算法，IMAPFG算法的定位精度惡化程度最低，IMAPFG算法的定位精度僅下降了18%，而對于MAPFG、MLEFG和FG算法而言，定位精度分別下降了28%、29%以及31%. 橫向比較各算法的導(dǎo)航精度可以看出，IMAPFG算法在整個實驗測試軌跡內(nèi)擁有最優(yōu)的導(dǎo)航精度，相比于MAPFG、MLEFG和FG算法， IMAPFG算法的定位精度分別提高了25%、59%、60%. 另外，除了定位性能外，無論在開闊區(qū)域還是遮擋區(qū)域內(nèi)，IMAPFG算法的定速與定姿精度也均為最優(yōu)。

另外需要說明的是，雖然在仿真測試中生成的INS與實驗測試中應(yīng)用的INS精度相差近10倍，但是融合結(jié)果卻相差不多，這主要是由于更新頻率和子系統(tǒng)精度的差異引起，具體有以下兩點原因：一方面，實驗測試中采用UWB系統(tǒng)代替LiDAR系統(tǒng)，而UWB系統(tǒng)能夠提供更新頻率更高的子系統(tǒng)觀測量(10 Hz);另一方面，在實驗測試中采用INS、UWB、磁力計3種導(dǎo)航源應(yīng)用標(biāo)準(zhǔn)FG算法計算得到的融合結(jié)果作為參考真實值，而當(dāng)再次使用以上3種導(dǎo)航源數(shù)據(jù)作為觀測量時，就使得它們的測量值與視為參考真實值的導(dǎo)航結(jié)果較為接近，即相對精度更高，因此導(dǎo)致相對于該參考真實值的導(dǎo)航精度較高。

表5 實驗測試中各算法性能比較Tab.5 Performance comparison of the algorithms in the experimental test

6 結(jié)論

針對傳統(tǒng)多源組合導(dǎo)航因子圖融合算法精度受到觀測量噪聲時變特性影響的問題，本文提出了一種基于迭代最大后驗估計的IMAPFG算法，為了評價算法的性能，分別設(shè)計了仿真測試與實驗測試對算法進行驗證，并將測試結(jié)果與FG、MLEFG和MAPFG算法進行比較。所得主要結(jié)論如下：

1)仿真測試結(jié)果顯示IMAPFG算法能夠?qū)ψ酉到y(tǒng)狀態(tài)變化做出快速準(zhǔn)確的反應(yīng)，相比于FG、MLEFG和MAPFG算法，IMAPFG算法輸出的定位精度分別提高了28%、50%和71%.

2)在實驗測試中，無論在開闊區(qū)域還是植被遮擋區(qū)域，IMAPFG算法的定位、定速、定姿精度均為4種算法中最優(yōu)。

3)本文所提出的IMAPFG算法能夠有效提高子系統(tǒng)觀測狀態(tài)變化時的多源組合導(dǎo)航估計精度。