，， ，，

(西北工業(yè)大學(xué) 航海學(xué)院，西安 710072)

0 引言

目前，大部分自主水下航行器(Autonomous Underwater Vehicle，AUV)實現(xiàn)的是水中巡游，定點作業(yè)能力差而且不能沉底作業(yè)，而遙控潛水器(Remote Operated Vehicle，ROV)雖然有沉底作業(yè)能力但是作業(yè)面小并且更換作業(yè)位置費時費力[1-3]。因此需要一種同時具有機動性高、海底作業(yè)面廣的新型水下航行器。深海爬游無人潛水器就是一種既可在深海巡游，又可在海底爬行的新型深海無人潛水器(Unmanned underwater vehicle，UUV)。它在游動時既具有水下自主航行器高效、大范圍的機動能力，同時在海底爬行時又具有遙控機器人的精確移動定位能力。如圖1所示，布置在潛水器兩側(cè)的六條機械腿用于海底爬行；尾部為兩臺水平推進器，可在游動狀態(tài)下提供推力，通過2個推進器差動推進實現(xiàn)水平轉(zhuǎn)向；中軸線上首尾各有一臺導(dǎo)管槳垂向推進器，可提供垂向推力，實現(xiàn)俯仰和深度控制。

圖1 爬游無人潛水器結(jié)構(gòu)示意圖

潛水器控制系統(tǒng)由 “上層導(dǎo)航控制+下層運動控制”兩層控制結(jié)構(gòu)組成。與傳統(tǒng)水下航行器單控制核心的控制系統(tǒng)[4-5]相比，它實現(xiàn)了合理的任務(wù)分配和設(shè)備管理，提高了數(shù)據(jù)處理速度和決策規(guī)劃效率。圖2是爬游無人潛水器控制系統(tǒng)的總體結(jié)構(gòu)設(shè)計框圖，其中上層導(dǎo)航控制系統(tǒng)主要用于完成導(dǎo)航、通信管理、決策規(guī)劃、環(huán)境感知、電源管理等任務(wù)；下層運動控制系統(tǒng)主要用于作業(yè)機械手管理、巡游推進器管理、爬行腿足關(guān)節(jié)管理等任務(wù)。

圖2 爬游無人潛水器控制系統(tǒng)結(jié)構(gòu)框圖

針對新型水下航行器水中巡游和海底爬行作業(yè)的任務(wù)需求，控制系統(tǒng)實時性強、控制精度高、可靠性好的要求，本文提出了一種以PC104和VxWorks為嵌入式平臺設(shè)計的導(dǎo)航控制系統(tǒng)。

1 導(dǎo)航控制系統(tǒng)硬件設(shè)計

導(dǎo)航控制系統(tǒng)主要由導(dǎo)航控制計算機、狀態(tài)監(jiān)測計算機、環(huán)境感知計算機三部分組成。其中，導(dǎo)航控制計算機是導(dǎo)航控制系統(tǒng)的核心部分；狀態(tài)監(jiān)測計算機用于監(jiān)測系統(tǒng)狀態(tài)和拋載上浮自救；環(huán)境感知計算機用于探測周圍障礙和海底坡度等環(huán)境信息。圖3是潛水器導(dǎo)航控制系統(tǒng)硬件結(jié)構(gòu)圖。

圖3 潛水器導(dǎo)航控制系統(tǒng)硬件結(jié)構(gòu)圖

(1)導(dǎo)航控制控制計算機。

它是潛水器控制系統(tǒng)上層導(dǎo)航控制的核心，主要用于潛水器導(dǎo)航控制、工作模式管理、路徑規(guī)劃、設(shè)備管理、供電管理和應(yīng)急處理等任務(wù)。配置有慣性導(dǎo)航設(shè)備(Inertial Navigation System，INS)、高度深度計、多普勒測速儀(Doppler Velocity Log，DVL)、全球定位系統(tǒng)(Global Positioning System，GPS)、超短基線定位系統(tǒng)(Ultra Short Base Line，USBL)5種導(dǎo)航傳感器，同時安裝有水聲通訊設(shè)備、銥星設(shè)備、無線局域網(wǎng)(Wireless Fidelity，WIFI)、網(wǎng)絡(luò)交換機等通信設(shè)備。

(2)狀態(tài)監(jiān)測計算機。

