全自動(dòng)水陸兩棲服務(wù)機(jī)器人平臺(tái)*

吳英健，歐 濤，張京玲，林柏誠(chéng)，候飛龍

（五邑大學(xué) 智能制造學(xué)部，廣東 江門529020）

“十一五”期間，國(guó)家已經(jīng)把“廢墟搜索與輔助救援機(jī)器人”列入國(guó)家863項(xiàng)目，由此可見(jiàn)救援機(jī)器人的重要性。全自動(dòng)水陸兩棲服務(wù)機(jī)器人具有很強(qiáng)的環(huán)境適應(yīng)能力，有廣泛的作業(yè)范圍，方便投放與回收，可在人類無(wú)法完成作業(yè)的地形，安全快捷檢測(cè)水質(zhì)[1-2]，例如陸地、湖泊、河流、沼澤和海洋等，既可以應(yīng)用于河道水質(zhì)檢測(cè)，也可以應(yīng)用于淡水養(yǎng)殖和緊急的救援活動(dòng)等。

本論文設(shè)計(jì)了全自動(dòng)水陸兩棲服務(wù)機(jī)器人平臺(tái)，可以實(shí)現(xiàn)河道水質(zhì)全自動(dòng)檢測(cè)，淡水養(yǎng)殖全自動(dòng)管理和全自動(dòng)緊急救援等功能。用戶還可以通過(guò)電腦端或者手機(jī)端直接控制水陸兩棲服務(wù)機(jī)器人完成作業(yè)。

1 平臺(tái)總體設(shè)計(jì)

全自動(dòng)水陸兩棲服務(wù)機(jī)器人平臺(tái)由水陸兩棲機(jī)械結(jié)構(gòu)、環(huán)境建模子系統(tǒng)（包括環(huán)境建模主控器、通信系統(tǒng)、定位導(dǎo)航避障系統(tǒng)、攝像頭）、工控子系統(tǒng)、中央控制平臺(tái)和多種配件裝置等部分組成。

中央控制平臺(tái)獲取用戶各種任務(wù)指令，系統(tǒng)對(duì)每個(gè)任務(wù)劃分優(yōu)先級(jí)，通過(guò)A*算法[3]生成一條大致路徑，之后再通過(guò)布谷鳥(niǎo)優(yōu)化算法[4]生成一條優(yōu)化路徑，通信系統(tǒng)自動(dòng)控制水陸兩棲服務(wù)機(jī)器人完成相應(yīng)任務(wù)。水陸兩棲服務(wù)機(jī)器人利用定位模塊和避障模塊實(shí)時(shí)獲取自身位置和周邊障礙物情況，采用履帶和螺旋槳兩種動(dòng)力傳輸，完成水陸混雜任務(wù)。同時(shí)，還可通過(guò)攝像頭在電腦和手機(jī)端進(jìn)行遠(yuǎn)程實(shí)時(shí)遙控。平臺(tái)上設(shè)定導(dǎo)軌和卡口，可以根據(jù)不同應(yīng)用場(chǎng)景的不同任務(wù)需求，靈活搭建多種配件裝置。

平臺(tái)電控部分由環(huán)境建模主控器、WiFi通信模塊、定位模塊、避障模塊、攝像頭模塊、工控控制器、電機(jī)驅(qū)動(dòng)模塊、各配置功能控制模塊和電源模塊等組成。水陸兩棲服務(wù)機(jī)器人電控框圖如圖1所示。

圖1 全自動(dòng)水陸兩棲服務(wù)機(jī)器人電控框圖

2 多傳感器融合導(dǎo)航系統(tǒng)設(shè)計(jì)

定位模塊采用基于TDOA算法UWB定位技術(shù)與低成本GPS組合定位的水上機(jī)器人位置定位方法提供高精度定位[5]。導(dǎo)航避障模塊采用毫米波雷達(dá)傳送目標(biāo)信息，激光雷達(dá)傳送360°距離信息，風(fēng)向傳感器傳送風(fēng)速及角度，定位系統(tǒng)傳送水陸兩棲服務(wù)機(jī)器人位置信息。環(huán)境建模主控制器處理這些信息判定自身與目標(biāo)體的相對(duì)位置并得出相應(yīng)指令控制機(jī)器人。多傳感器融合的導(dǎo)航系統(tǒng)框圖如圖2所示。

圖2 多傳感器融合的導(dǎo)航系統(tǒng)框圖

2.1 UWB與GPS組合高精度定位

使用基于TDOA算法的UWB定位技術(shù)與低成本GPS組合定位方法。不僅能夠節(jié)省技術(shù)成本而且能夠大幅度提高定位精度。

基于TDOA的UWB定位技術(shù)測(cè)量出來(lái)的水陸兩棲服務(wù)機(jī)器人的實(shí)時(shí)位移觀測(cè)方程和GPS定位技術(shù)的偽距觀測(cè)方程得到位置補(bǔ)償方程。然后，位置補(bǔ)償方程與GPS的偽距觀測(cè)方程使用擴(kuò)展卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，最后得到具有實(shí)時(shí)性和方向性的精確水上定位坐標(biāo)。

2.2 基于人工勢(shì)場(chǎng)法的自主避障算法

