兩棲球形機器人的路徑規(guī)劃策略

馬宇科，鄭 亮,2*，胡高凱，吉曉雯，司兆怡，劉晏彤

(1.吉林農業(yè)科技學院，吉林 吉林 132101；2.長春理工大學，吉林 長春 130022)

1 引 言

目前，在機器人研究領域，兩棲仿生機器人的研究逐步成為研究熱點，通過對一種動物運動方式的長時間觀察與研究，提出了相應具有運動模式的仿生機器人的設計方案。這類機器人具有較為靈活的兩棲運動能力，能夠在水下實現(xiàn)最佳的路徑控制和對目標具有一定的識別與追蹤能力，但是，以往提出的仿生機器人大多只適應于水下的運動環(huán)境，對于目前所需要的兩棲運動環(huán)境并不合適，球形兩棲機器人能夠很好地解決這一問題。在一些特殊的應用環(huán)境下，例如兩棲偵察、海底探測、深海探測等相關領域，一般性能的機器人無法滿足要求，所以兩棲機器人應運而生。由于兩棲機器人特殊的靈活性、超強的適應能力、便于投放和回收的優(yōu)越特性，使它可以獨立在水下完成偵查、搜救、探測、數(shù)據(jù)收集等工作。所以兩棲球形機器人很自然地成為人類延伸自己感知能力的主要工具之一。本文提出的兩棲球形機器人是一種先進的執(zhí)行裝置，包括運動推進器，傳感器，控制板和安裝在球形殼體中的電源裝置。作為微型球形機器人，這種機器人得到廣泛應用主要依靠四個技術優(yōu)勢。第一個特點是球形機器人是一種可移動行走的移動機器人，可以保持先進的平衡性、穩(wěn)定性和運動連續(xù)性。第二個特點是球形機器人具有良好的密封性，可以完全保護內部控制單元和機構，這是其他機器人無法做到的。第三個特點是球形機器人具有很強的適應性，能在無人區(qū)、灰塵、濕氣、腐蝕性和惡劣環(huán)境下完成任務。最后是矢量推進器具有更高的穩(wěn)定性和靈活性，能使機器人保持更好的水下運動性能和抗噪聲干擾能力。

兩棲機器人控制技術發(fā)展迅速，北京理工大學仿生機器人與系統(tǒng)教育部重點實驗室郭書祥團隊研究的兩棲球形機器人，是以球形為主體的機器人，整體結構分為上半球和下半球。在陸地模式時，下半球可以折疊到上半球，用4個由8個舵機組成的機械臂行走，在水下模式的時候，折疊的上半球通過二個舵機封閉下半球，由噴水電機推進行走，球體內部也安裝了通信和穩(wěn)定控制模塊，該球基本實現(xiàn)了球形機器人的基本功能,但沒有實現(xiàn)水下的自動路徑規(guī)劃[1-3]。北京郵電大學孫漢旭團隊研究的球形機器人以摩擦力為驅動力，沒有被動摩擦力的球形機器人，該機器人具有運動效率高，對路面要求低，適應能力強等優(yōu)點[4]。哈爾濱工程大學葉秀芬團隊，也對球形機器人“基于噴水推進的微小球形水下潛器”進行了深入的研究[5]。天津理工大學郭健團隊也對球形機器人在陸地上的路徑規(guī)劃進行了深入的研究[6]。但目前針對水下路徑規(guī)劃的相關研究相對較少。

機器人執(zhí)行有障礙的復雜水下任務時，路徑規(guī)劃對水下機器人實現(xiàn)任務目標具有非常重要的意義。文獻[7]提出了一種基于先驗知識強化學習策略的最佳路徑選擇的新算法。針對未知空間中移動機器人的路徑規(guī)劃問題，Yuan等提出了一種基于門控遞歸單元-遞歸神經(jīng)網(wǎng)絡模型的動態(tài)路徑規(guī)劃方法[8]。Bae等提出了一種結合深度學習和卷積神經(jīng)網(wǎng)絡的多機器人路徑規(guī)劃算法[9]。文獻[10]提出了一種非完整的三輪移動機器人的路徑規(guī)劃和控制方法，該機器人用于在道路跟蹤和復雜環(huán)境中進行在線導航。文獻[11]開發(fā)了一種“增強輪輞跳躍”的方法，該方法不依賴于逐點定位，而是通過找到障礙物之間的多次切線來獲得最短路徑。盡管在兩棲球形機器人的路徑規(guī)劃方面已有許多研究，但是多數(shù)研究是基于單一的陸地環(huán)境下進行的。本文以實現(xiàn)球形兩棲機器人最佳路徑規(guī)劃為研究目標，針對兩種基于視覺伺服的廣義約束優(yōu)化(Generalized Constraint Optimization,GCOP)和序列二次規(guī)劃(Sequential Quadratic Procontrol,SQP)的機器人運動控制算法進行對比分析，使用視覺伺服傳感器實現(xiàn)兩棲機器人的最佳路徑規(guī)劃，并實時對監(jiān)測動態(tài)目標進行標定、移動目標監(jiān)測、水下障礙物識別和目標跟蹤。最后，通過水下運動測試和仿真來對比驗證算法的可行性。該運動的路徑規(guī)劃方案提高了球形兩棲機器人的運動性能，使機器人可以執(zhí)行更為復雜的水下任務。

