大尺度欠驅(qū)動高速AUV導(dǎo)航系統(tǒng)研制

翟云峰，王冠學(xué)，徐國華，陳柱，張鑫

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

0 引 言

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

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

本文將基于某新型大尺度欠驅(qū)動高速AUV，設(shè)計一種基于VxWorks嵌入式操作系統(tǒng)的水下機器人導(dǎo)航系統(tǒng)，同時考慮到導(dǎo)航系統(tǒng)的容錯性和設(shè)計靈活性，以及導(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)部，在頂層通過水面監(jiān)控系統(tǒng)進行全艇的狀態(tài)監(jiān)控及任務(wù)信息下達，從而實現(xiàn)AUV的自主作業(yè)。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.1 無跡卡爾曼濾波理論

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

對于一個n維隨機向量x，已知其均值和方差分別為和P，通過UT變換，可得到下列2n+1個Sigma點

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

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

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

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

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

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

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

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

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

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

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

4.2 聯(lián)邦濾波

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

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

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

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

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

2）時間更新：各子濾波器與主濾波器獨立進行時間更新。

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

3）量測更新：主濾波器無量測值，因此沒有量測更新，量測更新僅在各子濾波器中獨立進行。量測更新如下式所示：

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

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

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

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

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

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

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

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

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

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

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

對于DVL的測量誤差，僅考慮其速度偏移誤差，其誤差方程為

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

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

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

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

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

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

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

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

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

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

PHINS/GPS的觀測方程為

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

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

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

6 仿真與試驗

6.1 數(shù)值仿真

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

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

6.2 調(diào)試及試驗

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

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

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

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

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

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

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

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

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

7 結(jié) 語

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