它配置有滲漏傳感器和高度深度計，主要用于監(jiān)測整個潛水器狀態(tài)，并且及時在潛水器超深、海水滲漏、控制系統(tǒng)故障等情況發(fā)生時拋載上浮自救。

(3)環(huán)境感知計算機。

它配置有聲吶作為環(huán)境感知傳感器。本文采用單獨的環(huán)境感知計算機做聲吶信息處理和障礙識別，以此來降低導(dǎo)航控制計算機負擔，提高控制系統(tǒng)的數(shù)據(jù)處理速度和實時性。

2 導(dǎo)航控制系統(tǒng)軟件設(shè)計

2.1 控制系統(tǒng)軟件總體結(jié)構(gòu)

爬游無人潛水器導(dǎo)航控制系統(tǒng)軟件采用模塊化設(shè)計，將系統(tǒng)的不同功能通過模塊的形式實現(xiàn)[6]。圖4為潛水器導(dǎo)航控制系統(tǒng)軟件結(jié)構(gòu)圖，主要由初始化模塊、設(shè)備管理模塊、通信管理模塊(主要有以太網(wǎng)通信模塊、水聲通信模塊、無線電通信模塊組成)、數(shù)據(jù)采集模塊、導(dǎo)航?jīng)Q策模塊(主要有INS、DVL、GPS、USBL、高度深度計等傳感器設(shè)備)、避障決策模塊、作業(yè)管理模塊、數(shù)據(jù)存儲模塊、應(yīng)急處理模塊等組成。

圖4 潛水器導(dǎo)航控制系統(tǒng)軟件結(jié)構(gòu)圖

2.2 導(dǎo)航控制系統(tǒng)多任務(wù)劃分

VxWorks是基于優(yōu)先級的搶占式和輪轉(zhuǎn)式多任務(wù)調(diào)度的嵌入式實時操作系統(tǒng)，導(dǎo)航控制系統(tǒng)在VxWorks下的實現(xiàn)就體現(xiàn)為多個任務(wù)的調(diào)度?？刂葡到y(tǒng)運行時，所執(zhí)行的任務(wù)主要分為以下幾類：

(1)初始化任務(wù)：實現(xiàn)程序初始化，包括初始化主板和擴展串口模塊的串行通信接口，初始化I/O輸出，初始化網(wǎng)絡(luò)接口，調(diào)用任務(wù)創(chuàng)建函數(shù)來建立多任務(wù)處理，初始化看門狗監(jiān)測任務(wù)等。

(2)設(shè)備管理任務(wù)：包括潛水器各設(shè)備開關(guān)量的輸出，設(shè)備的參數(shù)設(shè)置、電源管理，低能耗管理與規(guī)劃，傳感器設(shè)備緊急故障重啟等。

(3)數(shù)據(jù)采集任務(wù)：導(dǎo)航控制系統(tǒng)通過串口獲取各傳感器采集的數(shù)據(jù)，包括姿態(tài)角、速度、角速度、經(jīng)緯度、深度、高度等狀態(tài)信息。

(4)數(shù)據(jù)存儲任務(wù)：主要是用來保存?zhèn)鞲衅鞑杉瘮?shù)據(jù)和設(shè)備狀態(tài)信息。同時為保證存盤數(shù)據(jù)的完整性，設(shè)計了共享數(shù)據(jù)區(qū)保護機制。

(5)通信管理任務(wù)：主要負責和水面、運動控制系統(tǒng)之間的通信。其中，導(dǎo)航控制系統(tǒng)和水面之間的通信分為三部分：在母船上采用WIFI通信[7]部分；在水面上采用無線電通信部分；在水下時采用水聲通信部分。而導(dǎo)航控制系統(tǒng)和運動控制系統(tǒng)都在潛水器電子艙內(nèi)，采用以太網(wǎng)交換機進行網(wǎng)絡(luò)通信。

(6)導(dǎo)航?jīng)Q策任務(wù)：包括潛水器的導(dǎo)航定位、運動決策和模態(tài)切換等功能的實現(xiàn)。

(7)避障決策任務(wù)：主要用于潛水器在巡游和爬行狀態(tài)下的障礙識別決策和路徑規(guī)劃。海底爬行避障的指令發(fā)出過程如圖5所示，由傳感器和環(huán)境感知系統(tǒng)獲取的數(shù)據(jù)輔助導(dǎo)航控制系統(tǒng)的海底爬行任務(wù)發(fā)出爬行指令來避障，這些指令將會發(fā)送給運動控制系統(tǒng)，解算出腿足關(guān)節(jié)角度，以實現(xiàn)潛水器在海底的避障爬行。

