自主水下航行器控制系統的設計及實現

楊 濤

（1.上海交大中海龍水下防務研究中心有限公司，上海 200231；2.上海交大海洋水下工程科學研究院有限公司，上海 200231）

近年來，隨著海洋開發的興起，無人水下航行器越來越多地出現在水下工程應用中。一般無人水下航行器可分為有纜遙控水下航行器（Remote Operated Vehicle，ROV）和自主水下航行器（Autonomous Underwater Vehicle，AUV）兩種。ROV有一根臍帶纜連接到母船，母船為ROV提供電力，發出遙控指令，水下機器人反饋水下視頻、傳感器數據到母船。與ROV相比，一般AUV的體積與質量都較小，同時載荷較小。由于沒有臍帶纜的限制，AUV可以自由航行（在續航里程內），一般具有自主路徑規劃和自主航行能力。

本文結合筆者負責的AUV項目說明AUV的硬件構成、軟件系統以及航位推算算法。該AUV可以進行自主路徑規劃和自主航行，也可以通過一根光纖與岸上控制盒通信，實現手動遙控控制。AUV總長約1.5 m，主體外徑200 mm，最大工作水深100 m，最大航速為4節，續航力不小于10 n mile。

1 AUV總體設計方案

1.1 系統組成

圖1 AUV三維效果圖

艏段艙內放置了水下攝像頭和配重塊。艏段采用透水結構。攝像頭單獨密封，通過水密電纜與水密接插件和電子艙連接。電子艙段是整個水下機器人的核心，包含電池包、主控制板、電機驅動器和各種傳感器等。控制板安裝在框架上，框架作為整體與艙段內部固定。主推進段功能包括AUV系統的推進和操縱控制，采用槳后舵的形式。動密封采用磁耦合形式。

岸上控制盒由充電電池、低功耗嵌入式計算機、光端機以及控制按鈕等組成。它的主要功能包括顯示視頻、深度、航向角以及電池剩余電量等AUV信息，控制AUV的運動狀態，設定AUV的使命數據。

1.2 工作模式

AUV有兩種工作模式，一種是自主作業模式，另一種是光纖遙控模式。自主作業模式是根據岸上控制盒預先設定好的航行計劃（包括航跡、航速、巡航時間以及定深等參數）自主航行。光纖遙控模式是岸上控制盒通過光纖實時控制AUV運動，實時顯示AUV視頻、參數等信息。

2 AUV硬件控制系統

AUV硬件控制設備包括主控PC104、光端機、網絡交換機、WiFi模塊、電機驅動器、電池控制板、全球定位系統（Global Positioning System，GPS）、光纖慣導、多普勒測速儀（Doppler Velocity Log，DVL）、深度傳感器以及漏水傳感器等各種傳感器。岸上控制盒主要包括控制板、顯示屏、光端機、操作按鈕以及電池等。

主控PC104由核心板、串口板和電源板組成。核心板選用盛博科技的SAM-3612B型嵌入式系統模塊。IO模塊選用SysExpanModule/ADT882-AT型擴展板。電源板選用PSI104型電源模塊。圖2為硬件控制系統組成框圖。

圖2 硬件控制系統組成框圖

3 AUV軟件控制系統

3.1 MOOS軟件系統

AUV軟件系統采用MOOS-IvP（Mission Oriented Operating Suite-Interval Programming）軟件系統。MOOS-IvP軟件系統是麻省理工學院開發的，包含一系列庫函數，具有分布式的控制體系結構，用于水下機器人領域的開源綜合性軟件系統。該系統的核心是MOOSDB（MOOS Database），為系統各功能模塊提供統一、高效和穩定的信息通信機制。MOOS-IvP系統的設計理念是把水下機器人各個工作單元抽象成獨立的應用（在平臺上編寫的程序稱為應用，如GPS采集程序）。它的軟件構架是星形框架結構，每個應用與MOOSDB獨立通信，所有的通信都必須經過MOOSDB。應用和MOOSDB之間傳輸數據的格式為字符串（STRING）和浮點型（DOUBLE）兩種[1]。

MOOS系統提供了各種應用程序接口（Application Programming Interface，API）。例如，iDVL負責與多普勒測速儀（DVL）通信，iCompass負責與電子羅盤通信。本項目有關的幾個API介紹如下：

河北省圖書館對培養對象和相關部室主任分別提出了具體的要求：要求相關部室主任加強對學員的規范管理，強化考勤制度，安排學員在本部室的各個崗位學習鍛煉，并親自輔導，同時做好培訓記錄，結束時進行考核，并請學員對培訓進行書面評價；堅決杜絕走過場，形式主義。并根據學員實際學習情況，在征求學員自身意愿基礎之上，不定期調整學員的培訓崗位。已完成培訓的34名基層文化工作人員先后在河北省圖書館采編部、借閱部、特藏部、數字圖書館工作部、宣傳推廣部、參考咨詢部等部室學習工作。

（1）iGPS負責與GPS通信，獲取GPS信息；

（2）iDVL負責與多普勒測速儀（DVL）通信，獲取AUV的3個維度的速度信息；

