基于因子圖的魯棒性增量平滑算法的水面無人艇組合導(dǎo)航方法

戴海發(fā)，卞鴻巍，馬 恒，王榮穎

（海軍工程大學(xué) 電氣工程學(xué)院，武漢 430033）

隨著作戰(zhàn)方式的變革，水面無人艇等（USV）智能化無人作戰(zhàn)系統(tǒng)已成為現(xiàn)在及未來軍事武器裝備發(fā)展的主流趨勢，并得到了世界各國的重視。與常規(guī)艦艇相比，水面無人艇具有小型、無人、反應(yīng)快速、機動靈活、隱蔽性好、可長航等特點，適用于執(zhí)行危險以及不適于載人船只執(zhí)行的任務(wù)。

當(dāng)前，水面無人艇的研制特點趨于小型化、低成本化和高精度化。在設(shè)計水面無人艇導(dǎo)航系統(tǒng)時，采用MEMS傳感器可以大大降低系統(tǒng)的成本，同時減小無人艇的體積，然而MEMS傳感器普遍精度較低，單獨使用，無法實現(xiàn)高精度導(dǎo)航。為了提高水面無人艇導(dǎo)航系統(tǒng)的精度和可靠性，需要利用衛(wèi)星導(dǎo)航系統(tǒng)、計程儀、天文導(dǎo)航系統(tǒng)等傳感器來協(xié)助導(dǎo)航。這些傳感器具有不同的誤差特性，結(jié)合多種傳感器的互補特性和余度信息能夠提供比單個傳感器更精確、更具魯棒性的估計。但是這些傳感器通常都按照不同的頻率工作，并且輸出信息有些是非線性測量值，給組合導(dǎo)航系統(tǒng)的設(shè)計增加了挑戰(zhàn)。此外，水面無人艇在不同的任務(wù)和環(huán)境中，需要配置和可以使用的傳感器種類并不完全相同，因此需要根據(jù)實際任務(wù)需求動態(tài)的改變組合導(dǎo)航系統(tǒng)中傳感器的配置。

針對上述問題，傳統(tǒng)的方法大多采用基于聯(lián)邦卡爾曼濾波器的組合導(dǎo)航方法[1-3]。雖然這些方法能夠通過數(shù)據(jù)同步處理方法[4]融合不同速率的傳感器信息并實時計算出導(dǎo)航解，但是為了保持?jǐn)?shù)據(jù)的同步往往需要丟棄一部分測量值，這樣就會造成信息的浪費；同時，標(biāo)準(zhǔn)卡爾曼濾波器只能夠解決線性問題，而大多數(shù)的傳感器模型都包含非線性成分；至于擴展卡曼濾波器，由于邊緣化了過去的狀態(tài)而只使用了最近的狀態(tài)估計，導(dǎo)致表示這些測量值的非線性因子在估計過程中不能夠很好地重新線性化，因此該優(yōu)化過程可能是概率不連續(xù)的，估計值并不是最優(yōu)的。而UKF、PF等非線性濾波器由于計算量大，在工程上難以實現(xiàn)。此外，當(dāng)某個傳感器的信息出現(xiàn)故障變得不可用時，聯(lián)邦濾波器需要進行較復(fù)雜的系統(tǒng)重構(gòu)處理，而且該方法不支持新導(dǎo)航傳感器的擴展。

文獻[5-6]針對即時定位與構(gòu)圖問題，提出了一種基于因子圖的導(dǎo)航傳感器信息融合方法，該方法將信息融合問題用因子圖模型來表示。因子圖將未知變量結(jié)點和已知測量值的關(guān)系進行編碼，融合來自不同的、可能非同步的傳感器觀測值的問題就變成在因子圖中連接測量值定義的因子與相應(yīng)結(jié)點的問題。使用因子圖可以使系統(tǒng)具有即插即用的功能，當(dāng)有新的傳感器接入系統(tǒng)時，只需要在因子圖中增加新的結(jié)點；同樣的，當(dāng)某個傳感器由于信號丟失或者傳感器故障變得不可用時，系統(tǒng)只需要限制增加相應(yīng)的因子，而不需要特別的處理。但是該方法存在計算量大、延遲的問題。為此，文獻[7]提出了一種基于因子圖的增量平滑算法的信息融合方法，該方法只更新受新觀測值影響的貝葉斯網(wǎng)絡(luò)，避免了一些重復(fù)計算，極大地減少了計算量，達到了實時估計的要求。文獻[8]將該方法應(yīng)用到了 IMU/視覺/磁力計等機器人導(dǎo)航系統(tǒng)中，也取得了較好的效果。

