基于MOOS的自主式水下機(jī)器人導(dǎo)航系統(tǒng)

邱海洋， 曾慶軍， 智鵬飛

(江蘇科技大學(xué) 電子信息學(xué)院， 江蘇 鎮(zhèn)江 212003)

自主式水下機(jī)器人(autonomous underwater vehicle，AUV)又稱為自主式水下無(wú)人運(yùn)載體，是一種具有探測(cè)和感知能力，且可以在水下運(yùn)動(dòng)的裝備.它可以通過(guò)遠(yuǎn)程控制或自主操作的方式代替人類進(jìn)行海洋調(diào)查和水下作業(yè)，具有廣闊的應(yīng)用前景[1-2].AUV導(dǎo)航系統(tǒng)發(fā)展方向是減少系統(tǒng)對(duì)母船的依賴，最終形成更加智能的自主導(dǎo)航系統(tǒng)[3].但AUV是一個(gè)復(fù)雜系統(tǒng)的集合，集成了運(yùn)動(dòng)控制、多傳感器融合、模式識(shí)別等多項(xiàng)技術(shù)，因此對(duì)軟件控制系統(tǒng)提出了極高的要求.由于AUV的試驗(yàn)成本極高，水下機(jī)器人的應(yīng)用與算法驗(yàn)證都需要一個(gè)理想的仿真環(huán)境進(jìn)行測(cè)試，水下機(jī)器人仿真環(huán)境的建模較為困難，要考慮到水下的流體力學(xué)及作用于機(jī)器人上的一些復(fù)雜傳感器參數(shù).當(dāng)前，使用較為廣泛的系統(tǒng)主要有基于ROS(robot operating system)系統(tǒng)的UUV仿真(unmanned underwater vehicle simulator)系統(tǒng)和基于分布式結(jié)構(gòu)星型拓?fù)涞腗OOS系統(tǒng).UUV仿真系統(tǒng)是基于歐盟智能水下異構(gòu)集群協(xié)作網(wǎng)絡(luò)項(xiàng)目(SWARMs)開(kāi)發(fā)出來(lái)的，載體模型由Gazebo物理引擎生成，屬于純數(shù)據(jù)仿真.MOOS軟件系統(tǒng)由美國(guó)麻省理工大學(xué)開(kāi)發(fā)完成，對(duì)水下機(jī)器人具有更高的專業(yè)性與兼容性，已經(jīng)成功應(yīng)用于水下無(wú)人航行器及大型海洋傳感系統(tǒng)中.它具有自身分布式和模塊化的設(shè)計(jì)理念、憑借易用性、可擴(kuò)展性和可靠性等優(yōu)點(diǎn)，為海洋航行器的設(shè)計(jì)提供優(yōu)良的軟件功能[4-5].為此，筆者提出應(yīng)用MOOS系統(tǒng)對(duì)AUV導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航位姿、通訊及三維信息的仿真，從而更好地模擬AUV水下航行軌跡及導(dǎo)航數(shù)據(jù)采集.

1 AUV導(dǎo)航系統(tǒng)和MOOS系統(tǒng)簡(jiǎn)介

1.1 AUV工作原理及導(dǎo)航系統(tǒng)

以美國(guó)Hydroid公司研發(fā)的水下機(jī)器人為例[6].AUV艙體分為前艙、中間艙和后艙3個(gè)部分.前艙主要用于放置各種水文傳感器、水下攝像頭、多波束等傳感器模塊.中間艙主要裝置PC104控制計(jì)算機(jī)、通訊設(shè)備、聲吶和慣性導(dǎo)航單元等.后艙主要是動(dòng)力控制、能源及動(dòng)力驅(qū)動(dòng)等系統(tǒng).

