改進(jìn)人工勢(shì)場(chǎng)法的水陸兩棲機(jī)器人路徑規(guī)劃*

馬文宇，許云猛，王玉甲

(哈爾濱工程大學(xué)機(jī)電工程學(xué)院，哈爾濱 150001)

0 引言

水陸兩棲機(jī)器人能夠在海洋、陸地及海陸過渡環(huán)境下工作，幫助人類執(zhí)行搜索、監(jiān)測(cè)、運(yùn)輸?shù)热蝿?wù)，是目前人類在近海域研究領(lǐng)域內(nèi)的重點(diǎn)之一[1]。由于水陸兩棲機(jī)器人工作時(shí)的復(fù)雜環(huán)境，其路徑規(guī)劃研究成為兩棲機(jī)器人智能控制的核心問題之一。

在現(xiàn)有的路徑規(guī)劃算法當(dāng)中，人工勢(shì)場(chǎng)法是一種運(yùn)算量小，算法結(jié)構(gòu)簡(jiǎn)單的方法，常被用于各種無人平臺(tái)的路徑規(guī)劃中。但傳統(tǒng)人工勢(shì)場(chǎng)法存在兩個(gè)問題：目標(biāo)不可達(dá)與局部平衡[2]。姚靖靖等[3]通過對(duì)障礙物作用范圍進(jìn)行限制和添加虛擬目標(biāo)的方法，使機(jī)器人規(guī)避障礙物，解決到達(dá)不了目標(biāo)點(diǎn)的問題；劉忠等[4]結(jié)合模擬退火法解決局部陷入最小值的問題；GEVA等[5]在斥力勢(shì)場(chǎng)中加入動(dòng)態(tài)的斥力因子來解決目標(biāo)不可達(dá)問題；SUN等[6]為克服目標(biāo)不可達(dá)問題，設(shè)置動(dòng)態(tài)步長(zhǎng)，減小障礙物斥力影響；ZHANG等[7]利用概率論知識(shí)重新建立勢(shì)場(chǎng)模型來解決目標(biāo)不可達(dá)的問題。相較于單一環(huán)境下工作的無人平臺(tái)，水陸兩棲機(jī)器人工作在二維疊加三維的復(fù)雜環(huán)境中，傳統(tǒng)人工勢(shì)場(chǎng)法在處理三維環(huán)境路徑規(guī)劃方面還少有研究。

本文提出利用改進(jìn)人工勢(shì)場(chǎng)法對(duì)水陸兩棲機(jī)器人進(jìn)行路徑規(guī)劃，建立改進(jìn)的人工勢(shì)場(chǎng)與機(jī)器人運(yùn)動(dòng)相結(jié)合。通過調(diào)整斥力勢(shì)場(chǎng)解決目標(biāo)不可達(dá)問題，通過改變斥力作用方向來突破局部受力平衡。機(jī)器人水下運(yùn)動(dòng)時(shí)，通過制定水下避障策略，將機(jī)器人在三維環(huán)境中的路徑規(guī)劃轉(zhuǎn)化為平面內(nèi)的運(yùn)動(dòng)研究。最后利用MATLAB對(duì)傳統(tǒng)與改進(jìn)兩種方法進(jìn)行仿真對(duì)比，驗(yàn)證本文所提方法對(duì)于水陸兩棲機(jī)器人路徑規(guī)劃研究的有效性。

1 環(huán)境建模

1.1 二維環(huán)境

水陸兩棲機(jī)器人在陸地上運(yùn)動(dòng)屬于二維環(huán)境中水平面運(yùn)動(dòng)，建立其二維笛卡爾坐標(biāo)系，如圖1所示。

將機(jī)器人的目標(biāo)點(diǎn)和障礙物信息在該坐標(biāo)系中進(jìn)行描述，機(jī)器人位于坐標(biāo)系原點(diǎn)(0,0)，目的地坐標(biāo)設(shè)為(X0,Y0)；障礙物分布在機(jī)器人的任務(wù)環(huán)境中，其坐標(biāo)位置設(shè)為(Xi,Yi)。