圖5 避障爬行指令發(fā)出過程圖

(8)作業(yè)管理任務(wù)：主要用于按系統(tǒng)預(yù)設(shè)作業(yè)流程，發(fā)出作業(yè)指令給運動控制系統(tǒng)來驅(qū)動機械手實現(xiàn)抓取海底樣本等作業(yè)任務(wù)。

(9)應(yīng)急處理任務(wù)：主要用于在潛水器超深、海水滲漏、控制系統(tǒng)故障等情況發(fā)生時及時拋載上浮自救。它與狀態(tài)監(jiān)測計算機互為備份，相互監(jiān)督，確保潛水器在水下出現(xiàn)緊急情況時及時拋載上浮。

3 軟件功能模塊實現(xiàn)

以多任務(wù)調(diào)度機制、通信管理功能、數(shù)據(jù)采集存儲功能的實現(xiàn)為例，介紹導(dǎo)航控制系統(tǒng)軟件實現(xiàn)和結(jié)構(gòu)。

3.1 多任務(wù)調(diào)度機制

潛水器導(dǎo)航控制系統(tǒng)所有任務(wù)，不僅要根據(jù)各任務(wù)相互關(guān)系基于優(yōu)先級進行調(diào)度，還需要借助信號量進行任務(wù)之間的同步和互斥[8]。優(yōu)先級設(shè)置和任務(wù)間通訊方式如表1所示。通信管理任務(wù)接收水面指令，并通過消息隊列向下級任務(wù)發(fā)送。數(shù)據(jù)存儲和開關(guān)量輸出采用基于優(yōu)先級的搶占式調(diào)度方式，通過優(yōu)先級的高低先后執(zhí)行。傳感器數(shù)據(jù)采集任務(wù)的優(yōu)先級都較高，它們從響應(yīng)的中斷得到相應(yīng)的二進制信號量后，從串口讀取數(shù)據(jù)，然后請求互斥信號量，寫數(shù)據(jù)區(qū)，任務(wù)之間采用基于時間片的輪詢調(diào)度。導(dǎo)航控制任務(wù)設(shè)置了最高的優(yōu)先級，以保證潛水器在水下姿態(tài)控制的優(yōu)先。

表1 任務(wù)優(yōu)先級和調(diào)度表

3.2 通信管理功能實現(xiàn)

以導(dǎo)航控制系統(tǒng)和水面的通信為例，導(dǎo)航控制系統(tǒng)接收到水面命令時，需要以相對應(yīng)的格式進行應(yīng)答，應(yīng)答通信格式中以“@”為幀頭，0x+命令代碼+(參數(shù))結(jié)束。

如果水面發(fā)送的命令中的數(shù)據(jù)無效時，則表明導(dǎo)航控制系統(tǒng)需要向水面上傳相應(yīng)的數(shù)據(jù)；當數(shù)據(jù)有效時，表明導(dǎo)航控制系統(tǒng)應(yīng)執(zhí)行該命令，并回傳相應(yīng)的數(shù)據(jù)。

以導(dǎo)航控制系統(tǒng)收到水面的命令“定位爬行命令”(0x24)為例，它帶2個雙精度浮點型數(shù)據(jù)，依次為經(jīng)度、緯度。

struct command {

char ’ ’; (幀頭，為unsigned char=’ ’)

unsigned char 0x24; (當前發(fā)送命令)

double longitude; (潛水器INS經(jīng)度命令)

double latitude; (潛水器INS緯度命令)};

當經(jīng)緯度數(shù)據(jù)longitude和latitude有效時，導(dǎo)航控制系統(tǒng)將該數(shù)據(jù)作為潛水器爬行目標地址來控制潛水器爬行，并上傳“@0x24”，并附帶潛水器INS當前的經(jīng)、緯度數(shù)據(jù)；若經(jīng)緯度數(shù)據(jù)無效時，則不進行任何處理，僅上傳“@0x24”，并附帶潛水器INS當前的經(jīng)、緯度數(shù)據(jù)。

3.3 數(shù)據(jù)采集存儲功能實現(xiàn)

