自主式水下航行器水下回收融合引導技術方案及算法

趙蕊，許建

中國艦船研究設計中心， 湖北 武漢 430064

0 引 言

目前，海洋資源開發日益受到重視，作為勘探和開發的先進裝備[1]——無纜自主式水下航行器（autonomous underwater vehicle, AUV ）能夠完成海洋環境監測、海底地貌地形測量、水下資源勘探和取樣等多種任務[2-4]。隨著AUV技術的不斷發展及其研究的不斷深入，AUV的自主與機動能力有了很大提高，已廣泛應用于海底探測和水下作業[5-7]。AUV在水下作業的時間也受其所攜帶的能源限制，即完成某項任務后需回收進行能源補充、數據交換、新任務下載等[8-10]。因此，在執行水下觀測等這種耗時任務時，對于提高工作效率、節省人力和成本而言，AUV水下自動對接系統有著重要意義[11-12]。然而，水下環境復雜而特殊，AUV水下對接定位仍面臨極大的挑戰，從而影響AUV的定位精度，故自主回收對接技術成為了國內外業界一個研究的熱點和難點[13-15]。

在水下平臺的支持與布放回收技術領域，我國相關研究起步較晚[16]，與國外的差距明顯，而這也是水下平臺搭載作業中最核心的技術。鑒此，為克服單一傳感器的可靠性和辨識準確性低[17-19]的問題，本文將面向移動平臺（或移動目標）的AUV回收對接作業，提出一種基于慣性導航（INS）、聲學超短基線定位（USBL）、光學等信號的多信息融合的水下引導系統方案，并結合回收對接過程中的5個階段建立運動方程，設計基于擴展聯邦卡爾曼濾波器（federated extended Kalman filter）及融合INS和USBL及光學信息的導航系統，得到一種基于上述3種信息的多源數據融合導航算法。最后，設計幾條典型對接軌跡，通過仿真驗證AUV回收對接引導過程中的各階段所用方法的可行性。

1 多源數據融合引導技術方案及實現框架

1.1 技術方案

本文提出面向移動目標的AUV水下回收對接引導過程（也稱定位導航）采用INS和USBL及光學引導相結合的方式[20-24]。其中，在中遠程階段，采用聲吶輔助定位[25]，即通過安裝在航行器底部的聲吶定位設備（例如USBL）與布置在移動目標背部的聲應答器之間的聲傳輸來完成；光學引導過程則由前、后和末期這3個階段組成，分別對應于近程、垂向末端及平移鎖緊階段，此引導過程是通過AUV底部安裝的水下攝像機對移動目標背部布置的引導用LED燈陣和燈帶進行感知來獲取相應的導航信息。圖1所示為AUV水下回收對接引導的全過程。下文將詳述各階段操作的過程。

圖1 AUV回收作業對接引導的全過程Fig.1 Whole process of guiding AUV docking in recovery operation

階段1：AUV完成作業后，通過自主導航返回到與移動目標處于中遠距離的A處（約1～2 km），經與移動目標協商并確定回收后，再航行至移動目標上方附近的B處（位于移動目標20 m外）。在此階段，AUV使用自身攜帶的聲吶定位系統定期測定移動目標的相對位置以輔助AUV導航。

階段2：AUV到達移動目標附近的B處后，移動目標背部布置的LED燈陣開啟，燈陣發出的光束方向朝向AUV（在燈陣光束方向可調的情況下），使AUV進入光學引導范圍內。屆時，AUV使用自身攜帶的攝像機捕獲移動目標上的燈陣，引導AUV向移動平臺正上方移動，邊緩慢下降邊靠近移動目標。此時，燈陣光束方向也同步緩慢移向移動目標上方，AUV開始追蹤燈陣并運動至移動目標正上方的C處（約距離移動目標10～20 m）。在此階段，AUV離移動目標較遠，攝像機拍攝到的燈陣光暈相互重疊，會形成單光源，故AUV采用融合追蹤光源的方式來導航靠近移動目標，并到達移動目標的正上方。