機(jī)器人在陸上屬于在二維空間中運(yùn)動(dòng)，依靠機(jī)器人自身攜帶的雙目視覺系統(tǒng)檢測(cè)障礙物，當(dāng)檢測(cè)到前方存在障礙物時(shí)，機(jī)器人開始減速，繞開障礙物進(jìn)行避障，通過障礙物后返回原本航線，為保證機(jī)器人運(yùn)動(dòng)過程中的安全，對(duì)機(jī)器人和障礙物進(jìn)行處理，如圖2所示。

圖1 二維環(huán)境坐標(biāo)系 圖2 障礙物膨脹化處理

對(duì)障礙物進(jìn)行膨脹處理是進(jìn)行路徑規(guī)劃時(shí)一種常見的數(shù)學(xué)建模手段[8]。如果將障礙物以原形進(jìn)行規(guī)劃計(jì)算，會(huì)增加規(guī)劃時(shí)間，降低工作效率且易增加碰撞的風(fēng)險(xiǎn)。文中將障礙物進(jìn)行圓形處理，將障礙物看做一定尺寸的圓，便于進(jìn)行數(shù)學(xué)計(jì)算和建立數(shù)學(xué)模型，在圓形包圍處理后得到的工作環(huán)境模型中，可將該圓看作是障礙物影響范圍。經(jīng)處理后的機(jī)器人和障礙物的尺寸半徑為Rr和Ro?？紤]到機(jī)器人在運(yùn)動(dòng)過程中存在運(yùn)動(dòng)誤差及保證機(jī)器人運(yùn)動(dòng)安全性，對(duì)兩個(gè)半徑乘以一定的安全系數(shù)變?yōu)镽r1和Ro1，Rd表示機(jī)器人與障礙物的中心距離，是最終的障礙物半徑，其大小為：

Rd=Rr1+Ro1

(1)

膨脹化處理后的障礙物影響范圍是一個(gè)以Rd為半徑的圓。與此同時(shí)，經(jīng)膨脹化處理后的機(jī)器人可以看作是一個(gè)點(diǎn)，經(jīng)規(guī)劃出來的路徑是由一系列點(diǎn)構(gòu)成的折線。路徑規(guī)劃過程就是規(guī)劃一系列路徑點(diǎn)的過程，并且連續(xù)兩點(diǎn)之間的連線不能與經(jīng)膨脹化處理后障礙物圓相交。

1.2 三維環(huán)境

利用仿真軟件建立三維環(huán)境模型，如圖3所示。通過仿真軟件對(duì)水底地貌進(jìn)行描述，模擬水陸兩棲機(jī)器人的水下工作環(huán)境，水平面將整個(gè)工作區(qū)域劃分為水下部分和陸上部分。路徑規(guī)劃在可行區(qū)域內(nèi)進(jìn)行，在進(jìn)行路徑規(guī)劃時(shí)要避開障礙物，確保規(guī)劃出來的路徑是安全的。

機(jī)器人在水下屬于在三維空間中運(yùn)動(dòng)，工作環(huán)境更為復(fù)雜，遇到的障礙物主要可分為小型和大型兩種，水下的大型障礙物一般為礁石、大型海底廢棄物等，若機(jī)器人仍像陸上避障一樣選擇繞行，會(huì)增加機(jī)器人的航行距離，增大推進(jìn)器功耗。因此，當(dāng)機(jī)器人在水下遇到大型障礙物時(shí)，采取垂直面避障[9]，選擇翻越障礙物，避障示意圖如圖4所示。

圖3 三維環(huán)境模型 圖4 避障示意圖