AUV主要工作是在貼近水面或者復(fù)雜水下環(huán)境中自主展開(kāi)執(zhí)行各項(xiàng)任務(wù)[7].其硬件導(dǎo)航系統(tǒng)主要配備了慣導(dǎo)系統(tǒng)(INS)、多普勒測(cè)速儀(DVL)、衛(wèi)星定位系統(tǒng)(GPS)、深度計(jì)和高度計(jì).傳感器主要參數(shù)見(jiàn)表1.其中，慣性導(dǎo)航的精度包括陀螺儀精度與加速度計(jì)精度，DVL參數(shù)為速度百分比，GPS參數(shù)為單點(diǎn)定位精度，深度計(jì)與高度計(jì)參數(shù)為全量程(FS).

表1 傳感器主要參數(shù)表

導(dǎo)航信息通過(guò)計(jì)算機(jī)處理，應(yīng)用網(wǎng)線與主控計(jì)算機(jī)相連，采用TCP協(xié)議向控制系統(tǒng)發(fā)送導(dǎo)航信息.此外，AUV導(dǎo)航系統(tǒng)采用開(kāi)放式硬件構(gòu)架，可以通過(guò)隨時(shí)增加導(dǎo)航信息源來(lái)保證導(dǎo)航系統(tǒng)的精度.AUV導(dǎo)航系統(tǒng)軟件采用的模塊化設(shè)計(jì)有效降低程序的復(fù)雜性，一般由3個(gè)模塊組成，分別為串口通信模塊、導(dǎo)航濾波模塊和網(wǎng)絡(luò)通信模塊.導(dǎo)航系統(tǒng)模塊結(jié)構(gòu)圖如圖1所示.

圖1 導(dǎo)航系統(tǒng)模塊結(jié)構(gòu)圖

操作系統(tǒng)是基于可編程邏輯控制單元VxWorks軟件系統(tǒng)開(kāi)發(fā)的，在執(zhí)行任務(wù)時(shí)基于任務(wù)優(yōu)先級(jí)，通過(guò)搶占方式進(jìn)行調(diào)度，對(duì)不同的任務(wù)以數(shù)據(jù)結(jié)構(gòu)共享的方式進(jìn)行通訊處理.為了保證系統(tǒng)軟件的可靠性，采用卡爾曼濾波器對(duì)數(shù)據(jù)進(jìn)行濾波處理，以減免異常數(shù)據(jù)干擾對(duì)系統(tǒng)的影響.

1.2 MOOS系統(tǒng)簡(jiǎn)介

MOOS系統(tǒng)作為1種全新的設(shè)備，通常被描述為各種應(yīng)用程序的“中間設(shè)備”，它通過(guò)類似中央服務(wù)器機(jī)能的MOOSDB模塊連接了所有正在運(yùn)行的應(yīng)用程序APP[8-9].每個(gè)應(yīng)用程序都繼承了1個(gè)通用的MOOS接口，該接口提供了一種功能強(qiáng)大、易于使用的數(shù)據(jù)交互方法，可完成客戶端與服務(wù)器端及其他應(yīng)用程序的通信[10].

2 基于MOOS 的AUV系統(tǒng)設(shè)計(jì)

2.1 MOOS整體設(shè)計(jì)思路

MOOS軟件的設(shè)計(jì)主要基于以下3個(gè)思想：

1) 后座駕駛員的設(shè)計(jì)理念.圖2為后座駕駛員設(shè)計(jì)關(guān)系圖.

圖2 后座駕駛員設(shè)計(jì)關(guān)系圖

后座駕駛員設(shè)計(jì)的主要思想是實(shí)現(xiàn)航行器控制系統(tǒng)和自主系統(tǒng)的分離[10].航行器控制系統(tǒng)運(yùn)行在主航行器計(jì)算機(jī)上，而自主系統(tǒng)運(yùn)行在單獨(dú)的負(fù)載計(jì)算機(jī)上.這樣操作的主要優(yōu)點(diǎn)是通過(guò)將航行器自主系統(tǒng)與控制系統(tǒng)分離，提高了運(yùn)算速度，對(duì)航行器的控制也更加高效.它們之間的工作過(guò)程簡(jiǎn)述如下：航行器從主計(jì)算機(jī)上接收航行器的位置和航線信息，從負(fù)荷計(jì)算機(jī)上獲取方向、速度和深度信息；自主系統(tǒng)為航行器控制系統(tǒng)提供前進(jìn)的方向、速度和深度等命令，控制系統(tǒng)執(zhí)行命令，并傳遞航行信息給自主系統(tǒng).圖2中，MOOS系統(tǒng)具有自治功能，可以將各個(gè)APP統(tǒng)一為1個(gè)信息處理整體，但是需要滿足有效載荷的計(jì)算機(jī).

