翟云峰，王冠學(xué)，徐國(guó)華，陳柱，張?chǎng)?/p>

華中科技大學(xué) 船舶與海洋工程學(xué)院，湖北 武漢 430074

0 引 言

自主式水下機(jī)器人（AUV）是一種可在水下探測(cè)并作業(yè)的設(shè)備，在民用和軍用方面有著廣闊的應(yīng)用前景。民用上，其可用于海洋資源的勘測(cè)與開(kāi)發(fā)、海洋救險(xiǎn)等；軍事上，可用于排雷、偵察及戰(zhàn)術(shù)海圖等方面［1］。但這些都需要AUV具備高精度的導(dǎo)航定位能力。而精確的導(dǎo)航信息也是AUV實(shí)現(xiàn)自主作業(yè)并安全返回的必要條件。AUV的工作使命和航行環(huán)境對(duì)導(dǎo)航系統(tǒng)的可靠性及精度提出了非常高的要求。

目前通用的導(dǎo)航模式均為采用多傳感器信息融合的組合導(dǎo)航方法，相較單獨(dú)的導(dǎo)航系統(tǒng)，該方法具備更高的導(dǎo)航精度和容錯(cuò)性能。組合導(dǎo)航系統(tǒng)一般采用卡爾曼濾波器。但是由于導(dǎo)航系統(tǒng)觀測(cè)數(shù)據(jù)間的非線性關(guān)系，當(dāng)前多采用擴(kuò)展卡爾曼濾波（EKF）進(jìn)行導(dǎo)航定位誤差處理算法設(shè)計(jì)。EKF實(shí)際上是利用Taylor函數(shù)展開(kāi)的一階項(xiàng)對(duì)非線性模型進(jìn)行近似線性化，但當(dāng)系統(tǒng)高度非線性時(shí)，EKF可能導(dǎo)致濾波發(fā)散。而Julier等［2-3］提出的無(wú)跡卡爾曼濾波器（UKF）則很好地克服了EKF的缺點(diǎn)，且其計(jì)算復(fù)雜程度并未增加。

本文將基于某新型大尺度欠驅(qū)動(dòng)高速AUV，設(shè)計(jì)一種基于VxWorks嵌入式操作系統(tǒng)的水下機(jī)器人導(dǎo)航系統(tǒng)，同時(shí)考慮到導(dǎo)航系統(tǒng)的容錯(cuò)性和設(shè)計(jì)靈活性，以及導(dǎo)航系統(tǒng)模型的非線性特性，選擇基于UKF的聯(lián)邦卡爾曼濾波器作為組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合算法。

1 AUV系統(tǒng)組成

大尺度高速AUV的系統(tǒng)由水面監(jiān)控系統(tǒng)、通信傳輸系統(tǒng)和艇載控制系統(tǒng)組成，其組成圖如圖1所示。艇載控制系統(tǒng)均已嵌入到AUV內(nèi)部，在頂層通過(guò)水面監(jiān)控系統(tǒng)進(jìn)行全艇的狀態(tài)監(jiān)控及任務(wù)信息下達(dá)，從而實(shí)現(xiàn)AUV的自主作業(yè)。

為了方便系統(tǒng)控制，導(dǎo)航系統(tǒng)要設(shè)計(jì)成黑盒，僅僅實(shí)現(xiàn)測(cè)量輸入和信息輸出。在高航速下，對(duì)AUV導(dǎo)航系統(tǒng)的實(shí)時(shí)性、可靠性和精度提出了更高的要求。

2 導(dǎo)航系統(tǒng)硬件構(gòu)架

AUV導(dǎo)航系統(tǒng)主要配備了捷聯(lián)慣導(dǎo)系統(tǒng)（PHINS）、多普勒測(cè)速儀（DVL）、衛(wèi)星定位系統(tǒng)（GPS）、深度計(jì)和高度計(jì)，其主要參數(shù)如表1所示。導(dǎo)航系統(tǒng)采用嵌入式結(jié)構(gòu)設(shè)計(jì)，在VxWorks6.6實(shí)時(shí)操作系統(tǒng)下實(shí)現(xiàn)AUV的導(dǎo)航定位。VxWorks可以為導(dǎo)航任務(wù)提供高效、實(shí)時(shí)的任務(wù)調(diào)度和中斷管理，并提供實(shí)時(shí)的系統(tǒng)資源以及任務(wù)通信，從而避免導(dǎo)航軟件開(kāi)發(fā)過(guò)程中對(duì)系統(tǒng)資源的管理，提高系統(tǒng)的實(shí)時(shí)性和可靠性。