機(jī)器人依靠自身攜帶的雙目視覺系統(tǒng)和避碰聲吶來檢測(cè)障礙物，當(dāng)檢測(cè)到障礙物時(shí)，機(jī)器人上浮到障礙物上方，保持一定航行高度通過障礙物。通過對(duì)水下避障策略的制定，可以限制機(jī)器人在水下時(shí)只有前行、后退及沉浮動(dòng)作，因此可將對(duì)機(jī)器人在水下三維環(huán)境中的路徑規(guī)劃轉(zhuǎn)換為對(duì)機(jī)器人在豎直平面內(nèi)的運(yùn)動(dòng)研究。

2 人工勢(shì)場(chǎng)法

2.1 傳統(tǒng)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法被應(yīng)用到各種無人設(shè)備的路徑規(guī)劃研究當(dāng)中[10]，利用“同性相斥，異性相吸”的物理原理來描述目標(biāo)點(diǎn)和障礙物在機(jī)器人運(yùn)動(dòng)過程中對(duì)其產(chǎn)生的作用[11]，目標(biāo)點(diǎn)作為“異性”對(duì)機(jī)器人具有吸引作用，引力隨二者之間距離的增大而增大，引力勢(shì)場(chǎng)函數(shù)可以用式(2)表示：

(2)

式中，k為引力系數(shù)且k>0；X為機(jī)器人的位置向量；XT為目標(biāo)點(diǎn)的位置向量。

障礙物作為“同性”對(duì)機(jī)器人具有排斥作用，斥力隨二者之間距離的增大而減小，斥力勢(shì)場(chǎng)函數(shù)用式(3)表示為：

(3)

式中，kr為斥力系數(shù)且kr>0；ρ為障礙物影響范圍；XO為障礙物的位置向量。

在兩個(gè)勢(shì)場(chǎng)函數(shù)的作用下機(jī)器人的綜合勢(shì)場(chǎng)函數(shù)用式(4)表示為：

(4)

機(jī)器人在綜合勢(shì)場(chǎng)的作用下不斷運(yùn)動(dòng)，可由式(5)推導(dǎo)每次運(yùn)動(dòng)后的下一個(gè)位置。

(5)

式中，(xk,yk)為當(dāng)前位置；(xk+1,yk+1)為下一步位置；l為運(yùn)動(dòng)步長(zhǎng)；θ為勢(shì)場(chǎng)合力與x軸的正向夾角。

2.2 改進(jìn)人工勢(shì)場(chǎng)法

(1)目標(biāo)不可達(dá)問題。使用傳統(tǒng)人工勢(shì)場(chǎng)法時(shí)，隨著機(jī)器人不斷地向指定目標(biāo)點(diǎn)運(yùn)動(dòng)，引力作用減弱，當(dāng)存在于目標(biāo)點(diǎn)周圍的障礙物產(chǎn)生的斥力作用高于引力作用時(shí)，會(huì)導(dǎo)致機(jī)器人運(yùn)動(dòng)不到目標(biāo)點(diǎn)[12]。若想解決此問題，需要降低斥力產(chǎn)生的影響[13]，從斥力勢(shì)場(chǎng)入手，對(duì)斥力勢(shì)場(chǎng)進(jìn)行改進(jìn)，改進(jìn)后的勢(shì)場(chǎng)函數(shù)用式(6)表示為：

(6)

式中，(X-XG)是機(jī)器人與目標(biāo)點(diǎn)之間的相對(duì)距離；n是任意正實(shí)數(shù)。對(duì)上式求其負(fù)梯度可得到斥力表達(dá)式為：

(7)

通過式(7)將斥力分解為2個(gè)分量Frep1和Frep2，2個(gè)分量的表達(dá)式如式(8)所示：

(8)

圖5 機(jī)器人受力分析

在斥力分解后的兩個(gè)分量和引力作用下機(jī)器人的受力分析如圖5所示，由圖可看出，新的合力指向目標(biāo)點(diǎn)，機(jī)器人在改進(jìn)的人工勢(shì)場(chǎng)作用下可以繼續(xù)逼近目標(biāo)點(diǎn)。

改進(jìn)的斥力勢(shì)場(chǎng)函數(shù)中的n的取值，若0

(9)

