基于分層式結構的多AUV協同導航方法及仿真

馮 鵬，熊 凌

（1.武漢科技大學 機器人與智能系統研究院，武漢430081；2.武漢科技大學 信息科學與工程學院，武漢430081）

自主水下航行器AUV 是探索和研究海洋的重要工具。 隨著對海洋領域的研究不斷擴大和深入，單個航行器很難完成復雜的水下任務， 多AUV 協作系統被提出。 這也是近年水下航行器發展的主流趨勢。 多AUV 通過信息共享來進行協同導航，有利于提高系統導航的整體精度和魯棒性，增強了系統的協作能力。

國內外學者對多AUV 協同導航進行了諸多研究。 歐盟資助的“自主水下航行器的合作認知控制”項目[1]中，多AUV 協同控制和導航是研究重點之一，對此做了大量的仿真模擬和硬件測試。文獻[2]研究了EKF 和無跡卡爾曼濾波在組合導航方面的應用并進行了海上試驗，驗證了算法的有效性；文獻[3]將多航行器分為移動航行器和地標航行器兩組，移動航行器通過對靜止地標航行器觀測定位，移動航行器與地標航行器是交替的，每個AUV 可在較小的漂移范圍內導航定位；文獻[4]提出協作SLAM 框架，用于解決多AUV 協同定位通訊受限問題；文獻[5]提出一種基于Kullback-Leibler 距離估計的協同導航算法，仿真驗證了其有效性；文獻[6]將GM-PHD濾波算法和信息熵理論應用于系統定位，該算法對多個位置估計值加權求和以得到最終的狀態估計；文獻[7-8]研究了協同導航過程中通信延遲的問題，提出誤差補償算法以提高定位的精度；文獻[9]總結了近年來多AUV 協同導航的研究進展，包括結構分類、協同定位算法和對未來發展的初步預測。

1 協同導航結構

協同導航結構可分為主從式結構、并行式結構和分層式結構[10]。 在此研究分層式結構。

在多AUV 系統中，每個AUV 的導航設備性能存在差異， 可以將AUV 群中導航精度相當的AUV歸為一層，并按導航精度高低分層。 同層的AUV 可以共享導航信息，導航精度高的在高層，導航精度低的在低層，高精度層AUV 向底精度層AUV 傳遞導航信息。 分層式結構中的相鄰2 層如圖1 所示。AUV1和AUV2在同一層， 為高精度層；AUV3，AUV4和AUV5在同一層，為低精度層。

圖1 多AUV 的分層式結構Fig.1 Hierarchical structure of multiple AUV

在此重點研究分層式結構中高低精度層之間AUV 的協同導航。 低精度層AUV 通過水聲信號測量計算與高精度層AUV 之間的相對距離、 相對方位角， 同時接收高精度層AUV 廣播的自身位置信息，通過擴展卡爾曼濾波EKF 融合內部和外部信息完成協同導航定位。

2 多AUV 協同導航

2.1 AUV 運動模型

AUV 是在水下三維空間中運動的載體，位置坐標為 （x，y，z）。 深度z 能由深度傳感器直接測得，可以單獨考慮。于是，將AUV 的三維運動投影到二維平面，建立AUV 的二維非線性離散數學模型，即

式中：x（k-1），y（k-1）和ψ（k-1）］分別為在k-1時刻的橫坐標、縱坐標和偏航角，此時的運動狀態為X（k）= ［x（k），y（k），ψ（k ）］T；V（k-1）和ω（k-1）為前向速度和偏航角速度，可由多普勒測速儀DVL（doppler velocimeter）和陀螺儀測得。 實際中，傳感器的測量帶有誤差，故

式中：Vm（k-1）和ωm（k-1）分別為AUV 的線速度和偏航角速度測量值；ξV（k-1）和ξω（k-1）為測量噪聲。假設測量噪聲相互獨立， 都為零均值的高斯白噪聲，噪聲協方差為

AUV 的數學模型可簡寫為

2.2 水聲測量模型

AUV 主要通過水聲脈沖信號進行相對距離和方位角的測量。 水聲測距方法有水聲脈沖到達時間TOA（time of arrival）測距法和水聲脈沖接收強度RSS（received signal strength）測距法，通常使用TOA 測距。 TOA 方法中，只要測出水聲脈沖在2 個AUV 之間的傳播時間Δt，根據聲信號在海水中的傳播速度c，就能算出2 個AUV 的相對距離。 AUV 之間時鐘同步，可以通過水聲脈沖單向傳播時間，測得2 個AUV 之間的斜距。

將高精度層AUV 記為AUVi（i=1，2），低精度層AUV 記為AUVj（j=3，4，5）。AUVi和AUVj的斜距為

AUVi和AUVj的坐標分別為（xi（k），yi（k），zi（k）），（xj（k），yj（k），zj（k）），其 中 的zi（k）和zj（k）分 別 為AUVi，AUVj的深度，由深度傳感器測得。 可以將斜距投影到水平面上，根據

可計算得到2 個AUV 在水平方向的距離。 測量的距離與狀態量的關系為

除了能測得相對距離， 還能測得相對方位角。水下航行器的聲吶通常由水聽器陣列組成，水聲脈沖到達不同的水聽器用時不同。