階段3：AUV到達移動目標的正上方后，AUV控制自身速度、姿態，并與移動目標保持基本一致。AUV基本保持與移動目標以相同方向和速度航行，逐漸靠近移動目標并到達D處（距離移動目標約10 m）。此時，移動目標正上方的AUV離目標仍較遠，AUV上的攝像機拍攝到的燈陣光暈仍相互重疊，會形成單光源，在此階段，AUV繼續采用融合追蹤光源的導航方式。

階段4：隨著AUV靠近移動目標，AUV上的攝像機逐漸能夠完全分辨出每個LED光源的輪廓，此時，AUV使用相應的光學引導算法，得到AUV與移動目標間精準的相對位置及相對姿態，從而進行聲、光融合導航。經過穩定估計出高精度的AUV相對位置及相對姿態，直至航行到移動目標正上方的E處（距離移動目標約1 m），AUV調整好姿態使其與移動目標保持一致。在此階段，AUV采用融合了INS和USBL及光學引導的導航方式。

階段5：AUV航行至移動目標正上方的E處后，此時，移動目標的LED燈陣關閉，LED燈帶開啟。AUV調整好姿態繼續緩慢向前相對平移，并根據自身攜帶的攝像機拍攝出視角內LED燈帶的圖像，解算出AUV相對于移動目標的橫向及縱向角度偏差。在此階段，AUV采用融合了INS和高度計及光學引導的導航方式。

最后，只需將移動目標對接口中心軸航向距離調整至限位機構的誤差范圍內的F處，沿搖桿下滑，進一步調整艏向偏差，與AUV完成對接鎖緊即可。從上述AUV水下回收對接的全過程來看，INS和USBL及光學引導系統的多源數據融合導航在不同階段各子系統參與融合的數據有所不同。

1.2 實現框架

圖2所示為實現水下移動目標與AUV之間回收對接的引導系統的總體框架，圖中標示了各部分組件及其相互關系。

圖2 AUV與移動目標實現對接的系統總體框架Fig.2 General framework of implementation for AUV docking system with mobile platform

由圖2可見，水下引導系統主要由處理器、水下攝相機、USBL和INS以及布置在移動目標背部的LED燈陣與USBL聲應答器等設備組成。其中，布置在移動目標背部的USBL聲應答器用于響應安裝在AUV中的USBL發送的詢問信號，AUV中的USBL需定位時會發出聲脈沖信號，移動目標上的USBL聲應答器收到并發送應答信號，經水下傳輸后回到AUV，再經USBL中的水聽器陣列接收應答信號，解算出相對位置。

如圖2（a）所示，移動目標與AUV水聲通信時需獲取雙方的航態信息。一方面，移動目標將自身航態信息（例如位置、姿態、速度等）發送給AUV的光學引導統；另一方面，移動目標需控制LED燈陣的光束進行偏轉，控制器則需獲取AUV的航態信息用于控制光束發出的方向，以實時追蹤位置。此外，設置的LED燈陣還需有合理的間距和排列方式。

如圖2（b）所示，AUV水下引導子系統的處理器是整個系統的核心，其功能主要是實時處理攝像機采集的視頻流，以及運行光學引導相關算法，獲取定位信息，再結合USBL和INS系統的定位信息進行融合引導。由于引導系統需要的是相對位置，因此處理器還需要接收移動目標的航態信息，經融合導航處理后，輸出整個系統的最終定位信息，再經通信接口（RS232）與AUV進行信息傳輸，將AUV的航態信息發送至移動目標的控制器。屆時，所得定位信息將會作為AUV控制系統的輸入，引導下一時刻的運動。

2 多源數據融合導航算法研究

2.1 聯邦卡爾曼濾波器原理