2 兩棲球形機器人設計

2.1 機械設計

新型兩棲球形機器人不僅具有良好的陸地運動能力，而且能夠實現(xiàn)水下多自由度的運動。圖1是機器人陸地運動模式和水下運動模式的結構。在陸地模式下，機器人下半球殼體通過兩個舵機折疊至上半球，4個噴水推進器根據(jù)相應步態(tài)調整實現(xiàn)陸地行走[12]。在水下模式下，下半球閉合，機器人可以利用4個水下推進器實現(xiàn)水下多自由度運動。相比其他兩棲球形機器人，加入了穩(wěn)定控制塊、4個激光測距模塊和4個視覺采集模塊，用來實現(xiàn)路徑規(guī)劃的相關參數(shù)采集。

圖1 兩棲球形機器人的機械結構Fig.1 Mechanism of amphibious spherical robot

2.2 硬件構成

兩棲球形機器人采用模塊化設計，通過各模塊的合理分布與協(xié)調工作，保證球體運動的可靠性和穩(wěn)定性。如圖2所示，球體主要搭載一塊嵌入式處理器(ARM S3C6410,2 GB DDR3,linux 3.12.0)，用于數(shù)據(jù)處理；GPS模塊和聲納模塊用于機器人通訊。陀螺儀傳感器對機器人進行姿態(tài)感應與調整，伺服電機和電機控制器控制機器人的運動和姿態(tài)，圖像采集模塊用于機器人的視覺識別與動態(tài)目標捕捉，5000 mA鋰電供電模塊用于機器人的供電。

圖2 兩棲球形機器人的硬件結構Fig.2 Hardware structure of amphibious spherical robot

3 兩棲球形機器人上浮下潛原理

圖3 機器人上浮和下潛原理Fig.3 Principle for robot floating and diving

球體分為上半球和下半球兩個倉室，如圖3所示，下倉室裝有排水口，通過上倉室的倉室氣泵將上倉室的空氣壓入下倉室，通過下倉室的水壓調節(jié)罐控制水箱的進水量，根據(jù)阿基米德浮力原理，球體的浮力由球體排開水的體積決定。當機器人下潛時，防水舵機順時針旋轉，將放氣的閥門關閉，水從球體一端的孔進入，由于壓力的作用，原本在球體內部的氣體從球的另一端排出。水的重量再加上球體本身防水舵機的重量使球形機器人進行下潛。上浮時，防水舵機逆時針旋轉，將二氧化碳氣瓶中的壓縮二氧化碳氣體放出，此時排氣孔和進水孔都變成了排水孔，機器人上浮。

4 運動控制和路徑規(guī)劃算法

4.1 非線性6自由度算法

兩棲球形機器人能夠在陸地與水下多自由度運動，在陸地上要具有4個自由度，在水下要具有6個自由度的運動模式。

圖4 6自由度建模坐標標定Fig.4 Calibration of 6-DOF modeling coordinate

6自由度包括RPY(q，p，r)和ZYZ(v，u，w)參考系。為了達到精確控制的目的，分別給出了兩種受控源的旋轉矩陣。如圖4所示，參考坐標以x軸圍繞的角度“q”旋轉，并用矩陣Rx(q)表示。參考坐標以Y軸圍繞的角度“p”旋轉，并用矩陣Ry(p)表示。參考坐標以Z軸圍繞的角度“r”旋轉，并用矩陣Rz(r)表示[13]。

R(φ)=Rx(q)Ry(p)Rz(r)=

(1)

假設參考坐標繞Z軸旋轉一個φ的角度，并且旋轉矩陣為Rz(φ), 參考坐標繞Y軸旋轉一個?的角度，并且旋轉矩陣為Ry′(?),參考坐標繞Z軸旋轉一個ψ的角度，并且旋轉矩陣為Rz″(ψ)。最終坐標系的方向是通過合成相對于當前坐標系的旋轉矩陣并通過右乘計算得出的，獲得基本矩陣為：