2) 發(fā)布-訂閱式設(shè)計(jì).MOOS提供了基于發(fā)布-訂閱體系結(jié)構(gòu)和協(xié)議的功能.每個(gè)進(jìn)程通過(guò)星型拓?fù)渲械膯蝹€(gè)數(shù)據(jù)庫(kù)與其他進(jìn)程通信.MOOS進(jìn)程星型拓?fù)鋱D見(jiàn)圖3.MOOS社區(qū)是一系列MOOS應(yīng)用程序的集合，這些應(yīng)用程序通常運(yùn)行在1臺(tái)機(jī)器上，每個(gè)程序都有1個(gè)單獨(dú)的進(jìn)程ID.每個(gè)進(jìn)程通過(guò)1個(gè)MOOS數(shù)據(jù)庫(kù)模塊(MOOSDB)以發(fā)布-訂閱的方式進(jìn)行通信.每個(gè)進(jìn)程獨(dú)立于其他進(jìn)程，并由用戶設(shè)置的頻率執(zhí)行其內(nèi)部循環(huán).

圖3 MOOS進(jìn)程星型拓?fù)鋱D

3) 基于行為的控制設(shè)計(jì)理念.基于行為的控制設(shè)計(jì)理念是行為自主設(shè)計(jì)系統(tǒng)(IvP Helm)的核心設(shè)計(jì)思想.IvP Helm是1種基于行為的行為控制器，其自身也是1個(gè)MOOS應(yīng)用程序.其作用是通過(guò)多目標(biāo)值優(yōu)化方法，最終實(shí)現(xiàn)航行器的協(xié)調(diào)運(yùn)行.IvP Helm是1個(gè)非常特殊的軟件模塊，包含在所有自動(dòng)化系統(tǒng)的軟件模塊中.每個(gè)行為都有1個(gè)接口函數(shù)，可以配置每個(gè)行為的接口參數(shù)，從而實(shí)現(xiàn)想要的功能.這種配置通常包含了確切的坐標(biāo)點(diǎn)、搜索區(qū)域和速度等詳細(xì)信息.當(dāng)出現(xiàn)多個(gè)行為競(jìng)爭(zhēng)時(shí)， IvP Helm求解器會(huì)對(duì)它們進(jìn)行分析，并找出優(yōu)化方案.IvP Helm求解器優(yōu)化方案如圖4所示.

圖4 IvP Helm求解器優(yōu)化方案

求解器通過(guò)調(diào)用1個(gè)目標(biāo)函數(shù)來(lái)實(shí)現(xiàn)這種協(xié)調(diào).目標(biāo)函數(shù)來(lái)自航行器中的一些行為參數(shù)，例如，航行器的航向、速度和深度設(shè)置等.總之，IvP Helm是實(shí)現(xiàn)基于多目標(biāo)函數(shù)優(yōu)化的1種技術(shù)，可以通過(guò)合作和協(xié)調(diào)航行器的各種行為，保證命令被正常執(zhí)行.

圖4中的IvP Helm循環(huán)過(guò)程如下：① 從MOOSDB讀取郵件信息，讀取的信息被解析，并存儲(chǔ)在本地緩沖區(qū)中以供行為使用.② 讀取所有發(fā)來(lái)的消息，并存儲(chǔ)于本地后，MOOS將分析所有行為，并建立行為模型.③ 分析每種行為，生成IvP Helm函數(shù)，并在循環(huán)結(jié)束時(shí)將返回的變量值以消息模式發(fā)布到MOOSDB上.④ 分析目標(biāo)函數(shù)，從而實(shí)現(xiàn)最優(yōu)化處理.⑤ 所有變量值都發(fā)布到MOOSDB，以供其他MOOS進(jìn)程使用.

