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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 通用傳感器的因子公式

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

2.1 先驗(yàn)因子

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

對(duì)于高斯分布，先驗(yàn)信息以均值μυ和方差Συ方式給出：

2.2 IMU等效因子

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

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

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

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

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

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

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

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

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

等效IMU因子為

2.3 GNSS測(cè)量模型因子

一般的，GNSS測(cè)量方程可表示為：

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

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

2.4 DVL測(cè)量模型因子

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

所以，DVL測(cè)量因子可以表示為：

如果測(cè)量值為對(duì)水速度，其測(cè)量方程可以表示成：

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

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

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

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

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

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

3.1 捆綁優(yōu)化

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

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

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

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

3.2 增量平滑

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

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

3.3 魯棒性

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

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

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

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

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

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

改進(jìn)后的代價(jià)函數(shù)如式(31)所示：

式中，i*表示殘差未超過閾值的測(cè)量值序號(hào)。

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

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

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

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

22）end if；23）end while.

5 仿真試驗(yàn)

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

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

5.1 精度試驗(yàn)

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

基于地面真實(shí)軌跡，理想的IMU以100 Hz的速率產(chǎn)生測(cè)量值，同時(shí)考慮地球的自轉(zhuǎn)以及重力矢量的變化。共做了100次Monte-Carlo仿真試驗(yàn)，其中每一次仿真中，這些測(cè)量值都加入了常值偏差和零均值白噪聲。 加速度偏差標(biāo)準(zhǔn)差為σ=1mg，陀螺偏差標(biāo)準(zhǔn)差為σ=10(°)/h。加速度計(jì)和噪聲方差的標(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è)水面無人艇上裝有北斗接收機(jī)、多普勒計(jì)程儀，測(cè)量量分別為位置和對(duì)地速度，并且分別以 1 Hz、2 Hz的速率發(fā)送信息。理想的北斗接收機(jī)的位置和多普勒計(jì)程儀的速度信息只包含噪聲誤差，假設(shè)為零均值高斯白噪聲，誤差標(biāo)準(zhǔn)差分別為σGNSS=20 m和σDVL=0.1 kn。

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

圖3 真實(shí)軌跡與估計(jì)軌跡Fig.3 Ground real trajectory VS estimation trajectory

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

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

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

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

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

5.2 魯棒性試驗(yàn)

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

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

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

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

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

6 結(jié) 論

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

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