如上所述，本文提出的AUV水下回收對接引導系統融合了INS和USBL及光學定位這3種導航系統，它們的傳感器觀測得到的信息數據量測值均不相同，精度各一。因此，為獲取高精度的導航定位信息，還需要將各傳感器觀測得到的量測值進行組合濾波[26-27]。對于組合導航系統而言，相比采用一個卡爾曼濾波器來集中處理所有導航傳感器的信息，聯邦卡爾曼濾波器（federated Kalman filter,FKF）可克服前者的狀態維數高、計算量大、容錯性低等缺點[28]。因此，本文采用了FKF作為狀態估計的方法。FKF是一種典型的信息融合技術[29]，其由多個子濾波器和一個主濾波器組成，采取兩個階段分散濾波，最后，通過數據融合方法提高濾波精度。

INS和USBL及光學定位這3種傳感器各有優缺點。例如，INS輸出的姿態、速度及位置等三維信息具有更新速率高、短期精度和穩定性好的優點，但量測信息輸入到子濾波器后誤差會隨時間而積累；USBL可輸出精度較高的相對位置信息，但更新速率慢；光學定位系統可輸出高精度的相對位置和相對姿態信息，更新速率快，但作用距離短。

本文提出的采用基于FKF的多源數據融合導航方案中，若USBL量測值與光學量測值在同一時刻到來時，首先處理的是USBL量測值，然后是2個子濾波器分別進行擴展卡爾曼濾波，再將濾波結果傳輸給主濾波器進行數據融合，最后得到最優估計值。圖3所示為多速率聯邦擴展卡爾曼濾波器總體框架。圖中，z1，z2分別為INS和USBL量測信息，,,分別為光學前、后期及末期量測值。

圖3 多速率聯邦擴展卡爾曼濾波器總體框架Fig.3 General framework of federate extended Kalman filter with multi-rates

聯邦擴展卡爾曼濾波器的數據融合原理是：AUV多源數據融合引導系統中的全部數據在幾個子濾波器間進行初始分配，將分配的數據與局部濾波器融合，完成局部更新，重新組合成新的系統總信息。在此系統中，一般將INS作為公共參考系統，參與2個子濾波器的數據分配。根據信息守恒原理，分配系數滿足：

式中：γi為 第i個 子濾波器的信息分配系數；γm為主濾波器系數。

2.2 聯邦濾波器算法

設AUV的運動狀態方程為

式中：xk,xk?1分別為AUV在k和k-1時刻的狀態向量；Fk?1為AUV在k?1時 刻 的 狀 態 轉 移 矩 陣；ωk?1為AUV過程噪聲矩陣，也為零均值的高斯分布，其噪聲方差矩陣為Qk?1。

在卡爾曼濾波過程中，所有子濾波器與主濾波器共用一個狀態方程。不同的觀測系統對AUV的狀態信息進行獨立測量，則第i個（i=1,2,···,m）子濾波器的量測方程為

式中：zi,k為第i個 觀測系統在k時刻對AUV觀測得到的觀測向量；Hi,k為zi,k對應的觀測矩陣；xi,k為第i個濾波器采集到的AUV的狀態向量；vi,k為觀測的噪聲矩陣。

假設AUV各子系統本身的過程噪聲與觀測噪聲不相關，且子系統間的噪聲也互不相關，則滿足卡爾曼濾波的基本要求。

聯邦濾波算法可分為如下幾個步驟：

步驟1：信息分配。根據方差上界技術，對子濾波器與主濾波器設置的系統初始值包括誤差協方差Pi,0、 過程噪聲Qi,0、估計信息值以及全局最優濾波初始值Pg,0，Qg,0和，并分別表示如下：

步驟2：狀態更新。將主濾波器和子濾波器分別獨立進行狀態更新。

步驟3：量測更新。利用子系統量測值對其相應的子濾波器進行量測更新。其中，子系統量測值表示為Ki,k，觀測噪聲方差表示為Ri,k，I為初始噪聲矩陣。

因主濾波器無量測信息，故僅對主濾波器的狀態向量信息值Pm,k和進行更新：

步驟4：信息融合。將所有子濾波器與主濾波器的估計信息進行融合，計算全局狀態估計信息。

步驟5：信息重置。將 全局誤差協方差Pg,k、公共過程噪聲Qg,k以 及全局估計狀態向量x?g,k按照信息分配原則重置各子濾波器和主濾波器，如下式所示。