首先根據(jù)當(dāng)前姿態(tài)信息、環(huán)境感知信息以及路徑規(guī)劃信息生成當(dāng)前坐標(biāo)信息，當(dāng)水陸兩棲服務(wù)機(jī)器人前方檢測(cè)存在障礙物且周邊沒(méi)有障礙物時(shí)，進(jìn)一步判斷朝向是否與當(dāng)前目的地連接線段相接近以及當(dāng)前坐標(biāo)與當(dāng)前目標(biāo)坐標(biāo)在某一坐標(biāo)是否接近后，把當(dāng)前坐標(biāo)修改為接近的坐標(biāo)；當(dāng)水陸兩棲服務(wù)機(jī)器人前方檢測(cè)存在障礙物且周邊有障礙物時(shí)，按照人工勢(shì)場(chǎng)法的原則如公式（1）-（3）所示令水陸兩棲服務(wù)機(jī)器人與障礙物產(chǎn)生斥力，將當(dāng)前位置坐標(biāo)與障礙物邊緣的坐標(biāo)形成水陸兩棲服務(wù)機(jī)器人偏轉(zhuǎn)角度并生成新的目標(biāo)點(diǎn)實(shí)現(xiàn)避障。避障流程圖如圖3所示。

圖3 避障流程圖

3 基于分段式雙閉環(huán)PID的電機(jī)控制

水陸兩棲服務(wù)機(jī)器人在接收到工控機(jī)下達(dá)的目標(biāo)坐標(biāo)指令之后會(huì)啟動(dòng)電機(jī)推動(dòng)水陸兩棲服務(wù)機(jī)器人向目標(biāo)航向航行。

水面航行器容易受到慣性的影響而難以控制，因此一般的PID在水面航行會(huì)出現(xiàn)很難達(dá)到預(yù)想效果，所以增加對(duì)速度V與角速度AV變量來(lái)對(duì)慣性進(jìn)行調(diào)節(jié)。通過(guò)傳感器獲取的航向角度作為閉環(huán)的外環(huán)，實(shí)現(xiàn)航向角度的閉環(huán)控制。根據(jù)電機(jī)轉(zhuǎn)速大小分為低速、中速、高速三個(gè)檔次，通過(guò)給三個(gè)檔次分別設(shè)置三個(gè)特定的PID控制器，以此來(lái)克服無(wú)刷直流電機(jī)在越高轉(zhuǎn)速的情況下越難達(dá)到使水陸兩棲服務(wù)機(jī)器人航行穩(wěn)定的缺點(diǎn)。通過(guò)將兩棲機(jī)器人的速度作為閉環(huán)的內(nèi)環(huán)來(lái)實(shí)現(xiàn)其速度控制。在雙閉環(huán)PID控制器的控制下對(duì)PWM輸出進(jìn)行調(diào)節(jié)，最終實(shí)現(xiàn)對(duì)航向姿態(tài)的穩(wěn)定調(diào)整。雙閉環(huán)PID控制系統(tǒng)如圖4所示。如公式（4）所示，u（t）為控制器的輸出量，e（t）為控制輸入量，即給定量與輸出量的偏差，水陸兩棲服務(wù)機(jī)器人通過(guò)PID控制器計(jì)算得：

圖4 雙閉環(huán)PID電機(jī)控制系統(tǒng)

4 多任務(wù)路徑規(guī)劃算法

首先，在地圖上放置多邊形的可行區(qū)域，規(guī)定機(jī)器人平臺(tái)的活動(dòng)范圍，然后在軟件界面放置任務(wù)類型、緊急程度和坐標(biāo)，利用A*算法，把設(shè)備設(shè)置為起點(diǎn)和父類，各個(gè)任務(wù)坐標(biāo)點(diǎn)為終點(diǎn)，計(jì)算得到當(dāng)前設(shè)備坐標(biāo)與各個(gè)目標(biāo)點(diǎn)的G的總值和設(shè)備到達(dá)各個(gè)目標(biāo)點(diǎn)的路徑，記錄為一個(gè)各任務(wù)點(diǎn)間的距離矩陣[6]。用布谷鳥(niǎo)優(yōu)化算法對(duì)路徑進(jìn)行優(yōu)化，尋找時(shí)間成本最低的最短路徑。其中，用輪盤賭的方式產(chǎn)生一個(gè)較優(yōu)的初始化解，大幅度降低迭代次數(shù)，減少算法優(yōu)化的時(shí)間成本。而優(yōu)化過(guò)程使用自適應(yīng)算子，讓算法能快速收斂，能在短時(shí)間內(nèi)得到最優(yōu)解。采取以下方法準(zhǔn)則對(duì)路徑節(jié)點(diǎn)進(jìn)行優(yōu)化：

（1）刪除節(jié)點(diǎn)準(zhǔn)則。

（2）新增節(jié)點(diǎn)準(zhǔn)則。即進(jìn)行弗洛伊德平滑優(yōu)化，得到一條平滑的路徑。路徑規(guī)劃算法流程圖如圖5所示。

5 結(jié)論

該系統(tǒng)平臺(tái)定位誤差在10cm以內(nèi)，避障最大距離為50m，最大航速2.5m/s，可以較好完成水陸混雜的自動(dòng)巡航任務(wù)；能夠搭建多種配件裝置，例如機(jī)械臂裝置、水質(zhì)檢測(cè)裝置等用于洪水緊急救援、精細(xì)養(yǎng)殖以及改良河道等應(yīng)用場(chǎng)景。