表1 傳感器參數(shù)Table 1 Senor parameters

圖2所示為AUV導(dǎo)航系統(tǒng)的硬件構(gòu)架。其中導(dǎo)航計(jì)算機(jī)以基于VxWorks的CPCI系統(tǒng)作為硬件載體，包括集成第三代Intel Core i7處理器的核心模塊、4通道的串口卡以及24 V的獨(dú)立電源模塊。導(dǎo)航傳感器通過(guò)串口線與加固機(jī)內(nèi)部的串口卡相連，向加固機(jī)發(fā)送導(dǎo)航數(shù)據(jù)。串口卡與CPU模塊之間通過(guò)CPCI總線通信。導(dǎo)航計(jì)算機(jī)通過(guò)網(wǎng)線與主控計(jì)算機(jī)相連，采用TCP協(xié)議向水下主控系統(tǒng)發(fā)送解算后的導(dǎo)航數(shù)據(jù)。另外，加固機(jī)內(nèi)通風(fēng)截止波導(dǎo)板的設(shè)計(jì)則保證了導(dǎo)航系統(tǒng)良好的電磁兼容性能。

導(dǎo)航系統(tǒng)采用開(kāi)放式硬件構(gòu)架［5］，可以通過(guò)增加導(dǎo)航信息源來(lái)提高導(dǎo)航系統(tǒng)的精度。

3 導(dǎo)航系統(tǒng)軟件構(gòu)架

AUV導(dǎo)航系統(tǒng)的軟件采用模塊化設(shè)計(jì)，以降低程序復(fù)雜度，使程序結(jié)構(gòu)清晰，便于閱讀和理解，方便軟件的設(shè)計(jì)與后續(xù)的完善和修改。

導(dǎo)航工控機(jī)通過(guò)串口實(shí)現(xiàn)PHINS，DVL和GPS等傳感器數(shù)據(jù)的采集，然后通過(guò)組合導(dǎo)航算法解算出所需的導(dǎo)航數(shù)據(jù)，最后通過(guò)以太網(wǎng)形式與上層進(jìn)行交互，即定時(shí)接收到水下主控系統(tǒng)的詢問(wèn)報(bào)文后，導(dǎo)航系統(tǒng)立即將最新解算的導(dǎo)航數(shù)據(jù)發(fā)送給水上主控系統(tǒng)。根據(jù)組合導(dǎo)航系統(tǒng)的功能，可設(shè)計(jì)系統(tǒng)的模塊結(jié)構(gòu)圖，如圖3所示。

系統(tǒng)主要由3類(lèi)模塊組成：

1）串口通信模塊。該模塊主要通過(guò)調(diào)用串口驅(qū)動(dòng)程序?qū)崿F(xiàn)串口通信，主要包括實(shí)時(shí)采集導(dǎo)航傳感器的數(shù)據(jù)、對(duì)收到的數(shù)據(jù)進(jìn)行校驗(yàn)及解包、獲取組合導(dǎo)航解算所需的數(shù)據(jù)。

2）導(dǎo)航濾波模塊。該模塊主要包括傳感器數(shù)據(jù)處理、組合導(dǎo)航算法等部分。數(shù)據(jù)處理主要是對(duì)采集的傳感器數(shù)據(jù)進(jìn)行預(yù)處理，即野值剔除、平滑濾波等；組合導(dǎo)航的輸入即為數(shù)據(jù)處理后的傳感器信息，通過(guò)聯(lián)邦卡爾曼濾波器輸出AUV當(dāng)前時(shí)刻的速度、位置和姿態(tài)信息。