2.2 MOOS系統(tǒng)設(shè)計(jì)框架

MOOS系統(tǒng)具有模塊化設(shè)計(jì)、分布式設(shè)計(jì)和星型拓?fù)浣Y(jié)構(gòu)設(shè)計(jì)的特點(diǎn).模塊化設(shè)計(jì)的好處是通過(guò)把不同的功能抽象成軟件模塊，從而分工非常明確，對(duì)于新模塊的搭載也非常便利.系統(tǒng)中不同的應(yīng)用程序或模塊，如pAntler(開(kāi)啟任務(wù))、iAHRS(航向)、iGPS(定位)、iRemote(遠(yuǎn)程控制)、uMS(圖形界面)、pControler(控制程序)、pMOOSBrigeEx(銥星和無(wú)線通信)和pLogger(記錄數(shù)據(jù))等功能互異，且相互獨(dú)立的應(yīng)用程序都與中央服務(wù)器模塊MOOSDB連接.分布式設(shè)計(jì)可以實(shí)現(xiàn)不同模塊運(yùn)行在不同的水下機(jī)器人控制平臺(tái)，有利于資源合理分配.星型拓?fù)浣Y(jié)構(gòu)如圖3所示，MOOSDB處于結(jié)構(gòu)中心，是MOOS軟件結(jié)構(gòu)的核心部分，主要進(jìn)行不同模塊間信息交互的協(xié)調(diào)工作.所有通信都通過(guò)這個(gè)中央服務(wù)器應(yīng)用程序進(jìn)行.MOOSDB作為MOOS系統(tǒng)的服務(wù)站和數(shù)據(jù)管理站，被認(rèn)為是整個(gè)軟件框架的信息中心及信息交互平臺(tái).

MOOS系統(tǒng)客戶端之間相互透明獨(dú)立，互不干擾.網(wǎng)絡(luò)結(jié)構(gòu)設(shè)計(jì)保持簡(jiǎn)單，服務(wù)器完全了解所有活動(dòng)匹配，可以合理分配通信資源.

2.3 MOOS通訊機(jī)制

MOOSDB和客戶端之間傳輸?shù)臄?shù)據(jù)取決于用戶的需要.然而，數(shù)據(jù)的形式受到MOOS的限制.MOOS只允許以字符串或雙精度格式發(fā)送數(shù)據(jù)，格式數(shù)據(jù)通常以字符串格式進(jìn)行發(fā)送.以進(jìn)程1和進(jìn)程2為例，進(jìn)程1代表AHRS的驅(qū)動(dòng)程序，用來(lái)獲取AUV的航向姿態(tài)等數(shù)據(jù)，進(jìn)程2代表USV的控制進(jìn)程，用來(lái)訂閱USV的航向參數(shù).進(jìn)程1和進(jìn)程2都

包含在CCommsClient組件中，MOOSDB與它們相連.MOOSDB進(jìn)程間通信模型如圖5所示.

圖5 MOOSDB進(jìn)程間通信模型

進(jìn)程2要采集進(jìn)程1的航向數(shù)據(jù)，該過(guò)程是通過(guò)調(diào)用Register函數(shù)來(lái)通知MOOSDB服務(wù)器，MOOSDB接收后會(huì)通知進(jìn)程2,告知其最小的時(shí)間間隔.當(dāng)進(jìn)程1獲取航向信息后則會(huì)調(diào)用Notify函數(shù)，并發(fā)送數(shù)據(jù)給MOOSDB服務(wù)器.然后，當(dāng)MOOSDB服務(wù)器接收到進(jìn)程1發(fā)送過(guò)來(lái)的航向信息后，服務(wù)器會(huì)定時(shí)地把接收到的消息發(fā)送給進(jìn)程2，同時(shí)會(huì)把進(jìn)程2的基本信息反饋給進(jìn)程1.最后，進(jìn)程1和進(jìn)程2都可以通過(guò)調(diào)用Fetch函數(shù)以隨時(shí)獲取它們所需要消息.進(jìn)程1與MOOSDB通信流程圖如圖6所示.