隨著機(jī)器人向目標(biāo)點(diǎn)的不斷運(yùn)動(dòng)，相對(duì)距離(X-XG)趨于0，引力Fatt和Frep1趨于0，改進(jìn)斥力勢(shì)場(chǎng)中機(jī)器人指向目標(biāo)點(diǎn)的分量Frep2趨于無窮大，機(jī)器人不斷向目標(biāo)點(diǎn)移動(dòng)。

若n=1：

(10)

相對(duì)距離(X-XG)趨于0，引力Fatt和Frep1趨于0，改進(jìn)斥力勢(shì)場(chǎng)中機(jī)器人指向目標(biāo)點(diǎn)的分量Frep2是大于0的正數(shù)，合力大于0，機(jī)器人不斷向目標(biāo)點(diǎn)移動(dòng)。

若n>1：

(11)

隨著機(jī)器人向目標(biāo)點(diǎn)的逼近，引力Fatt和改進(jìn)斥力勢(shì)場(chǎng)下的兩個(gè)分量Frep1和Frep2均趨于0，機(jī)器人朝目標(biāo)點(diǎn)移動(dòng)，最后到達(dá)目標(biāo)點(diǎn)。

由此可看出無論n取何值，機(jī)器人都能向目標(biāo)點(diǎn)不斷運(yùn)動(dòng)，解決目標(biāo)不可達(dá)問題。

(2)局部受力平衡問題。機(jī)器人運(yùn)動(dòng)過程中，在某些特殊位置會(huì)遇到引力與斥力相等的情況，如3者共線或機(jī)器人行進(jìn)過程中存在多個(gè)障礙物，此時(shí)兩棲機(jī)器人受到的合力為0，導(dǎo)致機(jī)器人局部平衡，停止運(yùn)動(dòng)，不能達(dá)到指定目標(biāo)點(diǎn)，即是傳統(tǒng)人工勢(shì)場(chǎng)法存在的局部平衡問題[14-15]。

此時(shí)若給機(jī)器人施加一個(gè)外力，便可打破受力平衡狀態(tài)，突破局部所受合力為0的局面[13]。通過調(diào)整障礙物產(chǎn)生的斥力角度，可改變機(jī)器人所受合力，打破原有平衡，進(jìn)而向目標(biāo)點(diǎn)運(yùn)動(dòng)。在上文所研究的改進(jìn)后的斥力勢(shì)場(chǎng)基礎(chǔ)上，當(dāng)機(jī)器人陷入局部所受合力為0情況時(shí)，通過旋轉(zhuǎn)斥力方向來調(diào)整斥力的角度，破壞局部平衡，受力分析如圖6所示。

(a) 3者共線情況 (b) 多障礙物情況

如圖6a所示，當(dāng)機(jī)器人、障礙物和目標(biāo)點(diǎn)共線導(dǎo)致機(jī)器人局部受力為0時(shí)，順時(shí)針旋轉(zhuǎn)90°調(diào)整Frep1的方向，機(jī)器人在新的不為0的合力作用下繼續(xù)向目標(biāo)點(diǎn)運(yùn)動(dòng)；當(dāng)多障礙物導(dǎo)致機(jī)器人局部受力為0時(shí)，如圖6b所示，逆時(shí)針90°旋轉(zhuǎn)調(diào)整障礙物b的斥力Frepb1和障礙物a的斥力Frepa1的方向，使機(jī)器人繼續(xù)向目標(biāo)點(diǎn)運(yùn)動(dòng)。

3 仿真實(shí)驗(yàn)

本節(jié)利用MATLAB仿真軟件對(duì)改進(jìn)后建立的新人工勢(shì)場(chǎng)對(duì)于水陸兩棲機(jī)器人的適用性進(jìn)行驗(yàn)證，并與傳統(tǒng)人工勢(shì)場(chǎng)的結(jié)果進(jìn)行對(duì)比。

3.1 目標(biāo)不可達(dá)問題仿真實(shí)驗(yàn)