3）網(wǎng)絡(luò)通信模塊。該模塊主要基于TCP/IP網(wǎng)絡(luò)通信協(xié)議，將導(dǎo)航系統(tǒng)作為客戶機(jī)，水下主控系統(tǒng)作為服務(wù)器，實(shí)現(xiàn)對(duì)服務(wù)器查詢信息實(shí)時(shí)偵聽(tīng)，并向服務(wù)器發(fā)送導(dǎo)航信息的功能。

VxWorks嵌入式實(shí)時(shí)操作系統(tǒng)最顯著的特點(diǎn)就是多任務(wù)，程序在CPU中都是以任務(wù)的方式運(yùn)行［6］，因此任務(wù)的劃分至關(guān)重要。根據(jù)H.Gomma原則［7］，將AUV導(dǎo)航系統(tǒng)的任務(wù)劃分為PHINS數(shù)據(jù)采集、DVL數(shù)據(jù)采集、GPS數(shù)據(jù)采集、深度計(jì)數(shù)據(jù)采集、高度計(jì)數(shù)據(jù)采集、導(dǎo)航濾波處理、網(wǎng)絡(luò)數(shù)據(jù)上傳、錯(cuò)誤管理及數(shù)據(jù)存儲(chǔ)，此外，在調(diào)試階段還包括導(dǎo)航數(shù)據(jù)顯示。

由于AUV導(dǎo)航系統(tǒng)配備有多個(gè)傳感器，且各傳感器數(shù)據(jù)刷新比較快，為保證數(shù)據(jù)采集的實(shí)時(shí)性，串口數(shù)據(jù)采集任務(wù)均是基于中斷共享的。而網(wǎng)絡(luò)數(shù)據(jù)則包括導(dǎo)航數(shù)據(jù)、系統(tǒng)狀態(tài)信息及校驗(yàn)碼信息，通過(guò)TCP協(xié)議上傳至水下主控系統(tǒng)。為了保證導(dǎo)航系統(tǒng)軟件的可靠性，程序內(nèi)通過(guò)錯(cuò)誤管理任務(wù)實(shí)現(xiàn)對(duì)各任務(wù)的檢測(cè)及處理，若檢測(cè)到某任務(wù)異常，則及時(shí)進(jìn)行處理并重新發(fā)起該任務(wù)。導(dǎo)航濾波處理任務(wù)在系統(tǒng)功能中處于核心地位，主要通過(guò)聯(lián)邦卡爾曼濾波器進(jìn)行數(shù)據(jù)濾波。所有任務(wù)基于任務(wù)優(yōu)先級(jí)進(jìn)行，不同優(yōu)先級(jí)任務(wù)間以搶占方式進(jìn)行調(diào)度，不同任務(wù)間主要通過(guò)訪問(wèn)共享數(shù)據(jù)結(jié)構(gòu)進(jìn)行任務(wù)間的通信。

4 組合導(dǎo)航濾波算法

4.1 無(wú)跡卡爾曼濾波理論

當(dāng)系統(tǒng)高度非線性或Jacobian矩陣不可用時(shí)，EKF可能會(huì)導(dǎo)致濾波發(fā)散。針對(duì)EKF的這一缺點(diǎn)，Julier等［2-3］提出了基于UT變換的UKF方法。UT變換通過(guò)在原狀態(tài)分布中以一定的規(guī)則選取一些均值及協(xié)方差等于原狀態(tài)分布的點(diǎn)（Sigma點(diǎn)），將這些點(diǎn)代入非線性函數(shù)中，得到相應(yīng)的非線性函數(shù)值點(diǎn)，并求出其均值和協(xié)方差。通過(guò)UT變換得到的均值和協(xié)方差引入到卡爾曼濾波器中，就構(gòu)成了UKF濾波。由于UKF濾波過(guò)程中并沒(méi)有經(jīng)過(guò)線性化，沒(méi)有忽略其高階項(xiàng)，因此避免了EKF中因近似線性化引起的問(wèn)題［8］。

對(duì)于一個(gè)n維隨機(jī)向量x，已知其均值和方差分別為和P，通過(guò)UT變換，可得到下列2n+1個(gè)Sigma點(diǎn)