(1)數(shù)據(jù)采集。

潛水器所攜帶的導(dǎo)航傳感器設(shè)備都是采用的RS232和RS485串口，VxWorks串口初始化過程[9]如下所示：

①打開串口

fd=open("/tyCo/0",O_RDWR,0);

②設(shè)置串口raw模式，清空輸入輸出的緩沖區(qū)

ioctl(fd,FIOSETOPTIONS,OPT_RAW);

ioctl(fd,FIOFLUSH,0);

③設(shè)置波特率，數(shù)據(jù)位，停止位，校驗方式ioctl(fd,FIOBAUDRATE,9600);

ioctl(fd,SIO_HW_OPTS_SET,(CLOCAL|CREAD|CS8));

④通過設(shè)備描述符fd，對串口read和write操作。

(2)數(shù)據(jù)存儲。

數(shù)據(jù)存儲主要是將各種數(shù)據(jù)通過文件系統(tǒng)存儲到VxWorks的SATA接口外接硬盤，以便進行事后分析。本文采用的是dosFs文件系統(tǒng)[10]，磁盤初始化步驟如下：

①使用ataDevCreate()函數(shù)在BLK_DEV API設(shè)備驅(qū)動子層上創(chuàng)建一個指向塊存取設(shè)備的指針pAta。

BLK_DEV * ataDevCreate(int ctrl,intdrive,int nBlocks,int blkOffset);

②調(diào)用dcacheDevCreate()函數(shù)為一個塊設(shè)備創(chuàng)建磁盤高速緩沖區(qū)并在CBIO to CBIO設(shè)備(dcacheCbio)子層上生成CBIO句柄。

CBIO_DEV_ID dcacheDevCreate(CBIO_DEV_ID subDev,char *pRamAddr,int memSize,char*pDesc)

③調(diào)用函數(shù)dosFsDevCreate()來在指定分區(qū)上創(chuàng)建dosFs文件系統(tǒng)。

STATUS dosFsDevCreate(char*pDevName,CBIO_DEV_ID cbio,u_int maxFiles,u_int autoChkLevel)

④通過CBIO句柄對磁盤進行write和read操作。

4 導(dǎo)航?jīng)Q策仿真實驗與結(jié)果分析

在實驗室內(nèi)，實現(xiàn)數(shù)據(jù)采集、多任務(wù)調(diào)度、通信管理等功能模塊的基礎(chǔ)上，用聲吶、導(dǎo)航控制計算機、環(huán)境感知計算機來構(gòu)成導(dǎo)航?jīng)Q策仿真系統(tǒng)。由聲吶采集障礙數(shù)據(jù)，環(huán)境感知計算機處理障礙信息，最后由導(dǎo)航控制計算機做出避障決策。將導(dǎo)航控制系統(tǒng)輸出的爬行指令通過Matlab軟件繪圖，驗證導(dǎo)航控制系統(tǒng)決策的控制效果是否達到預(yù)期。

4.1 聲吶數(shù)據(jù)采集

聲吶采集數(shù)據(jù)以十六進制形式通過RS232串口發(fā)給環(huán)境感知計算機，由環(huán)境感知計算機解算出障礙的方向和距離。圖6為環(huán)境感知計算機收到聲吶數(shù)據(jù)的解算過程，首先提取接收緩沖區(qū)的數(shù)據(jù)，分離得到每幀掃描扇區(qū)的數(shù)據(jù)。然后從中提取出每幀數(shù)據(jù)障礙角度和距離，將數(shù)據(jù)按照方向和位置排列成障礙信息矩陣。最后，運用濾波算法，得到如圖7所示的障礙物左邊界、右邊界角度和與潛水器距離信息。

圖6 障礙數(shù)據(jù)處理

圖7 障礙結(jié)算結(jié)果

4.2 導(dǎo)航?jīng)Q策

潛水器在爬行作業(yè)中遇到障礙，采用左優(yōu)先策略進行避障。

聲吶掃描波束劃分如圖8所示，在前進時，設(shè)置其前向掃描波束為(fn1,fn2,fn3,fn4)，其中波束(fn2,fn3)為潛水器安全前進所需波束區(qū)域。

圖8 安全區(qū)域俯視示意圖