R(Φ)=Rz(φ)Ry′(?)Rz″(φ).

(2)

旋轉矩陣為：

(3)

參數(shù)?在[π,0]，可以表示為：

(4)

參數(shù)φ在[-π,0]，可以表示為：

(5)

4.2 GCOP控制算法

假設物體的表面由m個方程表示為：hi(x),i=1,2,…,m，并且它的內部方程為[14-16]：

h1<0∧h2∧…∧hm<0.

(6)

對每個hi構造新的函數(shù)：

(7)

其中：t是一個小的正實數(shù),vi是x和t的函數(shù)，對整個物體,構造函數(shù)V：

(8)

驗證從hi到vi和vi到V的兩個變換的性質。首先，函數(shù)vi對任意x和t總是正的，其次，vi是關于hi的遞增函數(shù)，即當hi>0時vi的值和當hi<0時vi的值。如果t?1,vi可以近似表示為：

(9)

其中：O(t2)為一個值非常小的正數(shù)。式(9)表明，除了點在物體表面附近時，hi=0；當hi>0時，vi>t；當hi<0時，vi

對于在物體hi內部和邊界附近的點，其他函數(shù)值hi(j=1,2,...,m,j≠i)是小于零的，因此得到vt=O(t2)。將式(7)替換到式(9)中，得到:

(10)

從式(10)可以看出，當所有的hi是負，即點在物體內部時，函數(shù)V非常小；當點在物體外部時，V?t+O(t2)；當點在物體邊界附近時，V≈t。

考慮當t→0時，則有：

(11)

兩個正數(shù)的和還是正數(shù)，一個正數(shù)與零的和是正數(shù)，兩個零的和是零。因此，如果把正數(shù)作為邏輯“1”，把零作為邏輯“0”，那么式(11)中vi的“加”操作對應了布爾“或”運算。所以得到：

(12)

在水下機器人路徑規(guī)劃中，不期望路徑太靠近障礙物。因此，引入一個小的正數(shù)Δv作為路徑到障礙物的距離控制參數(shù)，如果x滿足以下不等式：

V=∑vi≥Δv或Δv-∑vi<0.

(13)

那么這個點一定在由式(13)所確定的障礙物的外部。如果Δv→0，由Δv-∑vi≤0確定的邊界將趨近于障礙物的表面。如果一個物體的表面和外部由(h1≥0)∨(h2≥0)∨…∨(hm≥0)確定，那么它的外部和表面同樣可以由Δv-∑vi≤0確定，其中Δv→0。x若滿足Δv-∑vi≤0，那么x就落在物體的外部。

4.3 SQP控制算法

為了在三個維度上對兩棲球形機器人進行建模，必須設定6個獨立變量構建環(huán)境的空間位置、方向、大小和形狀，O=O(x0,y0,z0)代表機器人的幾何中心，θ=(θ1,θ2,θ3)代表機器人的定位方向[17-19]。

x=x(x0,y0,z0,θ1,θ2,θ3,v),

(14)

y=y(x0,y0,z0,θ1,θ2,θ3,v),

(15)

z=z(x0,y0,z0,θ1,θ2,θ3,v),

(16)

其中：x，y，z是機器人邊界上的一個點，用于構造沒有碰撞條件的機器人，x0,y0,z0是空間位置，θ1,θ2,θ3是空間方向，v是具有兩個參數(shù)的向量，(t1,t2)用于表示特定機器人的邊界點，球形機器人定義如下：

((z-z0)/rz)s/s1=1,

(17)

x=rxcoss1(t1)coss2(t2),

(18)

y=rycoss1(t1)coss2(t2),-π/2≤t1≤π/2,

(19)

z=rzcoss1(t1),0≤t1≤2π.

(20)

基于SQP算法，路徑規(guī)劃問題轉化為半無限約束優(yōu)化問題。假設空間中有n個障礙物，則j個障礙物的表面可以表示為[20]：

h1(x,y,z)=1,j=1,2,…,n.

(21)

整個可用空間可表示為：

1-h1(x,y,z)≤0,j=1,2,…,n.

(22)

沒有碰撞的必要和充分條件是曲面上所有點都必須無碰撞，獲得無碰撞的充分條件是：

1-hj(xloc,yloc,zloc)≤1,

(23)

其中：

xloc=x(x0,y0,z0,θ1,θ2,θ3,v)