圖6 進(jìn)程1與MOOSDB通信流程圖

MOOS通訊機(jī)制對(duì)于其他模塊的添加變得更加便捷、高效，也便于軟件的開(kāi)發(fā)與維護(hù).由于各模塊都是以MOOSDB為核心的服務(wù)器，也保證了自主式水下機(jī)器人系統(tǒng)在采集數(shù)據(jù)時(shí)可以做到同步性和一致性.

在水下航行器的實(shí)際研究中，由于增加的模塊不多，所需處理的數(shù)據(jù)有限.如GPS衛(wèi)星導(dǎo)航、數(shù)字羅盤(pán)、航向姿態(tài)采集系統(tǒng)、多普勒測(cè)速儀等數(shù)據(jù)較少的傳感器，往往通過(guò)串行口進(jìn)行通信.而攝像頭與其他一些圖像信息等數(shù)據(jù)較多的系統(tǒng)都是通過(guò)以太網(wǎng)通信，并發(fā)送給其他處理器，由它們處理信息.因此，MOOSDB的通信機(jī)制完全勝任數(shù)據(jù)傳遞和信息共享的工作.以MOOSDB為核心的MOOS通信機(jī)制有很多優(yōu)點(diǎn)，例如，當(dāng)水下機(jī)器人工作時(shí)，星型拓?fù)涞慕Y(jié)構(gòu)形式使得其上面無(wú)論搭載了多少模塊，它們的通信方式都是一樣的.

3 基于MOOS的AUV導(dǎo)航仿真

3.1 仿真實(shí)例

基于MOOS的AUV仿真研究是將MOOS IvP軟件安裝在Ubuntu系統(tǒng)中.該系統(tǒng)基于Debian GNU/Linux，支持x86、AMD64(即x64)和ppc架構(gòu)的開(kāi)源操作系統(tǒng).首先，要在計(jì)算機(jī)上安裝上Ubuntu系統(tǒng)，本研究中所用的Ubuntu系統(tǒng)的版本為Ubuntu 14.04LTS.然后，在控制臺(tái)輸入命令：

S|cd moos-ivp/ivp/missions/s1_alpha

S|pAntler alpha_moos

當(dāng)顯示圖7所示頁(yè)面時(shí)，代表MOOS IvP已經(jīng)安裝完成，并模擬AUV相應(yīng)錄像，左側(cè)藍(lán)色區(qū)域?yàn)閷?dǎo)航信息參數(shù).

圖7 MOOS IvP仿真AUV運(yùn)行圖

3.2 AUV導(dǎo)航系統(tǒng)模塊仿真分析

MOOS包含了1個(gè)名為CMOOSApp的基類，它簡(jiǎn)化了作為派生子類編寫(xiě)新的MOOS應(yīng)用程序的過(guò)程[11].在CMOOSApp基類下是1個(gè)循環(huán)，它重復(fù)調(diào)用1個(gè)名為Iterate的函數(shù)，該函數(shù)在默認(rèn)情況下不執(zhí)行任何操作.如果想要編寫(xiě)1個(gè)新的應(yīng)用程序，其中1項(xiàng)工作就是編寫(xiě)這個(gè)函數(shù)，使應(yīng)用程序能夠執(zhí)行想要的任務(wù).在執(zhí)行程序循環(huán)時(shí)，CMOOSApp還需要檢查是否向應(yīng)用程序發(fā)送了新郵件消息.如果有，則調(diào)用另一個(gè)虛函數(shù)OnNewMail，這是用來(lái)處理新郵件消息的函數(shù).MOOSDB模塊主要功能是為水下機(jī)器人提供人工控制接口，操作人員可以通過(guò)岸上軟件平臺(tái)上的鍵盤(pán)等外設(shè)設(shè)備實(shí)現(xiàn)對(duì)機(jī)器人的遠(yuǎn)程控制.