導(dǎo)航控制系統(tǒng)避障決策邏輯為：當且僅當在安全區(qū)域(fn2,fn3)內(nèi)沒有檢測到有障礙時保持原方向前行；當避障聲吶檢測到安全區(qū)域(fn2,fn3)內(nèi)有障礙時，潛水器原地左轉(zhuǎn)5 °，再次檢測前方障礙情況，直到檢測到安全區(qū)域(fn2,fn3)內(nèi)無障礙，區(qū)域(fn3,fn4)內(nèi)有障礙時，保持這個方向前進；當避障聲吶檢測到區(qū)域(fn2,fn4)內(nèi)無障礙時，原地右轉(zhuǎn)5 °，再次檢測前方障礙情況，并觀察指向終點的方向，直到安全區(qū)域(fn2,fn3)內(nèi)無障礙，且潛水器前進方向基本與其指向終點的方向重合時，令前進方向為指向終點的方向，保持這個方向前進。

4.3 仿真結(jié)果

潛水器在爬行前進過程中，輸入起點O與終點P的坐標，最終仿真結(jié)果輸出一條路徑，能夠?qū)崿F(xiàn)自主避障并走到終點附近。認定當某一位置到終點P的距離d<0.2 m時即為走到終點。

潛水器及其腿部活動空間的寬度為W=2.259 m，前方安全距離為6 m。在進行仿真時設(shè)置潛水器每次轉(zhuǎn)向為5 °，前進步長為0.3 m。仿真結(jié)果如下：

(1)設(shè)置障礙物為有一定曲率的矩形，起點為(5,15)，目標點為(35,15)，仿真結(jié)果如圖9所示。

圖9 左優(yōu)先避障規(guī)則仿真結(jié)果1

(2)設(shè)置障礙物為不規(guī)則五邊形，起點為(5,10)，目標點為(35,15)，仿真結(jié)果如圖10所示。

圖10 左優(yōu)先避障規(guī)則仿真結(jié)果2

(3)設(shè)置障礙物為不規(guī)則五邊形，起點為(5,15)，終點為(35,15)，仿真結(jié)果如圖11所示。

圖11 左優(yōu)先避障規(guī)則仿真結(jié)果3

觀察圖9到圖11的仿真結(jié)果，粗線的路徑為潛水器避障的路徑點，兩條細曲線組成的路徑即為寬度為W的能夠為潛水器提供足夠活動空間的路徑。每一次仿真結(jié)果均顯示三條曲線組成的路徑與障礙物無重合區(qū)域，能夠令潛水器及其腿部有足夠的空間執(zhí)行相應(yīng)的運動指令。

圖9與圖11相比，起點相同，障礙物不同；圖10與圖11相比，障礙物相同，起點不同。對左優(yōu)先避障規(guī)則的仿真我們分別從不同起點，不同障礙物角度出發(fā)，綜合驗證了左優(yōu)先避障規(guī)則的正確性。自主避障規(guī)則的正確性體現(xiàn)在2個方面：第一是能夠給出一條路徑繞過障礙物走到終點附近；第二是根據(jù)這一條路徑計算出的寬度為W的爬行路徑不能夠與障礙物有重合的區(qū)域。

經(jīng)過上述三組實驗驗證，實際的仿真結(jié)果和設(shè)計的導(dǎo)航?jīng)Q策邏輯吻合，從而驗證了導(dǎo)航控制系統(tǒng)的控制決策達到了預(yù)期的效果。

5 結(jié)束語

本文以PC104嵌入式計算機為硬件平臺，實時多任務(wù)操作系統(tǒng)VxWorks為軟件平臺，為新型水下航行器——爬游無人潛水器設(shè)計了高效、穩(wěn)定的嵌入式導(dǎo)航控制系統(tǒng)。設(shè)計過程中給出了系統(tǒng)導(dǎo)航控制計算機、狀態(tài)監(jiān)測計算機、環(huán)境感知計算機的硬件設(shè)計方案；同時實現(xiàn)了多任務(wù)調(diào)度、通信管理、數(shù)據(jù)采集等軟件功能模塊，最后通過實驗驗證了導(dǎo)航控制系統(tǒng)導(dǎo)航?jīng)Q策的正確性，為爬游無人潛水器實現(xiàn)深海巡游和海底爬行的任務(wù)目標提供了可靠的導(dǎo)航控制系統(tǒng)，同時對其它嵌入式控制系統(tǒng)的開發(fā)具有重要參考價值。