對于水面無人艇的多傳感器組合導(dǎo)航，經(jīng)常會出現(xiàn)測量值異常的情況（比如GNSS接收機受到干擾或壓制、多普勒計程儀無法探測到海底），這些異常測量值一般稱為野值點。如果不對這些野值點進行處理，就會污染導(dǎo)航信息的融合結(jié)果，導(dǎo)致估計的導(dǎo)航狀態(tài)精度下降。目前常用的魯棒性方法包括傳統(tǒng)的M估計和基于圖優(yōu)化的方法[9]，M 估計主要包括 Huber和Cauchy方法?；趫D優(yōu)化的方法包括可切換約束（Switch Constraint）、動態(tài)協(xié)方差尺度（Dynamic Covariance Scaling）以及最大混合（Max-mixtures）法，前兩者適用于單峰高斯噪聲分布，而后者適用于混合高斯分布。文獻[9]通過試驗證明了基于圖優(yōu)化的方法性能要略優(yōu)于M估計的方法，此外，在圖優(yōu)化的方法中，可切換約束的方法性能最佳，但是計算量大。

本文針對上述問題，結(jié)合文獻[7]的信息融合方法和文獻[9]的可切換約束魯棒性方法提出了一種新的基于因子圖的水面無人艇組合導(dǎo)航方法。第1節(jié)闡述了組合導(dǎo)航系統(tǒng)的因子圖表示方法；第2節(jié)推導(dǎo)了先驗及常用傳感器的因子表達式；第3節(jié)首先介紹了捆綁優(yōu)化的原理，然后闡述了增量平滑算法的原理，最后針對異常測量值的問題，對切換約束的方法進行改進并應(yīng)用到增量平滑算法中形成了基于因子圖的魯棒性增量平滑算法；第4節(jié)給出了實時的算法實現(xiàn)架構(gòu)；第5節(jié)通過仿真環(huán)境試驗分析比較了算法的性能，驗證了所提算法的有效性。

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

假設(shè)水面無人艇安裝了一組輸出頻率不同步的傳感器，其中包括產(chǎn)生高頻率輸出的IMU傳感器以及輸出頻率相對較低的全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS）、多普勒計程儀（Doppler Velocity Log,DVL）等。有些傳感器在復(fù)雜環(huán)境下可能變得不可用，而另一些傳感器只在特定情況下可用。組合導(dǎo)航的目的是通過融合所有可用的信息源計算出最有可能的導(dǎo)航解。

進一步假設(shè)x表示導(dǎo)航狀態(tài)，包含水面無人艇的位置p、速度v和姿態(tài)ψ信息；c表示IMU的校正參數(shù)，包括加速度計和陀螺的零偏。令xi、ci分別表示時刻ti的導(dǎo)航狀態(tài)和校正參數(shù)，定義直到當(dāng)前時刻tk的一組全部導(dǎo)航狀態(tài)和全部校正參數(shù)為：

假設(shè)Λk為直到當(dāng)前時刻 tk的所有變量組：Λk={Xk,Ck}。

根據(jù)上述定義，聯(lián)合概率分布函數(shù)為：

其中， Zk為直到當(dāng)前時刻 tk接收到的所有觀測值。

假設(shè)時刻 ti的觀測值為 zi，則那么，待估計參數(shù)的最大后驗估計為

根據(jù)文獻[8]，聯(lián)合概率分布式(2)可以因式分解成一個先驗信息和獨立過程以及測量模型。令 p(Λ0)表示所有的可用先驗信息，則因式分解可寫成：