由上述步驟可見，各子濾波器和主濾波器可以同時更新狀態，并進行信息融合。子濾波器的狀態估計值雖不是最優的，但可以證明全局濾波估計值是最優的。圖4所示為整個聯邦擴展卡爾曼濾波處理流程。

圖4 多速率聯邦擴展卡爾曼濾波數據處理流程Fig.4 Data processing flow of federate extended Kalman filter with multi-rates

2.3 AUV運動狀態方程

為方便建立AUV的運動狀態方程，對坐標系進行了定義，如圖5所示。建立的兩種坐標系中中，導航坐標系（n系）原點O一般選在海面上的某一點，xn軸 指向正北，yn軸指向正東，zn軸垂直于地面指向地心；載體坐標系（b系）建立在AUV上，坐標系原點一般選在AUV的重心上，xb軸為指向艏向的AUV縱軸，yb軸為指向右舷的AUV橫軸，zb軸垂直于AUV向下。AUV的姿態用歐拉角表示，繞xb軸 旋轉的為橫滾角φ ， 繞yb軸旋轉的是俯仰角θ ，繞zb軸 旋轉的航向角ψ。

圖5 導航坐標系和載體坐標系定義Fig.5 Definitions of navigation and body coordinate system

如第1節所述，AUV對接過程中的多源數據融合引導系統在不同階段所獲量測信息不同，且引導的全過程被分為5個階段和3種導航方式。當AUV距移動目標較遠時，僅能獲取INS和USBL信息，此時采用聲學輔助導航方式。假定回收開始時，AUV經與移動目標協商獲取移動平臺目標j的初始位置（Pj,0=[xj,0yj,0zj,0]）、姿態（Θj=[φjθjψj]） 與速度（vj=[x˙jy˙jz˙j]），然后，移動目標保持定向勻速航行。此時，以東、北、天這3種導航坐標系作為參考系，選取AUV與移動目標 間 的 相 對 位 置（ ΔxkΔykΔzk）、AUV的 姿 態（ φkθkψk）、速度（x˙ky˙kz˙k）作為狀態向量（共9維），也即

式中： ωk?1為 回收初始狀態矩陣；Rnb(Θk?1)為k-1時刻AUV的載體坐標系向導航坐標系轉換的旋轉矩陣，并定義為

1）當AUV處于回收引導階段1時，INS系統可以輸出AUV的三維位置、姿態和速度，結合移動目標的初始的三維位置、姿態和速度，即可以獲得相對位置的量測值。觀測向量z1,k與狀態向量xk的形 式 一 致，觀 測矩陣 為H1,k=I9×9。對于USBL系統，其僅能獲取相對位置的信息，且觀測矩陣為H2,k=[I3×303×6]。此時，利用USBL測量信息來糾正INS系統發散的位置誤差，而在獲取USBL信息時，可計算USBL與INS兩個傳感器獲取的位置差值，用于補償INS系統的量測值。

2）當AUV處于回收引導階段2和階段3時，光學引導系統開始進入前期工作。此時，AUV距離移動目標較遠，光學引導系統僅能夠獲得AUV相對于移動目標的橫向角偏差α和縱向角偏差β ，取z′2,k=[αkβk]，并分別建立如下光學前量測方程z′2,k和 量測矩陣H2′,k。其中，h(xk)為AUV相對移動目標在k時刻時的位置函數，v2,k為相對于移動目標在k時刻時的速度函數。

4）當AUV處于回收引導階段4時，光學引導系統開始進入后期工作。此時，光學引導系統能獲得AUV相對于移動目標的相對位置xk和相對姿態信息（ Δφk，Δθk，Δψk），此時，可以擴充狀態向量，在加入相對姿態信息后，AUV的狀態向量（共12維）更新為x′k=[xkΔφkΔθkΔψk]，并分別建立如下光學后量測方程z′2′,k和 量測矩陣H2′′,k。