這些Sigma點(diǎn)對(duì)應(yīng)的均值及協(xié)方差的權(quán)值分別為：

式（1）和式（2）中：下標(biāo)m表示均值，c表示協(xié)方差；上標(biāo)表示第幾個(gè)采樣點(diǎn)；為矩陣方根的第i列；β為非負(fù)的權(quán)系數(shù)，可以合并方程中高階項(xiàng)的動(dòng)差，當(dāng)x服從高斯分布時(shí)，取β=2最優(yōu)；λ為縮放比例參數(shù)，用來(lái)降低總的預(yù)測(cè)誤差，，其中α為尺度參數(shù)，控制Sigma點(diǎn)的分布狀態(tài)，取值范圍為0≤α≤1，κ為待選參數(shù)，當(dāng)狀態(tài)變量為多變量時(shí)，一般選擇κ=3-n，可以合并方程中高階項(xiàng)的動(dòng)差。對(duì)于離散化后的非線性系統(tǒng)：

式中：Xk，Xk+1分別為系統(tǒng)k和k+1時(shí)刻的狀態(tài)向量；Zk為系統(tǒng)k時(shí)刻的觀測(cè)量；Wk，Vk分別為系統(tǒng)的過(guò)程噪聲向量和觀測(cè)噪聲向量；f[·]，h[·]為非線性函數(shù)。

采用Sigma點(diǎn)的UKF濾波遞推步驟如下［2，9］：

3）計(jì)算系統(tǒng)狀態(tài)量的一步預(yù)測(cè)及協(xié)方差：

式中，Q為系統(tǒng)噪聲協(xié)方差矩陣。

4）根據(jù)狀態(tài)量的一步預(yù)測(cè)值，代入式（3）的UT變換，產(chǎn)生新的Sigma點(diǎn)集

5）計(jì)算系統(tǒng)觀測(cè)值的均值和方差：

式中，R為觀測(cè)噪聲協(xié)方差矩陣。

6）卡爾曼濾波增益更新：

7）系統(tǒng)狀態(tài)更新和協(xié)方差更新。

4.2 聯(lián)邦濾波

對(duì)于水下組合導(dǎo)航系統(tǒng)，一般采用集中式融合和分布式融合2種結(jié)構(gòu)進(jìn)行多傳感器的信息融合。由于集中式濾波方法計(jì)算量大，容錯(cuò)能力低，而分布式濾波方法中，Carlson提出的聯(lián)邦濾波方法計(jì)算量小，信息分配方式靈活，有很強(qiáng)的容錯(cuò)性能［10］，因此聯(lián)邦濾波器在組合導(dǎo)航中應(yīng)用更加廣泛。

圖4為聯(lián)邦濾波器的一般結(jié)構(gòu)。圖中，Xg，Pg為全局狀態(tài)估計(jì)值和協(xié)方差；Xi，Pi為第i個(gè)子濾波器送入主濾波器的局部估計(jì)值和協(xié)方差；Xm，Pm為主濾波器的估計(jì)值和協(xié)方差。聯(lián)邦濾波器由若干個(gè)子濾波器和一個(gè)主濾波器構(gòu)成，其中各子濾波器獨(dú)立地進(jìn)行時(shí)間更新和測(cè)量更新；而主濾波器一方面進(jìn)行時(shí)間更新，另一方面將各子濾波器的結(jié)果進(jìn)行融合，得到全局最優(yōu)估計(jì)。

聯(lián)邦濾波器算法流程為：

1）信息分配過(guò)程：在各子濾波器與主濾波器間分配系統(tǒng)的過(guò)程信息，其中，當(dāng)i=m時(shí)表示主濾波器。

式中，βi＞0為第i個(gè)子濾波器的信息分配系數(shù)，并滿足信息守恒原理：

2）時(shí)間更新：各子濾波器與主濾波器獨(dú)立進(jìn)行時(shí)間更新。

式中，?為系統(tǒng)的一步狀態(tài)轉(zhuǎn)移矩陣。