根據(jù)文獻[7]，上述因式分解可以表示成因子圖模型。如圖1所示，因子圖是一種雙邊圖，包含兩種類型的結(jié)點（因子結(jié)點和變量結(jié)點），邊表示因子結(jié)點和變量結(jié)點存在關(guān)系。圖中的每個因子結(jié)點都可表示式(4)中的一個獨立項，因此：

圖1 包含多頻率測量值的因子圖：高頻率的IMU測量值、以及低頻率的GNSS、DVL測量值Fig.1 Factor graph representation with multi-frequency measurements: high-frequency IMU measurement,lower-frequency GNSS and DVL measurements

其中，d(.)表示代價函數(shù)。

對于高斯噪聲分布，因子 fi滿足如下形式：

特別地，表示測量模型的因子定義為：

式中，h(.)為非線性測量函數(shù)，zi為真實測量值。

因此，式(3)的最大后驗估計問題變成了最小化下列非線性最小二乘函數(shù)的問題：

2 通用傳感器的因子公式

從式(9)可知，因子 fi的推導(dǎo)是基于因子圖融合方法的關(guān)鍵問題。本節(jié)將推導(dǎo)水面無人艇主要導(dǎo)航傳感器測量模型的因子公式，這些傳感器包括IMU、GNSS、DVL。

2.1 先驗因子

可用的先驗信息 p(Λ0)可以進一步分解為某些變量的獨立信息，每一個獨立信息可以表示成一個分離的先驗因子。變量 υ ∈Λ0的先驗因子是一個一元因子：

對于高斯分布，先驗信息以均值μυ和方差Συ方式給出：

2.2 IMU等效因子

導(dǎo)航狀態(tài)x 的隨時間變化的關(guān)系可以描述成下述的非線性差分方程：

式中，h 為慣性導(dǎo)航系統(tǒng)的微分方程（參見文獻[10]）；fb、ωb分別是慣性傳感器測量得到的載體坐標(biāo)系下的比力和角加速度；c表示IMU的校正參數(shù)，可根據(jù)IMU誤差模型來修正IMU的測量值 fb和ωb。IMU的誤差模型的估計通常與導(dǎo)航狀態(tài)x 的估計結(jié)合在一起。線性化方程(12)將產(chǎn)生具有雅克比矩陣和過程噪聲的狀態(tài)方程。

通常，c隨時間的變化關(guān)系可以用自身的非線性模型（即隨機游走）來描述：

線性方程(12)(13)的離散化方程為：

根據(jù)同樣的方法，可以得到IMU的偏差因子：

式中，ck+1和ck在因子圖中表示為變量結(jié)點。

在實際中，考慮到導(dǎo)航應(yīng)用的實時性，按照IMU的輸出頻率在因子圖中增加新的因子來優(yōu)化導(dǎo)航解并不可行。因此本文采用了文獻[7]中的等效 IMU因子的更新策略，即根據(jù)預(yù)積分算法，將一系列連續(xù)的IMU測量值結(jié)合起來等效成一個IMU因子。

為了避免在重線性化的時候重新計算積分變化量，新的預(yù)積分算法采用了在預(yù)積分起始時刻ti所在的載體坐標(biāo)系上進行積分計算的策略，同時為了避免歐拉角方法的奇異性問題，采用了文獻[10]在流形上進行預(yù)積分的方法。

算法1：IMU測量值的預(yù)積分算法；先前的預(yù)積分變化量1）輸入：IMU測量值、；校正參數(shù)；2）根據(jù)校正參數(shù)修正IMU的測量值；3）位置更新：；4）速度更新：；5）方位更新：的指數(shù)映射；6）輸出：預(yù)積分變化量，其中 表示角速度向量為 .

等效IMU因子為

2.3 GNSS測量模型因子

一般的，GNSS測量方程可表示為：

該因子只與表示當(dāng)前導(dǎo)航狀態(tài)的結(jié)點xk有關(guān)。

當(dāng)GNSS的測量值為偽距時，也可以按照同樣的方法得到GNSS的因子表達式。