yloc=y(x0,y0,z0,θ1,θ2,θ3,v)

zloc=z(x0,y0,z0,θ1,θ2,θ3,v).

(24)

在路徑規(guī)劃中，配置變量需要一個約束，如公式(25)所示[21-22]：

x0l≤x0≤x0u,y0l≤y0≤y0u,z0l≤z0≤z0u,

θ1l≤θ1≤θ1u,θ2l≤θ2≤θ2u,θ3l≤θ3≤θ3u.

(25)

其中(x0l,y0l,z0l)和(x0u,y0u,z0u)分別是上限和下限,推導得到：

1-hj(rxcoss1(t1)coss2(t2)(cos(θ1)cos(θ2)cos(θ3)-sin(θ1)sin(θ3))-

rycoss1(t1)sins2(t2)(cos(θ1)sin(θ3)-cos(θ2)sin(θ2)sin(θ3))+rzsins1(t1)(cos(θ1)sin(θ2))+x0,

rxcoss1(t1)coss2(t2)(-cos(θ1)cos(θ2)sin(θ3)-sin(θ1)cos(θ3))+

rycoss1(t1)sins2(t2)(sin(θ1)cos(θ2)sin(θ3)-cos(θ1)sin(θ3))+rzsins1(t1)sin(θ2)sin(θ3)+y0,

rxcoss1(t1)coss2(t2)cos(θ1)sin(θ2)+rycoss1(t1)sins2(t2)sin(θ1)sin(θ2)+rzsins1(t1)cos(θ2)+z0≤0.

(26)

在方程中，-π/2≤t1≤π/2,0≤t2≤2π。

在非線性規(guī)劃問題中，目標函數(shù)必須具有下限的最小值。目標設置必須使全局最小值可變，二次函數(shù)的目標函數(shù)表示為：

f(x0,y0,z0,θ1,θ2,θ3)=w((x0-x0g)2+

(y0-y0g)2+(z0-z0g)2)+(1-w)((θ1-θ1g)2+

(θ2-θ2g)2+(θ3-θ3g)2).

(27)

其最優(yōu)化點的坐標為(x0g,y0g,z0g)，最優(yōu)化角度為(θ1g,θ2g,θ3g)，并且滿足方程式：

minf(x0,y0,z0,θ1,θ2,θ3)=

(x0g,y0g,z0g,θ1g,θ2g,θ3g)=0.

(28)

在前面的公式中，w表示用于調整空間位置(x0-x0g)2+(y0-y0g)2+(z0-z0g)2和機器人空間方向(θ1-θ1g)2+(θ2-θ2g)2+(θ3-θ3g)2之間的相對關系的權重，當w=1時計算空間位置。當w=0.5時，空間位置和空間的權重方向必然相等。

5 實驗及結果分析

5.1 仿真實驗

為驗證兩種基于路徑規(guī)劃的GCOP和SQP在水下兩棲機器人的路徑規(guī)劃過程中的有效性，使用MATLAB設置了兩種算法，從初始起始坐標Ol=(-200,-200,-200)到達目標Ou=(600,600,600)的仿真實驗的場景范圍。場景空間的長、寬和高為600 cm的立方體形狀。 基于SQP算法的不等式Ol≤O≤Ou通過四個參數(shù)確保生成的路徑在場景內，坐標分布分別在三維平面上的Os=(x0s,y0s,z0s)，θs=(θ1s,θ1s,θ1s)，Og=(x0g,y0g,z0g) 和θg=(θ1g,θ2g,θ3g) 。GCOP算法的初始時刻設定t=0，Δv=0.000 1，起點坐標為：

(x0s,y0s,z0s,θ1s,θ2s,θ3s)=

(-200,-200,-200,0,0,0),

(29)

(x0g,y0g,z0g,θ1g,θ2g,θ3g)=

(500,500,500,0,0,0).

(30)

目標函數(shù)定義為：

f=ω[(x0-500)2+(y0-500)2+

(31)

如果滿足等式，則該坐標點不會與球形障礙物碰撞。在模擬實驗中設置了4個球形障礙物，為確保基于兩種算法的準確性。4個障礙物的坐標和方程定義為：

(x-200)2+(y-200)2+(z-200)2=502，

x2+(y-200)2+(z-200)2=302，

(x-400)2+(y-200)2+(z-200)2=302，

(x-200)2+y2+(z-200)2=302.

(32)

球形機器人的參數(shù)為(rx,ry,rz)是對象的幾何間隔(s1=1,s2=1.5 表示機器人的形狀是球形)：

rx=5,ry=4,rz=3,s1=1,s2=1.5.

(33)

圖5為仿真結果(彩圖見期刊電子版)，在起點和目標點之間有4個障礙。粉色球和灰色球表示不同算法的兩棲機器人的運動軌跡。3個藍色球體和1個紅色球體分別代表障礙物。

圖5 兩種路徑規(guī)劃算法仿真分析Fig.5 Simulation analysis of two path planning algorithms

機器人從起始位置到目標位置經(jīng)過4個障礙物，兩種算法比較，SQP算法在路徑規(guī)劃上更加合理，并在3個采樣點處(黃色圓點)離障礙物始終保持了安全距離。但GCOP算法選擇的路徑在坐標點(-100，190，332.5)處離障礙物較近，如機器人發(fā)生小角度偏移，有發(fā)生碰撞的危險。所以SQP算法比GCOP算法在安全路徑規(guī)劃的路徑控制方面更加有效。而在達到目標坐標位置上，兩種算法的誤差 Δd=167.5 mm，因此SQP算法在水下路徑規(guī)劃上更具有優(yōu)勢。

5.2 水下實驗

為了進一步驗證文章所提出算法的有效性，本文設計了基于SQP算法的水下測試實驗，實驗是在長1 500 mm,寬為1 000 mm,高度為800 mm的封閉水池環(huán)境下進行，如圖6所示。圖中設置了跟仿真環(huán)境相匹配的4個球形障礙物，分別設計障礙物的固定坐標位置。機器人從起點出發(fā)，激光測距模塊和圖像采集傳感器實時采集障礙物在水中的坐標，并實時調整機器人的運動軌跡，按算法程序中設定的最佳路徑移動到目標位置。圖7(a)是t=0 s時刻機器人的初始位置，箭頭標明了規(guī)劃的機器人最優(yōu)路徑軌跡。

圖6 球形機器人避障實驗環(huán)境Fig.6 Experimental setup for avoiding obstacles of robot

圖7 SQP路徑規(guī)劃算法的實驗圖片F(xiàn)ig.7 Photoes of path planning by SQP algorithm

圖7(b)～7(i)是機器人從t=0 s移動至t=15 s的實際運動軌跡。在實驗過程中，采集6個不同的時間點，對機器人y軸和z軸方向的位移變化曲線進行采樣，從而判斷機器人在最佳路徑選擇上的穩(wěn)定性和可靠性。從圖8可以看出，機器人分別在第二個和第三個采樣點偏移誤差較大，誤差值達到20 mm，在其余4個采樣點誤差小于10 mm，這是因為機器人在第二個到第三個采樣點主要是對路徑的選擇階段，從而影響了機器人的運動形態(tài)。機器人的運動軌跡如圖9所示。

圖8 球形機器人y軸和z軸運動方向誤差Fig.8 Kinematic errors of robot in y-axis and z-axis

圖9 球形機器人的最佳路徑軌跡Fig.9 Optimal path trajectory of spherical robot

6 結 論

本文提出了兩種基于路徑規(guī)劃的算法(GCOP和SQP),通過設置4個不同尺寸的球形障礙物使兩棲球形機器人可以通過視覺伺服傳感器實現(xiàn)避障功能并選擇最佳路徑。在實驗中，設置3個采樣點對兩種路徑規(guī)劃算法進行評估，基于GCOP算法在(-100,190,500)和(-5,200,345.8)處距離障礙物較近；而基于SQP算法在相對應的采集點處坐標分別為(-100,190,500)和(-160,190,500)，相對處于離障礙物較為安全和合理的位置。在終點坐標位置，基于GCOP算法到達預計終點坐標為(500,500,332.5)，偏離了預先設定的終點坐標，而基于SQP算法到達終點的坐標為(500,500,500)，基本達到預先路徑規(guī)劃的要求。通過3個坐標點的數(shù)據(jù)顯示分析，兩個采樣點的坐標偏差最優(yōu)化是要保證距離障礙物的距離在合理范圍之內，SQP算法在兩個采集點處的障礙物距離更加合理。在第三個終點采樣點，兩種算法的機器人運動軌跡偏差為167.5 mm。實驗表明，對于兩棲水下機器人的水下運動控制，基于SQP的路徑規(guī)劃算法比GCOP的路徑規(guī)劃算法更具有優(yōu)越性。

未來的研究工作會在機械設計和控制方法上持續(xù)改進，以實現(xiàn)多機器人的水下多機協(xié)作控制與路徑規(guī)劃的最優(yōu)控制。