吳英健,歐 濤,張京玲,林柏誠,候飛龍
(五邑大學 智能制造學部,廣東 江門529020)
“十一五”期間,國家已經(jīng)把“廢墟搜索與輔助救援機器人”列入國家863項目,由此可見救援機器人的重要性。全自動水陸兩棲服務(wù)機器人具有很強的環(huán)境適應(yīng)能力,有廣泛的作業(yè)范圍,方便投放與回收,可在人類無法完成作業(yè)的地形,安全快捷檢測水質(zhì)[1-2],例如陸地、湖泊、河流、沼澤和海洋等,既可以應(yīng)用于河道水質(zhì)檢測,也可以應(yīng)用于淡水養(yǎng)殖和緊急的救援活動等。
本論文設(shè)計了全自動水陸兩棲服務(wù)機器人平臺,可以實現(xiàn)河道水質(zhì)全自動檢測,淡水養(yǎng)殖全自動管理和全自動緊急救援等功能。用戶還可以通過電腦端或者手機端直接控制水陸兩棲服務(wù)機器人完成作業(yè)。
全自動水陸兩棲服務(wù)機器人平臺由水陸兩棲機械結(jié)構(gòu)、環(huán)境建模子系統(tǒng)(包括環(huán)境建模主控器、通信系統(tǒng)、定位導(dǎo)航避障系統(tǒng)、攝像頭)、工控子系統(tǒng)、中央控制平臺和多種配件裝置等部分組成。
中央控制平臺獲取用戶各種任務(wù)指令,系統(tǒng)對每個任務(wù)劃分優(yōu)先級,通過A*算法[3]生成一條大致路徑,之后再通過布谷鳥優(yōu)化算法[4]生成一條優(yōu)化路徑,通信系統(tǒng)自動控制水陸兩棲服務(wù)機器人完成相應(yīng)任務(wù)。水陸兩棲服務(wù)機器人利用定位模塊和避障模塊實時獲取自身位置和周邊障礙物情況,采用履帶和螺旋槳兩種動力傳輸,完成水陸混雜任務(wù)。同時,還可通過攝像頭在電腦和手機端進行遠程實時遙控。平臺上設(shè)定導(dǎo)軌和卡口,可以根據(jù)不同應(yīng)用場景的不同任務(wù)需求,靈活搭建多種配件裝置。
平臺電控部分由環(huán)境建模主控器、WiFi通信模塊、定位模塊、避障模塊、攝像頭模塊、工控控制器、電機驅(qū)動模塊、各配置功能控制模塊和電源模塊等組成。水陸兩棲服務(wù)機器人電控框圖如圖1所示。
圖1 全自動水陸兩棲服務(wù)機器人電控框圖
定位模塊采用基于TDOA算法UWB定位技術(shù)與低成本GPS組合定位的水上機器人位置定位方法提供高精度定位[5]。導(dǎo)航避障模塊采用毫米波雷達傳送目標信息,激光雷達傳送360°距離信息,風向傳感器傳送風速及角度,定位系統(tǒng)傳送水陸兩棲服務(wù)機器人位置信息。環(huán)境建模主控制器處理這些信息判定自身與目標體的相對位置并得出相應(yīng)指令控制機器人。多傳感器融合的導(dǎo)航系統(tǒng)框圖如圖2所示。
圖2 多傳感器融合的導(dǎo)航系統(tǒng)框圖
使用基于TDOA算法的UWB定位技術(shù)與低成本GPS組合定位方法。不僅能夠節(jié)省技術(shù)成本而且能夠大幅度提高定位精度。
基于TDOA的UWB定位技術(shù)測量出來的水陸兩棲服務(wù)機器人的實時位移觀測方程和GPS定位技術(shù)的偽距觀測方程得到位置補償方程。然后,位置補償方程與GPS的偽距觀測方程使用擴展卡爾曼濾波算法進行數(shù)據(jù)融合,最后得到具有實時性和方向性的精確水上定位坐標。
首先根據(jù)當前姿態(tài)信息、環(huán)境感知信息以及路徑規(guī)劃信息生成當前坐標信息,當水陸兩棲服務(wù)機器人前方檢測存在障礙物且周邊沒有障礙物時,進一步判斷朝向是否與當前目的地連接線段相接近以及當前坐標與當前目標坐標在某一坐標是否接近后,把當前坐標修改為接近的坐標;當水陸兩棲服務(wù)機器人前方檢測存在障礙物且周邊有障礙物時,按照人工勢場法的原則如公式(1)-(3)所示令水陸兩棲服務(wù)機器人與障礙物產(chǎn)生斥力,將當前位置坐標與障礙物邊緣的坐標形成水陸兩棲服務(wù)機器人偏轉(zhuǎn)角度并生成新的目標點實現(xiàn)避障。避障流程圖如圖3所示。
圖3 避障流程圖
水陸兩棲服務(wù)機器人在接收到工控機下達的目標坐標指令之后會啟動電機推動水陸兩棲服務(wù)機器人向目標航向航行。
水面航行器容易受到慣性的影響而難以控制,因此一般的PID在水面航行會出現(xiàn)很難達到預(yù)想效果,所以增加對速度V與角速度AV變量來對慣性進行調(diào)節(jié)。通過傳感器獲取的航向角度作為閉環(huán)的外環(huán),實現(xiàn)航向角度的閉環(huán)控制。根據(jù)電機轉(zhuǎn)速大小分為低速、中速、高速三個檔次,通過給三個檔次分別設(shè)置三個特定的PID控制器,以此來克服無刷直流電機在越高轉(zhuǎn)速的情況下越難達到使水陸兩棲服務(wù)機器人航行穩(wěn)定的缺點。通過將兩棲機器人的速度作為閉環(huán)的內(nèi)環(huán)來實現(xiàn)其速度控制。在雙閉環(huán)PID控制器的控制下對PWM輸出進行調(diào)節(jié),最終實現(xiàn)對航向姿態(tài)的穩(wěn)定調(diào)整。雙閉環(huán)PID控制系統(tǒng)如圖4所示。如公式(4)所示,u(t)為控制器的輸出量,e(t)為控制輸入量,即給定量與輸出量的偏差,水陸兩棲服務(wù)機器人通過PID控制器計算得:
圖4 雙閉環(huán)PID電機控制系統(tǒng)
首先,在地圖上放置多邊形的可行區(qū)域,規(guī)定機器人平臺的活動范圍,然后在軟件界面放置任務(wù)類型、緊急程度和坐標,利用A*算法,把設(shè)備設(shè)置為起點和父類,各個任務(wù)坐標點為終點,計算得到當前設(shè)備坐標與各個目標點的G的總值和設(shè)備到達各個目標點的路徑,記錄為一個各任務(wù)點間的距離矩陣[6]。用布谷鳥優(yōu)化算法對路徑進行優(yōu)化,尋找時間成本最低的最短路徑。其中,用輪盤賭的方式產(chǎn)生一個較優(yōu)的初始化解,大幅度降低迭代次數(shù),減少算法優(yōu)化的時間成本。而優(yōu)化過程使用自適應(yīng)算子,讓算法能快速收斂,能在短時間內(nèi)得到最優(yōu)解。采取以下方法準則對路徑節(jié)點進行優(yōu)化:
(1)刪除節(jié)點準則。
(2)新增節(jié)點準則。即進行弗洛伊德平滑優(yōu)化,得到一條平滑的路徑。路徑規(guī)劃算法流程圖如圖5所示。
該系統(tǒng)平臺定位誤差在10cm以內(nèi),避障最大距離為50m,最大航速2.5m/s,可以較好完成水陸混雜的自動巡航任務(wù);能夠搭建多種配件裝置,例如機械臂裝置、水質(zhì)檢測裝置等用于洪水緊急救援、精細養(yǎng)殖以及改良河道等應(yīng)用場景。