2.4 DVL測量模型因子

DVL通過聲波的多普勒效應(yīng)測量載體的對地或?qū)λ俣龋绻麥y量值是對地速度，其測量方程可以表示成：

所以，DVL測量因子可以表示為：

如果測量值為對水速度，其測量方程可以表示成：

根據(jù)文獻[4]，海流速度的變化模型可以用一階Markov模型來描述：

式中，τcur為時間相關(guān)常數(shù)，ηcur為過程噪聲。

海流模型的離散化方程為[17]：

式中，T為時間間隔。因此，水流速度因子可以表示為：

式中，hcur(?)為水流速度預(yù)測函數(shù)，即式(27)。

3 基于魯棒性增量平滑的信息融合

3.1 捆綁優(yōu)化

目前為止，信息融合問題已經(jīng)使用因子圖的表示方式推導(dǎo)出來，并且定義了一些常用傳感器的因子。

因子圖編碼的非線性優(yōu)化問題可以通過在標(biāo)準(zhǔn)高斯-牛頓非線性優(yōu)化器中重復(fù)線性化來解決。從變量組Λk的一個初始估計值開始，高斯牛頓法通過線性化系統(tǒng)找到估計的修正值Δ滿足：

式(30)可通過矩陣求逆得到，但是當(dāng)數(shù)據(jù)量較大時，計算量變得很大，因此一般利用矩陣分解來求解。假設(shè)矩陣J可QR分解為J=QR，則有QTJ=[R0]T且QTb=[dc]T，又根據(jù)式(30)，有RΔ=d。由于R為上三角矩陣，因此可通過回代法求得Δ。得到Δ后，線性化點更新到一個新的估計值

在因子圖的框架中，該過程通過變量消除來完成[8]。首先確定變量消除的順序，然后從因子圖中依次消除各個變量，形成和弦貝葉斯網(wǎng)。在貝葉斯網(wǎng)絡(luò)中，更新值Δ通過回代法得到，具體過程可參見文獻[7]。

3.2 增量平滑

對于導(dǎo)航問題，系統(tǒng)每接收到一個新的傳感器測量值就在圖中產(chǎn)生一個新的因子。這等效于在線性化最小二乘問題的測量雅克比矩陣中增加新的一行，通常雅克比矩陣發(fā)生變化后，就需要從頭對新的雅克比矩陣進行分解，但這樣隨著時間的增長計算量會越來越大。根據(jù)文獻[8]，大部分的計算都和先前步驟是相同的，因此可以重復(fù)使用。當(dāng)使用 3.1節(jié)的算法重新計算貝葉斯網(wǎng)絡(luò)時，可以發(fā)現(xiàn)貝葉斯網(wǎng)絡(luò)中只有很少一部分會因為新增加的因子而發(fā)生變化，因此并不需要每次都從頭開始計算。

因此，當(dāng)新的測量值到來時，只需要關(guān)注貝葉斯網(wǎng)絡(luò)中受到影響的部分。一旦識別出貝葉斯網(wǎng)絡(luò)中受到影響的部分，就將該部分轉(zhuǎn)換回因子圖的形式，并加上新的因子，然后將產(chǎn)生的因子圖重新進行變量消除；再將變量消除得到的貝葉斯網(wǎng)絡(luò)融合到原來未受影響的貝葉斯網(wǎng)絡(luò)中，這樣得到的結(jié)果與捆綁優(yōu)化的結(jié)果是相同的。關(guān)于受到影響的貝葉斯網(wǎng)絡(luò)識別算法，本文采用的是文獻[7]提出的方法。

3.3 魯棒性

為了提高因子圖方法的魯棒性，一般會采用可切換約束的方法?？汕袚Q約束的方法在2012年被提出用于SLAM中檢測錯誤的閉環(huán)約束。后來，將其用來消除GNSS在城市環(huán)境中的多徑效應(yīng)[12]。文獻[9]將代價函數(shù)修正如下：