如圖7所示為目標(biāo)不可達(dá)問題仿真實(shí)驗(yàn)結(jié)果，機(jī)器人位于(0 m,0 m)，向目標(biāo)點(diǎn)(10 m,10 m)運(yùn)動(dòng)，在目標(biāo)點(diǎn)周圍設(shè)置一個(gè)障礙物，位置為(9.5 m,10 m)，其障礙物圓的影響半徑為2 m，設(shè)置安全距離0.3 m以保障機(jī)器人自身安全。由圖7a使用傳統(tǒng)方法路徑規(guī)劃的仿真結(jié)果可看出，機(jī)器人受障礙物影響并未運(yùn)動(dòng)到指定目標(biāo)點(diǎn)，過大的斥力導(dǎo)致目標(biāo)不可達(dá)問題；由圖7b使用改進(jìn)方法路徑規(guī)劃的仿真結(jié)果可看出改變斥力勢(shì)場(chǎng)后，機(jī)器人最終可到達(dá)指定目標(biāo)點(diǎn)。

(a) 傳統(tǒng)方法路徑規(guī)劃 (b) 改進(jìn)方法路徑規(guī)劃

3.2 局部合力為0仿真實(shí)驗(yàn)

如圖8所示為機(jī)器人、障礙物、目標(biāo)點(diǎn)3者共線導(dǎo)致機(jī)器人局部所受合力為0問題的仿真實(shí)驗(yàn)結(jié)果，機(jī)器人位于(0 m,0 m)，向目標(biāo)點(diǎn)(10 m,10 m)運(yùn)動(dòng)，障礙物位于(9 m,9 m)，其障礙物圓的影響半徑為2 m，設(shè)置安全距離0.3 m以保障機(jī)器人自身安全，由圖8a使用傳統(tǒng)方法路徑規(guī)劃的仿真結(jié)果可看出，3者共線時(shí)，機(jī)器人受局部合力為0影響，運(yùn)動(dòng)過程中便停止運(yùn)動(dòng)，未能抵達(dá)指定目標(biāo)點(diǎn)；調(diào)整斥力角度后的仿真結(jié)果見圖8b，機(jī)器人未出現(xiàn)局部受合力為0停止運(yùn)動(dòng)的情況，最終到達(dá)指定目標(biāo)點(diǎn)。

(a) 傳統(tǒng)方法路徑規(guī)劃 (b) 改進(jìn)方法路徑規(guī)劃

如圖9所示為兩障礙物導(dǎo)致機(jī)器人局部所受合力為0問題的仿真實(shí)驗(yàn)結(jié)果，障礙物坐標(biāo)分別為(7.5 m,8.5 m)和(8.5 m,7.5 m)，分布在機(jī)器人與目標(biāo)點(diǎn)連線兩側(cè)，由圖9a利用傳統(tǒng)方法進(jìn)行路徑規(guī)劃的仿真結(jié)果可看出，在兩個(gè)障礙物的影響下出現(xiàn)局部合力為0停止運(yùn)動(dòng)的情況；調(diào)整斥力角度后，機(jī)器人能夠穿越障礙物繼續(xù)運(yùn)動(dòng)，仿真結(jié)果如圖9b所示。

(a) 傳統(tǒng)方法路徑規(guī)劃 (b) 改進(jìn)方法路徑規(guī)劃

為進(jìn)一步驗(yàn)證改進(jìn)后的人工勢(shì)場(chǎng)對(duì)水陸兩棲機(jī)器人路徑規(guī)劃的適用性，隨機(jī)增加障礙物的數(shù)量，且位置隨機(jī)布置，仿真結(jié)果如圖10所示，可以看出，當(dāng)存在多個(gè)隨機(jī)位置的障礙物時(shí)，改進(jìn)后的人工勢(shì)場(chǎng)仍適用于機(jī)器人的路徑規(guī)劃，未出現(xiàn)局部合力為0導(dǎo)致機(jī)器人中途停止的情況。

(a) 4障礙物情況 (b) 7障礙物情況

