大尺度欠驅動高速AUV導航系統研制

翟云峰，王冠學，徐國華，陳柱，張鑫

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

0 引 言

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

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

本文將基于某新型大尺度欠驅動高速AUV，設計一種基于VxWorks嵌入式操作系統的水下機器人導航系統，同時考慮到導航系統的容錯性和設計靈活性，以及導航系統模型的非線性特性，選擇基于UKF的聯邦卡爾曼濾波器作為組合導航系統的數據融合算法。

1 AUV系統組成

大尺度高速AUV的系統由水面監控系統、通信傳輸系統和艇載控制系統組成，其組成圖如圖1所示。艇載控制系統均已嵌入到AUV內部，在頂層通過水面監控系統進行全艇的狀態監控及任務信息下達，從而實現AUV的自主作業。

為了方便系統控制，導航系統要設計成黑盒，僅僅實現測量輸入和信息輸出。在高航速下，對AUV導航系統的實時性、可靠性和精度提出了更高的要求。

2 導航系統硬件構架

AUV導航系統主要配備了捷聯慣導系統（PHINS）、多普勒測速儀（DVL）、衛星定位系統（GPS）、深度計和高度計，其主要參數如表1所示。導航系統采用嵌入式結構設計，在VxWorks6.6實時操作系統下實現AUV的導航定位。VxWorks可以為導航任務提供高效、實時的任務調度和中斷管理，并提供實時的系統資源以及任務通信，從而避免導航軟件開發過程中對系統資源的管理，提高系統的實時性和可靠性。

表1 傳感器參數Table 1 Senor parameters

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

導航系統采用開放式硬件構架［5］，可以通過增加導航信息源來提高導航系統的精度。

3 導航系統軟件構架

AUV導航系統的軟件采用模塊化設計，以降低程序復雜度，使程序結構清晰，便于閱讀和理解，方便軟件的設計與后續的完善和修改。

導航工控機通過串口實現PHINS，DVL和GPS等傳感器數據的采集，然后通過組合導航算法解算出所需的導航數據，最后通過以太網形式與上層進行交互，即定時接收到水下主控系統的詢問報文后，導航系統立即將最新解算的導航數據發送給水上主控系統。根據組合導航系統的功能，可設計系統的模塊結構圖，如圖3所示。

系統主要由3類模塊組成：

1）串口通信模塊。該模塊主要通過調用串口驅動程序實現串口通信，主要包括實時采集導航傳感器的數據、對收到的數據進行校驗及解包、獲取組合導航解算所需的數據。

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

3）網絡通信模塊。該模塊主要基于TCP/IP網絡通信協議，將導航系統作為客戶機，水下主控系統作為服務器，實現對服務器查詢信息實時偵聽，并向服務器發送導航信息的功能。

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

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

4 組合導航濾波算法

4.1 無跡卡爾曼濾波理論

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

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

這些Sigma點對應的均值及協方差的權值分別為：

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

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

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

3）計算系統狀態量的一步預測及協方差：

式中，Q為系統噪聲協方差矩陣。

4）根據狀態量的一步預測值，代入式（3）的UT變換，產生新的Sigma點集

5）計算系統觀測值的均值和方差：

式中，R為觀測噪聲協方差矩陣。

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

7）系統狀態更新和協方差更新。

4.2 聯邦濾波

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

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

聯邦濾波器算法流程為：

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

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

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

式中，?為系統的一步狀態轉移矩陣。

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

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

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

5 基于UKF的聯邦濾波器的設計

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

僅考慮AUV在近水面航行，模型簡化為在二維平面上的運動，導航系統的濾波器結構如圖5所示。

在聯邦濾波器中，以PHINS捷聯慣導系統作為參考系統，對PHINS/DVL子系統采用速度組合，對PHINS/GPS子系統采用位置組合。

5.1 PHINS/DVL子系統的狀態方程和觀測方程

對于PHINS/DVL子系統，取狀態變量為：

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

PHINS捷聯慣導系統的誤差方程［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為地球自轉角速率。

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

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

建立PHINS/DVL子系統的狀態方程：

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

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

5.2 PHINS/GPS子系統的狀態方程和觀測方程

PHINS/GPS子系統的狀態向量為：

式中，δLg，δλg分別為GPS的緯度和經度誤差。

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

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

PHINS/GPS子系統的非線性狀態方程為

PHINS/GPS的觀測方程為

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

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

當AUV潛至水下時，GPS無法接收衛星信號，PHINS/GPS子系統失效，則取信息分配系數βm=β2=0.5，β1=1。

6 仿真與試驗

6.1 數值仿真

根據所設計的模型，在Matlab軟件中進行數值仿真，對基于UKF的聯邦濾波算法進行驗證。同時，對于組合導航系統的線性化模型，采用傳統的卡爾曼濾波方法進行仿真，仿真結果如圖6～圖8所示。

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

6.2 調試及試驗

1）半實物仿真聯調試驗。該項試驗主要是在室內的半實物仿真平臺上進行AUV的全系統聯合調試。經測試，AUV全系統能穩定運行，導航系統可實時向全艇提供位姿、速度等導航信息。

2）水池試驗。試驗場地為一條室內長條形水池，由于在室內GPS無法接收到衛星信號，因此僅使用PHINS與DVL進行組合導航試驗，主要測試導航系統的測速精度。試驗的測速數據如表2所示。

表2 水池試驗速度數據Table 2 Velocity data of pool test

試驗表明，導航系統在不同航速下的速度誤差都較小，在0.02 m/s以內，達到了預期的試驗目標。

3）湖上試驗。為驗證該導航系統應用于實際AUV的可行性，將導航系統固定于游艇上，在湖上進行了長距離航行試驗。試驗艇為典型的欠驅動系統，其運動特性類似于AUV的近水面航行。試驗現場如圖10所示。

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

根據試驗結果，導航系統連續航行累計10 645.2 m，歷時2 h。相比GPS，導航位置相對偏差36.4 m，實際偏差率0.34%。導航系統的定位精度滿足AUV的試驗任務要求，可應用于大尺度欠驅動高速AUV。

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

在AUV近3 h的航行過程中，導航系統的絕對位置誤差不超過8 m，表明導航系統應用于AUV中仍具備較高的精度。

7 結 語

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