式中，i表示1到k時刻之間的測量值序號，j表示殘差未超過閾值的測量值序號，s∈{0,1}為切換約束變量，Sk表示到時刻k為止所有的約束變量，sj=0表示第j個測量值為正常值，否則為野值；γj為切換約束變量的初始估計，Ξ為切換約束變量的方差，ψ(sj)為實值函數(shù)且滿足：

與普通因子圖方法不同的是，該方法在對所有的狀態(tài)量進行優(yōu)化后，接著計算每一個測量值的殘差，如果殘差大于一定的閾值，則會在因子圖中加入與該測量值相關(guān)的切換變量，然后重新優(yōu)化，從而減輕野值點對優(yōu)化結(jié)果的影響。具有可切換變量的因子圖如圖2所示。

圖2 具有可切換約束的因子圖Fig.2 Factor graph with switchable constraint

顯然，由于上述方法需要進行兩遍優(yōu)化，因此計算量也成倍增加，不利于實時實現(xiàn)。針對上述問題，本文對該方法進行如下改進：

在將新測量值加入因子圖之前，先對測量值做一個判斷，如果測量值和預(yù)測值的殘差小于閾值，則按照普通的因子圖方法進行優(yōu)化，否則，測量值可能是野值點，此時在因子圖上增加一個與該測量值相關(guān)的可切換變量，使得該切換變量與狀態(tài)量一起進行優(yōu)化，這樣就可以在一遍優(yōu)化的過程中同時完成野值剔除和狀態(tài)估計，從而減少了計算時間。

改進后的代價函數(shù)如式(31)所示：

式中，i*表示殘差未超過閾值的測量值序號。

4 實時性能架構(gòu)

盡管基于增量平滑的因子圖融合算法的計算量要明顯小于捆綁優(yōu)化算法，但是融合一個新測量值的真實計算時間依賴于需要重復(fù)計算的貝葉斯網(wǎng)絡(luò)規(guī)模。但是在實際應(yīng)用中，需要實時的導(dǎo)航解。如果只處理IMU的數(shù)據(jù)，做到實時性是完全可能的；但是當(dāng)外部傳感器的信息到來時，處理時間盡管很小，實時性能還是難以保證。

為了解決這個問題，可以采用并行處理的方式。如算法1所示：當(dāng)IMU的測量值在時刻到來時，采用高優(yōu)先權(quán)的處理過程，根據(jù)算法 1預(yù)融合得到，然后通過、的估計值、以及預(yù)測函數(shù)實時解算出導(dǎo)航解；當(dāng)非IMU數(shù)據(jù)在時間點到來時，可以采用低優(yōu)先權(quán)的處理過程。該過程包括生成一個新的測量因子以及等效的IMU因子和偏差因子，這些因子和導(dǎo)航解以及校正結(jié)點、被加入到因子圖中，根據(jù)預(yù)測函數(shù)初始化這些變量的估計值。特別地，的估計值通過預(yù)測函數(shù)產(chǎn)生；然后進入增量平滑算法，得到新的估計值，同時標(biāo)記為新的預(yù)融合時間起始點，并且以為初始值；接著基于和實時計算導(dǎo)航解，其中k表示當(dāng)前時間；一旦增量平滑完成，就用、的更新估計值替換和，且不需要修改。

算法2：實時性能架構(gòu)1）初始化：新因子；2）初始化：根據(jù)先驗因子，新變量、設(shè)置、，；3）while接收到測量值z時 do；4）if z是IMU 的測量值；5）通過算法1，根據(jù)z更新初始化；6）根據(jù)生成導(dǎo)航解；7）else；8）基于、以及，計算、的預(yù)測值、；9）為測量值z創(chuàng)建一個合適的因子；10）創(chuàng)建等效的IMU因子和偏差因子；11）增加因子；12）if 增量平滑可用；13）對于新的因子和變量結(jié)點、、到和，執(zhí)行增量平滑算法；14）令；15）end if；16）初始化，；18）end if；19）if 增量平滑完成 then；20）重新更新最近的導(dǎo)航和校正參數(shù)結(jié)點的估計值x、c；21）令；17）令，，；

22）end if；23）end while.