AUVi與AUVj的方位角記為φij。 如圖2 所示，以2 個水聽器為例， 簡要說明相對方位角的測量。AUVj上的2 個水聽器A 和B 接收到AUVi上點P發出水聲脈沖， 根據接收到的時間可以計算PA，PB。 將PA，PB 投影到水平面內得到CA，CB；r 為2個水聽器的距離，r<

測量的方位角與狀態量的關系為

圖2 相對方位角的測量Fig.2 Relative azimuth measurement

距離和方位角的測量都帶有噪聲，將測量方程簡寫為

2.3 算法實現

多AUV 協同導航系統為非線性系統。 EKF 是解決非線性系統常用的方法， 其主要問題是線性化。 在狀態估計時，對系統方程在前一狀態估計值處做實時的線性泰勒近似；在預測步驟中，對測量方程在相應的預測位置也進行線性泰勒近似。

如圖1 所示，高精度層AUV 發出水聲脈沖，隨后發送位置信息。 低精度層AUV 接受到水聲脈沖后計算出相對距離或相對方位角。 低精度層AUV利用測量值和高精度層AUV 的位置信息， 通過EKF 濾波估計自身位置狀態。 高精度層AUVi狀態記為Xi（k）（i=1，2）；低精度層AUVj狀態記為Xj（k）（j=3，4，5）。

AUV 的運動模型為式（4）所示。 AUVj狀態的一步預測（k）為

預測協方差P-（k）為

式中：F（k-1）為f（k-1）關于X（k-1）的雅克比矩陣；L（k-1）為f（k-1）關于u（k-1）的雅克比矩陣。將（k-1），Vm（k-1）代入式（13）、式（14）計算，得

如1.2 節所述的相對距離和相對方位角測量模型， 若低精度層AUVj測量與高精度層AUV1，AUV2的距離，則測量方程（10）中的g（k）為

若低精度層AUVj測量相對AUV1，AUV2的方位角，則g（k）為

對于不同的測量值可以構造不同的觀測方程，測量噪聲的協方差為R（k）。 計算g（k）的雅克比行列式H（k），將（k）代入，得

由式（12）、式（17），計算卡爾曼增益K（k），得

AUVj狀態估計（k）的測量更新為

協方差更新為

式（19）為利用測量更新值得到的位置信息估計。 AUV3，AUV4和AUV5可以根據上述算法估計自身位置信息。 EKF 濾波位置估計誤差為

3 仿真試驗及其結果分析

利用MatLab 進行仿真分析。 多AUV 的真實運動軌跡如圖3 所示， 假設航行時各AUV 深度不發生變化。

圖3 多AUV 運動軌跡Fig.3 Multiple AUV trajectory

AUV1和AUV2在同一深度航行， 均以3 m/s 沿直線勻速航行， 航向角均為70°； 初始位置分別為（300，0，-30），（900，0，-30）。

AUV3，AUV4和AUV5在同一深度航行，均以3 m/s沿直線勻速航行，航向角均為60°；初始位置分別為（0，0，-60），（350，0，-60），（700，0，-60）。 這3 個AUV 的DVL 速度測量噪聲方差為（0.3 m/s）2，陀螺儀測量噪聲方差為（0.2°）2，常值誤差為0.1°，初始位置誤差為（-5，-5），仿真周期為1 s。

仿真1 將低精度層3 個AUV 與高精度層2個AUV 的相對距離作為測量值用于EKF。 距離測量噪聲方差均為（5 m）2。 基于距離的定位誤差如圖4 所示。

圖4 基于相對距離的定位誤差比較Fig.4 Comparison of position errors based on relative distance

圖4 為低精度層3 個AUV 推算誤差和基于距離的協同定位誤差比較。 低精度層AUV 若通過自身導航信息進行推算時，定位誤差會不斷增大。 而通過協同導航定位，可以有抑制誤差的增長。

仿真2 將低精度層3 個AUV 與高精度層2個AUV 的相對方位角作為測量值。 方位角測量噪聲方差均為（0.5°）2。 AUV3，AUV4，AUV5基于方位角的定位誤差如圖5 所示。

圖5 基于相對方位角的定位誤差比較Fig.5 Comparison of position errors based on relative azimuth

圖5 為低精度層AUV 的推算誤差和基于方位角的協同定位誤差比較。計算AUV3，AUV4，AUV5協同定位誤差穩定后的平均誤差見表1。 在仿真條件下，基于距離和基于方位角的協同定位能減小低精度層AUV 的定位誤差，平均誤差在2 m 以內。

表1 協同導航定位平均誤差Tab.1 Cooperative navigation average error

4 結語

研究了分層式結構的多AUV 協同導航， 將多AUV 群進行分層， 充分利用高精度層AUV 的導航信息。AUV 之間導航信息的共享和觀測值的測量需要通過水聲信號進行。 在仿真試驗中選擇相對距離和相對方位角分別作為觀測量進行EKF。 仿真結果表明，該方法能夠提高多AUV 中低精度層AUV 的導航定位精度，有利于多AUV 水下任務的執行。

聲 明

最近社會上有一些機構和個人，冒充《自動化與儀表》編輯部收取文章發表版面費，本刊特在此作如下聲明：《自動化與儀表》雜志的網站為www.zdhyyb.com，投稿郵箱為zdhyyb@vip.sina.com，其他的網站和郵箱地址，均與本編輯部無關，請作者提高警惕，謹防上當！

《自動化與儀表》編輯部