3）量測(cè)更新：主濾波器無(wú)量測(cè)值，因此沒(méi)有量測(cè)更新，量測(cè)更新僅在各子濾波器中獨(dú)立進(jìn)行。量測(cè)更新如下式所示：

4）信息融合：將各子濾波器的局部估計(jì)信息輸入至主濾波器進(jìn)行數(shù)據(jù)融合計(jì)算，得到全局最優(yōu)估計(jì)。

在局部濾波器中，由于方差上界技術(shù)引起的信息丟失，導(dǎo)致局部濾波變成次優(yōu)，但在主濾波器的融合過(guò)程中，這種非最優(yōu)性被重新融合，使得融合后的全局濾波達(dá)到最優(yōu)。

5 基于UKF的聯(lián)邦濾波器的設(shè)計(jì)

將聯(lián)邦濾波算法應(yīng)用于AUV導(dǎo)航系統(tǒng)中，可以提高系統(tǒng)的容錯(cuò)性和可靠性，同時(shí)將UKF方法應(yīng)用于聯(lián)邦濾波器中，可處理具有非線性特征的組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合問(wèn)題［11］，使導(dǎo)航系統(tǒng)能夠獲得更高的濾波精度，具有更好的魯棒性。趙琳等［12］研究表明，綜合考慮精度容錯(cuò)性和數(shù)據(jù)傳輸，無(wú)反饋聯(lián)邦濾波器比有反饋聯(lián)邦濾波器更具優(yōu)勢(shì)，因此本文采用無(wú)反饋模式的聯(lián)邦濾波器。

僅考慮AUV在近水面航行，模型簡(jiǎn)化為在二維平面上的運(yùn)動(dòng)，導(dǎo)航系統(tǒng)的濾波器結(jié)構(gòu)如圖5所示。

在聯(lián)邦濾波器中，以PHINS捷聯(lián)慣導(dǎo)系統(tǒng)作為參考系統(tǒng)，對(duì)PHINS/DVL子系統(tǒng)采用速度組合，對(duì)PHINS/GPS子系統(tǒng)采用位置組合。

5.1 PHINS/DVL子系統(tǒng)的狀態(tài)方程和觀測(cè)方程

對(duì)于PHINS/DVL子系統(tǒng)，取狀態(tài)變量為：

式中：δL，δλ分別為PHINS的緯度和經(jīng)度誤差；δVe，δVn分別 為PHINS東、北向速度誤差；φ為PHINS航向角誤差；δVde，δVdn分別為DVL測(cè)速誤差。

PHINS捷聯(lián)慣導(dǎo)系統(tǒng)的誤差方程［13］為：

式中：Re為地球WGS-84模型半長(zhǎng)軸，Re=6 378 137 m；f為地球扁率，f=1/298.257；RM為地球子午面半徑，RM=Re(1-2f+3fsin2L)；RN為地球卯酉面半徑，RN=Re(1+fsin2L)；wie為地球自轉(zhuǎn)角速率。

對(duì)于DVL的測(cè)量誤差，僅考慮其速度偏移誤差，其誤差方程為

式中：τd為速度偏移誤差的相關(guān)時(shí)間；wd為零均值高斯白噪聲。

建立PHINS/DVL子系統(tǒng)的狀態(tài)方程：

將PHINS輸出速度與DVL輸出速度之差作為觀測(cè)量，則PHINS/DVL的觀測(cè)方程為

式中，H1，V1(k)為測(cè)量噪聲，近似為高斯白噪聲，且

5.2 PHINS/GPS子系統(tǒng)的狀態(tài)方程和觀測(cè)方程

PHINS/GPS子系統(tǒng)的狀態(tài)向量為：

式中，δLg，δλg分別為GPS的緯度和經(jīng)度誤差。

GPS的定位精度受多種物理因素的影響，但相對(duì)而言，SA誤差才是GPS定位誤差中的主要成分。SA在時(shí)間域內(nèi)的表現(xiàn)形式為變化緩慢、相關(guān)強(qiáng)烈，其動(dòng)力學(xué)特性可采用一階馬爾科夫過(guò)程描述，GPS的東、北向定位誤差［11，14］可表示為