5 仿真試驗

本節(jié)中，利用仿真試驗搭建一個 IMU/GNSS/DVL組合導(dǎo)航系統(tǒng)驗證所提出的算法。共設(shè)計了兩組試驗：試驗1設(shè)定所有的測量值都是正常值，然后通過與文獻[6]的聯(lián)邦卡爾曼濾波方法進行對比，驗證本文算法的估計精度和收斂速度；試驗2通過對測量值人為設(shè)置野值，與文獻[14]的普通的基于因子圖增量平滑方法以及文獻[15]的切換約束方法進行了比較，驗證算法的魯棒性。

本文的算法是在開源庫 gtsam的基礎(chǔ)上修改實現(xiàn)，所有的方法都是在一臺單核3.4GHz、16GB RAM內(nèi)存的i7-6700處理器上完成的。

5.1 精度試驗

利用Monte-Carlo仿真水面無人艇的運動，驗證算法的可行性。首先創(chuàng)建了一條地面真實軌跡，仿真了水面無人艇以15m/s的速度航行。初始位置：緯度λ=108°，經(jīng)度L=24°，海拔高度h=0m；初始航向為北向；仿真時間為400 s。軌跡包含幾個勻速直行和大機動階段，運動軌跡如圖2所示，紅色箭頭表示水面無人艇航行方向。

基于地面真實軌跡，理想的IMU以100 Hz的速率產(chǎn)生測量值，同時考慮地球的自轉(zhuǎn)以及重力矢量的變化。共做了100次Monte-Carlo仿真試驗，其中每一次仿真中，這些測量值都加入了常值偏差和零均值白噪聲。 加速度偏差標(biāo)準(zhǔn)差為σ=1mg，陀螺偏差標(biāo)準(zhǔn)差為σ=10(°)/h。加速度計和噪聲方差的標(biāo)準(zhǔn)差分別為σ=100μg/，σ=0.1(°)/。初始位置誤差為(10,10,10)m（東北天坐標(biāo)系），初始速度誤差標(biāo)準(zhǔn)差(0.5,0.5,0.5)m/s，初始姿態(tài)誤差為(0.1,0.1,0.1)°。

此外，假設(shè)水面無人艇上裝有北斗接收機、多普勒計程儀，測量量分別為位置和對地速度，并且分別以 1 Hz、2 Hz的速率發(fā)送信息。理想的北斗接收機的位置和多普勒計程儀的速度信息只包含噪聲誤差，假設(shè)為零均值高斯白噪聲，誤差標(biāo)準(zhǔn)差分別為σGNSS=20 m和σDVL=0.1 kn。

圖3比較了純慣性算法（Inertial）、基于卡爾曼濾波器的聯(lián)邦算法（Federated Kalman Filter,FKF）以及本文提出的增量平滑算法（Incremental Smoothing,IS）的解算軌跡。從圖3中可以看出，隨著時間的增長，純慣導(dǎo)解算結(jié)果誤差不斷積累，導(dǎo)致軌跡嚴(yán)重偏離真實軌跡，而采用組合導(dǎo)航修正慣性器件誤差后，誤差發(fā)散得到了抑制，估計結(jié)果與真實軌跡幾乎吻合。

圖3 真實軌跡與估計軌跡Fig.3 Ground real trajectory VS estimation trajectory

圖4(a)～(e)比較了本文提出的信息融合方法與普通的聯(lián)邦濾波方法的性能，這些性能分別為單次仿真的位置估計誤差、速度估計誤差、姿態(tài)估計誤差、加速度計零偏估計、陀螺零偏估計誤差。其中，IS表示本文提出的基于因子圖的增量平滑算法的信息融合方法，F(xiàn)KF表示文獻[6]提出的基于聯(lián)邦濾波的方法。

從圖4中可以看出，本文所提出的算法相對于卡爾曼濾波的方法對于所有的狀態(tài)量都有更好的估計效果，加速度計與陀螺零偏估計誤差接近于零。此外，由于基于因子圖的增量平滑算法在完成估計的同時也進行平滑處理，因此該算法的估計誤差更加平滑，且收斂速度也要快于傳統(tǒng)的基于濾波的方法。

