爬游無人潛水器導(dǎo)航控制系統(tǒng)設(shè)計(jì)

，， ，，

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

爬游無人潛水器導(dǎo)航控制系統(tǒng)軟件采用模塊化設(shè)計(jì)，將系統(tǒng)的不同功能通過模塊的形式實(shí)現(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、高度深度計(jì)等傳感器設(shè)備)、避障決策模塊、作業(yè)管理模塊、數(shù)據(jù)存儲(chǔ)模塊、應(yīng)急處理模塊等組成。

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

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

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

(1)初始化任務(wù)：實(shí)現(xiàn)程序初始化，包括初始化主板和擴(kuò)展串口模塊的串行通信接口，初始化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ù)存儲(chǔ)任務(wù)：主要是用來保存?zhèn)鞲衅鞑杉瘮?shù)據(jù)和設(shè)備狀態(tài)信息。同時(shí)為保證存盤數(shù)據(jù)的完整性，設(shè)計(jì)了共享數(shù)據(jù)區(qū)保護(hù)機(jī)制。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

struct command {

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

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

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

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

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

3.3 數(shù)據(jù)采集存儲(chǔ)功能實(shí)現(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ù)位，停止位，校驗(yàn)方式ioctl(fd,FIOBAUDRATE,9600);

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

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

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

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

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

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

②調(diào)用dcacheDevCreate()函數(shù)為一個(gè)塊設(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句柄對(duì)磁盤進(jìn)行write和read操作。

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

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

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

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

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

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

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

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

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

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

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

4.3 仿真結(jié)果

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

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

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

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

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

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

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

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

觀察圖9到圖11的仿真結(jié)果，粗線的路徑為潛水器避障的路徑點(diǎn)，兩條細(xì)曲線組成的路徑即為寬度為W的能夠?yàn)闈撍魈峁┳銐蚧顒?dòng)空間的路徑。每一次仿真結(jié)果均顯示三條曲線組成的路徑與障礙物無重合區(qū)域，能夠令潛水器及其腿部有足夠的空間執(zhí)行相應(yīng)的運(yùn)動(dòng)指令。

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

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

5 結(jié)束語

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