式中：相關(guān)時(shí)間τSAλ，τSAL的取值范圍為100～200 s；wSAλ，wSAL為零均值白噪聲。

PHINS/GPS子系統(tǒng)的非線性狀態(tài)方程為

PHINS/GPS的觀測(cè)方程為

式中，H2，V2(k)為測(cè)量噪聲，同樣視為白噪聲，且

AUV導(dǎo)航系統(tǒng)采用無(wú)反饋的聯(lián)邦濾波器進(jìn)行數(shù)據(jù)濾波，各子濾波器的信息分配系數(shù)分別為：βm=0，β1=β2=0.5。此時(shí)，主濾波器不參與信息分配，無(wú)需進(jìn)行獨(dú)立濾波，2個(gè)子濾波器平均分配信息后進(jìn)行獨(dú)立濾波，然后將各子濾波器的局部估計(jì)值XPHINS（公共狀態(tài)）傳到主濾波器進(jìn)行融合，各子系統(tǒng)無(wú)反饋重置。則系統(tǒng)全局狀態(tài)估計(jì)為：

當(dāng)AUV潛至水下時(shí)，GPS無(wú)法接收衛(wèi)星信號(hào)，PHINS/GPS子系統(tǒng)失效，則取信息分配系數(shù)βm=β2=0.5，β1=1。

6 仿真與試驗(yàn)

6.1 數(shù)值仿真

根據(jù)所設(shè)計(jì)的模型，在Matlab軟件中進(jìn)行數(shù)值仿真，對(duì)基于UKF的聯(lián)邦濾波算法進(jìn)行驗(yàn)證。同時(shí)，對(duì)于組合導(dǎo)航系統(tǒng)的線性化模型，采用傳統(tǒng)的卡爾曼濾波方法進(jìn)行仿真，仿真結(jié)果如圖6～圖8所示。