圖4 增量平滑算法與聯(lián)邦濾波算法的性能對比Fig.4 Comparison between incremental smoothing and federated filter

表1列出了100次蒙特卡洛仿真試驗的各導(dǎo)航誤差的估計均值和標(biāo)準(zhǔn)差。從表中可以看出，本文所提出的算法相對于卡爾曼濾波的方法對于所有的狀態(tài)量都有更好的估計效果，位置估計誤差從8 m減少到5 m以內(nèi)，速度估計誤差從0.15 m/s減少到0.10 m/s以內(nèi)，姿態(tài)誤差從0.4°減少到0.1°以內(nèi)。

表1 100次蒙特卡洛仿真估計誤差統(tǒng)計特性Tab.1 Statistics of estimation error in 100 Monte Carlo simulations

5.2 魯棒性試驗

在試驗1的基礎(chǔ)上，假定 GNSS接收機由于受到干擾導(dǎo)致其從 100～200 s的測量值變得不可用(出現(xiàn)大量野值點)，然后利用 3.3節(jié)提出的改進的具有切換約束的增量平滑方法（Modified Switch Factor-Incremental Smoothing,MSFIS）對 IMU、GNSS、DVL測量值進行融合。

表2列出了普通的增量平滑算法（IS）、具有切換約束的增量平滑算法（Switch Factor-Incremental Smoothing,SFIS），以及MSFIS方法的計算時間。從表2可以看出：SFIS方法的計算時間最長，超過了IS方法的兩倍，這是由于SFIS方法增加約束因子而且進行了兩遍優(yōu)化；而 MSFIS方法雖然也增加了約束因子，但是它只做了一遍優(yōu)化，因此計算時間僅比IS方法稍有增加，相比SFIS方法節(jié)約了大約56%的時間。

表2 三種因子圖方法的計算時間Tab.2 Computation time of the three factor based methods

圖5 幾種基于因子圖方法的平均定位誤差Fig.5 Mean positioning errors for graph-based methods

圖5為 100次 Monte-Carlo試驗 IS、SFIS以及MSFIS三種方法的平均定位誤差。從圖中可以看出，由于野值點的影響，IS方法得到的優(yōu)化結(jié)果嚴(yán)重偏離了真實狀態(tài)，而后兩種方法由于增加了約束因子，對野值點具有較好的魯棒性。此外，從圖中還可以看出，本文所提出的MSFIS方法與SFIS方法估計精度相當(dāng)。綜上可知，本文所提出的MSFIS方法對野值點具有較好的魯棒性，而且顯著的縮短了計算的時間。

6 結(jié) 論

本文提出了一種基于因子圖的魯棒性增量平滑算法的水面無人艇組合導(dǎo)航方法，該方法可以相對容易的對多傳感器非同步測量數(shù)據(jù)進行實時融合，且可根據(jù)傳感器的可用性實現(xiàn)即插即用的功能。本文所提出的方法，基于非線性優(yōu)化的方法處理實時測量數(shù)據(jù)，并結(jié)合增量平滑的方法，實現(xiàn)了最小延遲條件下的近似最優(yōu)估計。

通過 IMU/GNSS/DVL組合系統(tǒng)仿真環(huán)境對本文所提出的方法進行驗證，并與傳統(tǒng)的基于卡爾曼濾波器的聯(lián)邦濾波算法及基于約束因子的魯棒算法進行了性能對比。仿真試驗表明，本文所提出的方法具有比傳統(tǒng)基于濾波的方法更好的估計精度和更快的收斂速度；此外，該方法對野值點具有良好的魯棒性，且計算時間要少于基于約束因子的魯棒方法。由于篇幅限制，本文只推導(dǎo)了一些常用傳感器的因子公式，并且假設(shè)測量噪聲滿足高斯分布，下一步將對更多傳感器的因子公式以及非高斯測量噪聲條件下的組合導(dǎo)航系統(tǒng)的融合方法進行深入研究。