5）當AUV處于回收引導階段5時，采取追蹤燈帶的方式對接。此時，光學引導系統可以量測AUV相對于移動目標的橫向角偏差α與縱向角偏差β，測量方式與光學引導的前期工作相似，并分別建立如下觀測向量z′2′,′k和 量 測 矩 陣H2′′,′k。

式中：n為 燈的編號，n=1,2,3,4；d為燈間距量測矩陣。

3 仿真分析

3.1 多源數據融合引導軌跡設定

為對多種航行軌跡下的數據融合導航性能進行對比，設計了3種典型的對接航行軌跡，如圖6所示。本文仿真涵蓋了整個AUV回收對接引導的全過程，其中，假設回收對接的初始階段AUV在水下約20 m處航行，而移動目標則在水下約40 m處航行，AUV分別從3個方向朝向移動目標航行，起始航向角偏差分別為0°，180°和90°。

圖6 AUV回收對接的3種航行軌跡Fig.6 Three trajectories of AUV docking in recovery operation

3.2 融合引導仿真

根據2.1節關于聯邦卡爾曼濾波的原理，仿真前需設定參數，包括濾波器需要的初始值、噪聲參數以及需要對比的真實值和量測值。例如，量測系統數據更新頻率（INS解算模擬模塊和光學引導模擬模塊為10 Hz，USBL測量模擬模塊為0.2 Hz）、仿真時間（906 s）、采樣間隔（0.1 s）。其他參數具體設定如下：

1）初始的狀態向量和誤差協方差。

以上式中：x0，分別為前3個階段使用的狀態向量和第4，5階段（光學引導后期和末期）使用的擴展后的狀態向量；P0，P為對應的誤差協方差；Q0，Q為相應的過程噪聲方差初始值；R1，R2，R，R′2′，R分別為INS和USBL及光學引導的前期、后期和末期的觀測噪聲方差初始值。

此外，USBL和光學系統引導的量測值精度與AUV和移動目標的相對距離有關，其標準差設置為相對距離的1%。而USBL的數據更新率FUSBL一般與聲波往返時間tRTT以及處理器處理時間tp有關，即

假定聲學信號處理時間tp為500 ms，聲速取1 500 m/s，近距離20 m的數據更新率約1.9 Hz，而遠距離1.5 km的數據更新率約0.4 Hz，加上數據傳輸等因素造成的延遲，實際的數據更新率還會小于上述值。為便于仿真，本文數據更新率統一取為0.2 Hz，融合導航模塊運行1次，獲得如圖7所示的相對位置軌跡。圖中：綠線表示真實值的軌跡；黑線表示INS解算結果；藍線表示INS加入USBL的估計值后，采用聯邦卡爾曼濾波方法有效修正的INS積累誤差；紅線表示加入光學信號引導并融合后的相對位置。

通過觀察圖7中的軌跡可以發現，僅使用INS解算的軌跡逐漸發散，這是因為其測量器件存在常值漂移和隨機誤差。藍線所示的INS加入了USBL估計值并采用聯邦卡爾曼濾波方法有效修正的INS積累誤差精度不夠，且USBL數據更新率低。而紅線所示加入了光學信號引導并融合后的相對位置得到的定位精度更高、穩定性更好，結果滿足對接要求。

圖7 AUV回收對接在不同軌跡下濾波估計的相對位置與真實值對比Fig.7 Comparison of filter estimated relative position and true value for different trajectories of AUV docking in recovery operation

4 結 語

本文以移動平臺搭載的AUV回收對接引導過程為研究對象，通過分析其水下回收的重難點，融合獲得來自INS和USBL及光學系統的多源數據，提出了一種面向移動平臺的AUV水下回收對接引導系統的方案。研究中，將AUV水下回收對接過程分為5個階段，根據不同階段使用不同的導航方式，制定了基于擴展聯邦濾波器的水下回收對接多源數據融合引導系統，得到了融合相應的導航算法。該系統結合了不同傳感器系統的優點，有效克服了集中式濾波存在的狀態維度高、計算量大的缺點，提高了AUV水下回收對接引導過程中的定位精度。仿真結果表明，所提方法可以滿足工程要求。