圖6為實(shí)際的航行軌跡與組合導(dǎo)航濾波軌跡對(duì)比圖。圖6右方為局部放大圖，可以更清楚地對(duì)比各自的軌跡。圖7、圖8分別為聯(lián)邦無(wú)跡卡爾曼濾波器及傳統(tǒng)卡爾曼濾波器下的位置和速度誤差曲線。圖9為這2種濾波器的對(duì)比。從圖中可以看出，將UKF技術(shù)應(yīng)用于無(wú)反饋的聯(lián)邦濾波器中，既可以處理組合導(dǎo)航系統(tǒng)模型的非線性問(wèn)題，避免截?cái)嗾`差，比傳統(tǒng)的卡爾曼濾波方法具備更高的估計(jì)精度，同時(shí)又由于無(wú)反饋聯(lián)邦濾波器良好的容錯(cuò)性能，使得導(dǎo)航系統(tǒng)具備更高的可靠性。

6.2 調(diào)試及試驗(yàn)

1）半實(shí)物仿真聯(lián)調(diào)試驗(yàn)。該項(xiàng)試驗(yàn)主要是在室內(nèi)的半實(shí)物仿真平臺(tái)上進(jìn)行AUV的全系統(tǒng)聯(lián)合調(diào)試。經(jīng)測(cè)試，AUV全系統(tǒng)能穩(wěn)定運(yùn)行，導(dǎo)航系統(tǒng)可實(shí)時(shí)向全艇提供位姿、速度等導(dǎo)航信息。

2）水池試驗(yàn)。試驗(yàn)場(chǎng)地為一條室內(nèi)長(zhǎng)條形水池，由于在室內(nèi)GPS無(wú)法接收到衛(wèi)星信號(hào)，因此僅使用PHINS與DVL進(jìn)行組合導(dǎo)航試驗(yàn)，主要測(cè)試導(dǎo)航系統(tǒng)的測(cè)速精度。試驗(yàn)的測(cè)速數(shù)據(jù)如表2所示。

表2 水池試驗(yàn)速度數(shù)據(jù)Table 2 Velocity data of pool test

試驗(yàn)表明，導(dǎo)航系統(tǒng)在不同航速下的速度誤差都較小，在0.02 m/s以內(nèi)，達(dá)到了預(yù)期的試驗(yàn)?zāi)繕?biāo)。

3）湖上試驗(yàn)。為驗(yàn)證該導(dǎo)航系統(tǒng)應(yīng)用于實(shí)際AUV的可行性，將導(dǎo)航系統(tǒng)固定于游艇上，在湖上進(jìn)行了長(zhǎng)距離航行試驗(yàn)。試驗(yàn)艇為典型的欠驅(qū)動(dòng)系統(tǒng)，其運(yùn)動(dòng)特性類(lèi)似于AUV的近水面航行。試驗(yàn)現(xiàn)場(chǎng)如圖10所示。

在導(dǎo)航系統(tǒng)校準(zhǔn)完成后，進(jìn)行了累計(jì)里程10.6 km的水面航行試驗(yàn)，歷時(shí)2 h，最大速度可達(dá)10 kn。試驗(yàn)的測(cè)量基準(zhǔn)為一臺(tái)定位誤差小于2 m的差分GPS。圖11為導(dǎo)航系統(tǒng)的運(yùn)動(dòng)軌跡曲線，圖12為導(dǎo)航系統(tǒng)與差分GPS的位置偏差隨巡航里程變化的曲線。

根據(jù)試驗(yàn)結(jié)果，導(dǎo)航系統(tǒng)連續(xù)航行累計(jì)10 645.2 m，歷時(shí)2 h。相比GPS，導(dǎo)航位置相對(duì)偏差36.4 m，實(shí)際偏差率0.34%。導(dǎo)航系統(tǒng)的定位精度滿足AUV的試驗(yàn)任務(wù)要求，可應(yīng)用于大尺度欠驅(qū)動(dòng)高速AUV。

4）艇載試驗(yàn)。將導(dǎo)航系統(tǒng)嵌入到AUV內(nèi)部進(jìn)行導(dǎo)航系統(tǒng)的實(shí)艇試驗(yàn)。試驗(yàn)過(guò)程中，AUV在近水面航行，因此艇內(nèi)安裝的單點(diǎn)GPS可作為導(dǎo)航系統(tǒng)的測(cè)量基準(zhǔn)。根據(jù)GPS數(shù)據(jù)及導(dǎo)航系統(tǒng)的數(shù)據(jù)對(duì)比，可繪出經(jīng)、緯度曲線及位置誤差曲線，如圖13～圖15所示。

在AUV近3 h的航行過(guò)程中，導(dǎo)航系統(tǒng)的絕對(duì)位置誤差不超過(guò)8 m，表明導(dǎo)航系統(tǒng)應(yīng)用于AUV中仍具備較高的精度。

7 結(jié) 語(yǔ)

本文介紹了某新型高速AUV基于VxWorks嵌入式操作系統(tǒng)的組合導(dǎo)航系統(tǒng)，并對(duì)導(dǎo)航系統(tǒng)的硬件、軟件構(gòu)架進(jìn)行了詳述。提出了以捷聯(lián)式慣導(dǎo)系統(tǒng)和多普勒測(cè)速儀為主、衛(wèi)星定位系統(tǒng)為輔的組合導(dǎo)航方法，并設(shè)計(jì)了基于UKF的聯(lián)邦卡爾曼濾波器。仿真結(jié)果表明，與傳統(tǒng)的卡爾曼濾波器相比，聯(lián)邦無(wú)跡卡爾曼濾波器具備更好的濾波效果，以及更高的容錯(cuò)性能，提高了導(dǎo)航系統(tǒng)的可靠性。通過(guò)半實(shí)物仿真聯(lián)調(diào)試驗(yàn)、水池試驗(yàn)、湖上試驗(yàn)及艇載試驗(yàn)，充分驗(yàn)證了AUV組合導(dǎo)航系統(tǒng)的實(shí)時(shí)性、可靠性和可行性，表明導(dǎo)航系統(tǒng)具備較高的測(cè)速及定位精度，能夠滿足大尺度欠驅(qū)動(dòng)高速AUV完成各種航行任務(wù)的需要。