3.3 基于MOOS的AUV導(dǎo)航系統(tǒng)模塊仿真分析

在用戶圖形界面(GUI)中任意調(diào)整軌跡長(zhǎng)度、縮放大小和顏色、航行器尺寸和顏色及平移和縮放等航行器的參數(shù).它們也可以在pMarineViewerMOOS配置模塊中設(shè)置.部分參數(shù)信息見(jiàn)表2.

pNodeReporter存儲(chǔ)位置、速度、航向和深度等航行器導(dǎo)航信息，以及操作模式和平臺(tái)類型等其他信息，并生成變量摘要信息，供pMarineViewer等查看器應(yīng)用程序使用.pNodeReporter應(yīng)用程序運(yùn)行在真實(shí)的或模擬的航行器上，并生成節(jié)點(diǎn)報(bào)告，作為AIS報(bào)告的代理，以便航行器之間信息共享.這個(gè)過(guò)程中不斷地收集本地平臺(tái)信息和導(dǎo)航數(shù)據(jù)，并以NODE_REPORT_LOCAL的MOOS變量的形式創(chuàng)建1個(gè)類似于船舶自動(dòng)識(shí)別系統(tǒng)(AIS)的報(bào)告.pNodeReporter配置參數(shù)見(jiàn)表3.

三維航行器模擬器應(yīng)用1個(gè)簡(jiǎn)單的程序uSimMarine，其功能是可以根據(jù)當(dāng)前執(zhí)行的任務(wù)和之前時(shí)刻航行器的狀態(tài)信息更新水下機(jī)器人的狀態(tài)、位置和軌跡信息，如三維的位姿狀態(tài)信息、速度信息、旋轉(zhuǎn)矢量，以及是否發(fā)送信息的邏輯位監(jiān)測(cè).其中方向x定義為前向，y定義為左手側(cè)，z定義為天向.由于AUV的運(yùn)行一般不經(jīng)常涉及垂直起降，因此z方向信息一般不列入?yún)?shù)配置清單.仿真過(guò)程中可以通過(guò)pause暫停，以讀取相關(guān)的狀態(tài)信息，并進(jìn)行調(diào)試.具體的配置參數(shù)見(jiàn)表4.表4中，通過(guò)drift相關(guān)變量的數(shù)據(jù)檢測(cè)，可以監(jiān)測(cè)航行器的位置偏移；通過(guò)檢測(cè)深度相關(guān)變量數(shù)據(jù)，可以知道絕對(duì)深度與深度變化速率信息；通過(guò)檢測(cè)sim_pause參數(shù)可以判斷是否暫停對(duì)系統(tǒng)仿真的操作，以便于利用中間變量值查閱故障診斷；通過(guò)start_pos參數(shù)值的采集和分析，可以知道仿真初始位置，以生成軌跡信息；利用thrust_factor數(shù)據(jù)可以了解載體的動(dòng)力信息.

4 結(jié) 論

1) 筆者介紹了AUV導(dǎo)航系統(tǒng)的原理與應(yīng)用場(chǎng)景，針對(duì)MOOS系統(tǒng)對(duì)AUV導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航位姿、通訊及三維信息的仿真，模擬AUV水下航行軌跡及導(dǎo)航數(shù)據(jù)采集.

2) 對(duì)MOOS系統(tǒng)中的配置模塊pMarineVie-wer、pNodeReporter和uSimMarine的核心配置參數(shù)進(jìn)行詳細(xì)介紹，實(shí)現(xiàn)AUV模擬調(diào)節(jié)的便捷操作.

3) 在GUI顯示模塊下，設(shè)計(jì)了實(shí)時(shí)的AUV運(yùn)動(dòng)行駛場(chǎng)景，并根據(jù)產(chǎn)生的導(dǎo)航數(shù)據(jù)進(jìn)行解析，使用集成算法進(jìn)行位置估計(jì)解算.