3.3 三維環(huán)境仿真實(shí)驗(yàn)

水陸兩棲機(jī)器人除在陸地上二維環(huán)境中運(yùn)動(dòng)，進(jìn)入水中后還要在三維環(huán)境中運(yùn)動(dòng)，針對(duì)改進(jìn)人工勢(shì)場(chǎng)法對(duì)其三維環(huán)境路徑規(guī)劃進(jìn)行仿真實(shí)驗(yàn)。依靠水陸兩棲機(jī)器人自身攜帶的視覺系統(tǒng)和避碰聲吶進(jìn)行障礙物檢測(cè)，采用遇小型障礙物時(shí)繞行避障，遇礁石等大型障礙物時(shí)上浮翻越避障的策略，機(jī)器人由陸地向水下運(yùn)動(dòng)時(shí)，機(jī)器人先在水面行駛一段時(shí)間，隨即開始下潛到一定深度，避免水面波浪影響。當(dāng)運(yùn)動(dòng)到近海岸時(shí)，機(jī)器人與海岸開始接觸，履帶模塊開始工作，通過自身履帶模塊沿海岸行駛出水面。在三維模型中，整個(gè)空間的大小為1000×1000×30 m，z=0為水平面，起始點(diǎn)坐標(biāo)(100 m,900 m,0 m)，目標(biāo)點(diǎn)坐標(biāo)(900 m,100 m,3 m)，水下航行時(shí)設(shè)置定深5 m，仿真結(jié)果如圖11所示。

(a) 障礙物與水平面相距較遠(yuǎn)情況 (b) 障礙物與水平面相距較近情況

由圖11a可看出當(dāng)機(jī)器人檢測(cè)到前方存在障礙物時(shí)，機(jī)器人開始上浮，上浮到一定高度翻越障礙物后再次回到定深航向軌跡中，最后沿著海岸通過履帶模塊駛出水面；當(dāng)障礙物與水平面相距較近時(shí)，機(jī)器人則浮出水面，在水平面上行駛通過障礙物，隨后再潛入水面，返回定深航線，如圖11b所示。

根據(jù)仿真實(shí)驗(yàn)結(jié)果可知，改進(jìn)人工勢(shì)場(chǎng)法可有效應(yīng)用于水陸兩棲機(jī)器人水下三維環(huán)境中運(yùn)動(dòng)，能夠規(guī)劃出一條有效且合理的路徑。

4 結(jié)論

為滿足水陸兩棲機(jī)器人在陸上及水下能夠規(guī)避各類障礙物并到達(dá)指定目標(biāo)點(diǎn)的需求，本文提出利用改進(jìn)人工勢(shì)場(chǎng)法對(duì)其進(jìn)行路徑規(guī)劃。在傳統(tǒng)方法的基礎(chǔ)上重新建立斥力勢(shì)場(chǎng)，使機(jī)器人所受合力的方向始終指向目標(biāo)點(diǎn)，解決目標(biāo)不可達(dá)問題；旋轉(zhuǎn)斥力角度，改變其作用方向，使機(jī)器人不受障礙物位置及數(shù)量影響，解決局部平衡問題。同時(shí)，通過制定機(jī)器人水下避障策略，將其在水下三維環(huán)境中的路徑規(guī)劃轉(zhuǎn)換為在二維垂直平面內(nèi)的運(yùn)動(dòng)研究。最后，利用MATLAB對(duì)改進(jìn)人工勢(shì)場(chǎng)法進(jìn)行仿真實(shí)驗(yàn)，并與傳統(tǒng)方法的仿真結(jié)果進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果表明本文所研究方法可有效規(guī)劃水陸兩棲機(jī)器人在二維及三維環(huán)境中的無碰路徑，目前此方法已成功應(yīng)用于實(shí)驗(yàn)室自制水陸兩棲機(jī)器人中，可為今后水陸兩棲機(jī)器人的開發(fā)及其路徑規(guī)劃研究提供有益參考。