（3）iINS負責與光纖慣導通信，獲取AUV航向角、姿態及位置信息；

（4）pNav根據MOOS獲得的各個傳感器信息，利用航位推算算法實現自主導航；

（5）iRemote實現光纖遙控模式，通過岸上控制盒手動實時控制AUV的運動；

（6）uMS是圖形用戶界面，通過它用戶可以看到每個API與MOOSDB的通信內容。

3.2 AUV航位推算算法

AUV在水面航行時使用GPS導航。AUV在水下航行時采用慣性導航。AUV也可以間隔一段時間上浮接收GPS信號，修正慣性導航的誤差。

航位推算的一般定義為從一已知點的坐標位置開始，根據航行器在該點的航向、速度和航行時間信息，推算下一時刻的坐標位置的導航過程[2]。

地球是一個具有復雜形狀的球體。它的確切形狀很難用一個簡單的幾何形狀來表示。為了便于計算，可將地球近似看成一個具有微小扁率的旋轉橢球體。以地心為中心，連接南北兩極的機軸為旋轉軸。子午線（或經線）就是通過旋轉軸的平面來截割地球橢球體表面得出的一組截線。這些截線是一些彼此相同的橢圓，長軸在赤道，短軸為極軸的橢圓[3]。這種橢圓又稱子午橢圓。

如果忽略AUV的橫滾角，航位推算可用式（1）～式（4）表示[4]：

式中：VN、VE分別為大地坐標系下AUV在正北方向和正東方向的速度；vx、vy、vz分別為多普勒速度儀測得AUV體坐標系下的前向速度、側向速度、垂向速度；φ和θ分別為AUV的航向角和俯仰角；ρ1和ρ2分別為子午橢圓的曲率和緯線圓的曲率；a為地球的赤道半徑，平均赤道半徑為6 378 137 m±2 m；b為地球的極半徑，平均極半徑為6 356 755 m±5 m；e為地球第一離心率；δ為地球緯度，δ∈（-90°，90°），δ＜0°為南半球，δ＞0°為北半球；ρ1,i和ρ2,i分別為i時刻子午橢圓和緯線圓的曲率；Δt為采樣周期；VE,i和VN,i分別為i時刻AUV在大地坐標系下的正東方向和正北方向的速度；J0和W0分別為AUV在初始時刻位置的經度及緯度；Jn和Wn分別為n時刻AUV所在位置的經度和緯度[5]。

3.3 AUV航位推算線程

AUV航位推算線程的主要函數有航位推算主函數和航位推算子函數。航位推算主函數負責整合航位推算所需的條件和傳感器數據，實時調用航位推算子函數進行航位推算。航位推算主函數流程如圖3所示。

圖3 航位推算主函數流程圖

航位推算子函數負責依據姿態及速度數據實時計算更新AUV的位置信息，其中包括從GPS獲取起始位置（或校正）的經緯度、計算當前的子午橢圓曲率和緯線圓曲率、計算在時鐘脈沖間隔內的經度差和緯度差等。航位推算子函數流程如圖4所示。

圖4 航位推算子函數流程圖

4 AUV湖試

為了全面驗證AUV的航行性能指標，本項目進行了湖試，如圖5所示。湖試分為水面航跡跟蹤測試、定深定向定速測試和水下航跡跟蹤測試3部分。

圖5 AUV湖試

4.1 水面航跡跟蹤測試

該實驗測試AUV水面軌跡跟蹤控制的能力。在水面上選取幾個經緯度點作為期望航跡，根據GPS獲取的經緯度信息作為反饋進行水面航跡跟蹤控制，結果如圖6和圖7所示。

圖6 水面航跡跟蹤控制航向誤差變化曲線

圖7 水面航跡跟蹤控制跟蹤誤差變化曲線

從測試結果分析看，水面上的實際航跡基本符合規劃的航跡。經過計算實驗數據，水面航跡跟蹤誤差的標準差為3.59 m。

4.2 水下路徑跟蹤測試

該實驗測試AUV水下路徑跟蹤控制的能力。設置幾個經緯度點作為水下期望路徑。由于在水下GPS信號衰減非常大，基本不能使用，因此采用慣導、DVL和航位推算的結果作為反饋進行閉環控制，定深5 m。

可以看出，AUV從開始調整至預期的航跡過程中有較大的偏差。當AUV進入設定的軌跡后，航向角、深度及水下航跡都基本符合預期軌跡，深度誤差在0.2 m以內，軌跡跟蹤誤差在0.25 m以內。

5 結語

結合負責的AUV項目，筆者介紹了AUV的各艙段結構、硬件控制系統以及MOOS軟件系統，重點說明了AUV的航位推算算法和最后的湖試結果。湖試結果驗證了這種航位推算算法的可行性，且數據表明這種方法具有良好的控制精度，可以適用于AUV的水下導航。AUV整體性能良好，航行平穩，達到了預期效果。該AUV可以完成水面或者水下規定航跡的巡視，同時可以采集航跡上的深度、溫度以及視頻等信息。