自主水下航行器控制系統(tǒng)的設(shè)計(jì)及實(shí)現(xiàn)

楊 濤

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

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

本文結(jié)合筆者負(fù)責(zé)的AUV項(xiàng)目說明AUV的硬件構(gòu)成、軟件系統(tǒng)以及航位推算算法。該AUV可以進(jìn)行自主路徑規(guī)劃和自主航行，也可以通過一根光纖與岸上控制盒通信，實(shí)現(xiàn)手動(dòng)遙控控制。AUV總長約1.5 m，主體外徑200 mm，最大工作水深100 m，最大航速為4節(jié)，續(xù)航力不小于10 n mile。

1 AUV總體設(shè)計(jì)方案

1.1 系統(tǒng)組成

圖1 AUV三維效果圖

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

岸上控制盒由充電電池、低功耗嵌入式計(jì)算機(jī)、光端機(jī)以及控制按鈕等組成。它的主要功能包括顯示視頻、深度、航向角以及電池剩余電量等AUV信息，控制AUV的運(yùn)動(dòng)狀態(tài)，設(shè)定AUV的使命數(shù)據(jù)。

1.2 工作模式

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

2 AUV硬件控制系統(tǒng)

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

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

圖2 硬件控制系統(tǒng)組成框圖

3 AUV軟件控制系統(tǒng)

3.1 MOOS軟件系統(tǒng)

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

MOOS系統(tǒng)提供了各種應(yīng)用程序接口（Application Programming Interface，API）。例如，iDVL負(fù)責(zé)與多普勒測速儀（DVL）通信，iCompass負(fù)責(zé)與電子羅盤通信。本項(xiàng)目有關(guān)的幾個(gè)API介紹如下：

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

（1）iGPS負(fù)責(zé)與GPS通信，獲取GPS信息；

（2）iDVL負(fù)責(zé)與多普勒測速儀（DVL）通信，獲取AUV的3個(gè)維度的速度信息；

（3）iINS負(fù)責(zé)與光纖慣導(dǎo)通信，獲取AUV航向角、姿態(tài)及位置信息；

（4）pNav根據(jù)MOOS獲得的各個(gè)傳感器信息，利用航位推算算法實(shí)現(xiàn)自主導(dǎo)航；

（5）iRemote實(shí)現(xiàn)光纖遙控模式，通過岸上控制盒手動(dòng)實(shí)時(shí)控制AUV的運(yùn)動(dòng)；

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

3.2 AUV航位推算算法

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

航位推算的一般定義為從一已知點(diǎn)的坐標(biāo)位置開始，根據(jù)航行器在該點(diǎn)的航向、速度和航行時(shí)間信息，推算下一時(shí)刻的坐標(biāo)位置的導(dǎo)航過程[2]。

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

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

式中：VN、VE分別為大地坐標(biāo)系下AUV在正北方向和正東方向的速度；vx、vy、vz分別為多普勒速度儀測得AUV體坐標(biāo)系下的前向速度、側(cè)向速度、垂向速度；φ和θ分別為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時(shí)刻子午橢圓和緯線圓的曲率；Δt為采樣周期；VE,i和VN,i分別為i時(shí)刻AUV在大地坐標(biāo)系下的正東方向和正北方向的速度；J0和W0分別為AUV在初始時(shí)刻位置的經(jīng)度及緯度；Jn和Wn分別為n時(shí)刻AUV所在位置的經(jīng)度和緯度[5]。

3.3 AUV航位推算線程

AUV航位推算線程的主要函數(shù)有航位推算主函數(shù)和航位推算子函數(shù)。航位推算主函數(shù)負(fù)責(zé)整合航位推算所需的條件和傳感器數(shù)據(jù)，實(shí)時(shí)調(diào)用航位推算子函數(shù)進(jìn)行航位推算。航位推算主函數(shù)流程如圖3所示。

圖3 航位推算主函數(shù)流程圖

航位推算子函數(shù)負(fù)責(zé)依據(jù)姿態(tài)及速度數(shù)據(jù)實(shí)時(shí)計(jì)算更新AUV的位置信息，其中包括從GPS獲取起始位置（或校正）的經(jīng)緯度、計(jì)算當(dāng)前的子午橢圓曲率和緯線圓曲率、計(jì)算在時(shí)鐘脈沖間隔內(nèi)的經(jīng)度差和緯度差等。航位推算子函數(shù)流程如圖4所示。

圖4 航位推算子函數(shù)流程圖

4 AUV湖試

為了全面驗(yàn)證AUV的航行性能指標(biāo)，本項(xiàng)目進(jìn)行了湖試，如圖5所示。湖試分為水面航跡跟蹤測試、定深定向定速測試和水下航跡跟蹤測試3部分。

圖5 AUV湖試

4.1 水面航跡跟蹤測試

該實(shí)驗(yàn)測試AUV水面軌跡跟蹤控制的能力。在水面上選取幾個(gè)經(jīng)緯度點(diǎn)作為期望航跡，根據(jù)GPS獲取的經(jīng)緯度信息作為反饋進(jìn)行水面航跡跟蹤控制，結(jié)果如圖6和圖7所示。

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

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

從測試結(jié)果分析看，水面上的實(shí)際航跡基本符合規(guī)劃的航跡。經(jīng)過計(jì)算實(shí)驗(yàn)數(shù)據(jù)，水面航跡跟蹤誤差的標(biāo)準(zhǔn)差為3.59 m。

4.2 水下路徑跟蹤測試

該實(shí)驗(yàn)測試AUV水下路徑跟蹤控制的能力。設(shè)置幾個(gè)經(jīng)緯度點(diǎn)作為水下期望路徑。由于在水下GPS信號衰減非常大，基本不能使用，因此采用慣導(dǎo)、DVL和航位推算的結(jié)果作為反饋進(jìn)行閉環(huán)控制，定深5 m。

可以看出，AUV從開始調(diào)整至預(yù)期的航跡過程中有較大的偏差。當(dāng)AUV進(jìn)入設(shè)定的軌跡后，航向角、深度及水下航跡都基本符合預(yù)期軌跡，深度誤差在0.2 m以內(nèi)，軌跡跟蹤誤差在0.25 m以內(nèi)。

5 結(jié)語

結(jié)合負(fù)責(zé)的AUV項(xiàng)目，筆者介紹了AUV的各艙段結(jié)構(gòu)、硬件控制系統(tǒng)以及MOOS軟件系統(tǒng)，重點(diǎn)說明了AUV的航位推算算法和最后的湖試結(jié)果。湖試結(jié)果驗(yàn)證了這種航位推算算法的可行性，且數(shù)據(jù)表明這種方法具有良好的控制精度，可以適用于AUV的水下導(dǎo)航。AUV整體性能良好，航行平穩(wěn)，達(dá)到了預(yù)期效果。該AUV可以完成水面或者水下規(guī)定航跡的巡視，同時(shí)可以采集航跡上的深度、溫度以及視頻等信息。