張鵬鑫,于海生,李 哲,孟祥祥

(青島大學 a.自動化學院;b.山東省工業(yè)控制技術重點實驗室,青島 266071)

0 引言

針對存在障礙物的場景,機器人如何進行避障[1-3]是一個關鍵問題,國內外研究人員對其進行研究并提出一些規(guī)劃算法,比如快速擴展隨機樹(RRT)算法[4],Dijkstra算法[5],PRM算法[6]以及蟻群算法[7]等。LIU等[8]提出一種時間最優(yōu)的快速探索隨機樹算法,滿足無碰撞的同時提高了規(guī)劃速度。JIANG等[9]將RRT算法與人工勢場法相結合,利用環(huán)境和節(jié)點信息使得機器人在短時間內擴展到目標區(qū)域,并在實驗平臺上證明了方法的有效性。李哲等[10]提出一種擴展點選擇策略,并優(yōu)化了隨機樹的分支,提高了計算效率。

針對目標定位問題,國內外研究人員提出一些視覺伺服[11-13]技術并成功應用于機器人控制。使用最為廣泛的視覺伺服算法分為基于位置的視覺伺服(PBVS)[14]和基于圖像的視覺伺服(IBVS)[15]。CHEN等[16]在傳統(tǒng)的PBVS的基礎上引入模糊PID控制,隨著位置誤差變化,自適應調整PID參數,提高系統(tǒng)的快速性。KRISHNAN等[17]將神經網絡與IBVS結合,經過訓練的網絡根據給定的圖像特征調整機器人的姿態(tài),實現精確抓取。LI等[18]提出一種基于PD-滑?？刂破鞯腎BVS控制器,通過仿真與實驗驗證了相比于傳統(tǒng)IBVS器控制具有更好的定位精度。

結合以上分析,本文設計了一種便于算法快速收斂的自適應步長和自適應隨機概率策略,并使用五次多項式插值對采集到的無碰撞路徑點擬合出一條連續(xù)光滑的路徑減少機器人運行過程中的抖動。在視覺伺服中,設計了一種基于積分滑模的視覺伺服控制器,使得機器人能夠快速且精準地跟蹤上移動中的目標。在進行理論推導后,對六自由度機器人進行仿真與實驗分析,并與已有算法進行比較,進一步驗證了所提算法的有效性與優(yōu)越性。

1 問題描述

1.1 傳統(tǒng)RRT算法原理

傳統(tǒng)RRT算法的思想是以樹的形式快速地在空間中生長出大量分支,尋找出一條可以到達的終點的路徑。其基本步驟是:

步驟1:創(chuàng)建樹Tree,并初始化起點qstart、終點qend和步長Step;

步驟2:在空間中隨機找一點qrand;

步驟3:尋找樹上與qrand最近的點qnear;

步驟4:按照指定步長生成新點qnew;

步驟5:判斷新點是否可行,可行添加至樹中,否則放棄此點;

步驟6:若找到終點,跳出程序,否則返回至步驟2繼續(xù)尋找路徑;

以上每一步對應的示意圖如圖1所示。

圖1 傳統(tǒng)RRT算法示意圖

雖然RRT算法已經被應用在機器人路徑規(guī)劃領域,但是該算法容易陷入局部最小值且導向性差。本文針對以上缺陷在傳統(tǒng)RRT算法的基礎上進行相應的改進。

1.2 機器人視覺伺服模型

IBVS的反饋信號是圖像特征,沒有經過繁瑣的位姿估計,影響控制精度的是特征提取的精度。其實現原理如圖2所示。

圖2 傳統(tǒng)IBVS控制原理

用圖像雅可比矩陣描述機器人的手眼映射關系,具體公式為:

(1)

為了反映攝像機的運動速度與機器人關節(jié)速度的關系,建立機器人雅可比矩陣,其公式為:

(2)

(3)

雖然傳統(tǒng)IBVS具有計算效率高,無需經過位姿估計,但是對外部擾動和環(huán)境變化較為敏感。本文針對該缺點設計了基于積分滑模的視覺伺服控制器。

2 自適應RRT與視覺伺服控制器設計

2.1 自適應RRT設計

為了避免繁瑣的機器人逆運動學求解,本算法在機器人的關節(jié)空間下進行設計。

2.1.1 自適應采樣概率策略

根據傳統(tǒng)RRT算法的步驟2,qrand由人為規(guī)定的概率隨機產生,但是每次生成的qrand并沒有考慮環(huán)境因素,隨機性強,這無疑增加了計算成本。在本節(jié)中,考慮每次生成的qnew是否導致機器人本體與障礙物發(fā)生碰撞,來自動調整生成qrand的概率。設計概率計算公式為:

(4)

式中:P為采樣概率,noCol為無碰撞標志,s為當前無碰撞次數,i為當前迭代次數。

該算法所計算出的隨機采樣概率與無碰撞次數具有非常重要的關系,如果無碰撞的次數越多,代表機器人距離障礙物越遠,那么采樣概率P越大,可以讓機器人無條件地向著終點方向生長路徑點,達到快速收斂的目的。

2.1.2 自適應步長策略

根據傳統(tǒng)RRT算法的步驟4,由于定步長的影響,較長的步長容易碰撞到障礙物,較短步長使得搜索路徑的時間增加,不具備隨著環(huán)境調整的能力。本節(jié)根據以上缺點提出自適應步長策略。

首先人為定義初始步長step,然后以該步長進行擴展新點:

(5)

然后根據機器人正運動學,得到機器人末端位置fq,并計算出與每個障礙物表面的距離:

(6)

式中:disj為機器人末端與第j個障礙物的距離,fj為障礙物的中心點,rj為障礙物的半徑。

為了方便計算,將dis進行歸一化處理:

(7)

式中:dj為距離歸一化后的結果。取dj中的最小值,代入式(8),得出自適應步長結果:

(8)

根據以上設計的策略,得出自適應RRT算法流程如圖3所示。

圖3 自適應RRT算法流程圖

2.1.3 軌跡優(yōu)化

由于RRT算法得到的是一系列離散的關節(jié)角度,這就使得機器人在運行過程中抖動劇烈,不利于機器人平穩(wěn)運行。為此,采用五次多項式插值算法,將離散的點擬合成平滑曲線,減少機器人在運行過程中的抖動。

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(9)

2.2 基于積分滑模的視覺伺服控制器設計

當機器人運行至工作空間時,需要對目標進行跟蹤并抓取,控制框圖如圖4所示。

圖4 基于積分滑模的視覺伺服控制框圖

2.2.1 公式推導

圖像特征點的速度與圖像誤差有關,因此可以得出傳統(tǒng)的視覺伺服控制器為:

(10)

(11)

式中:e=s*-s,s*=[u*,v*]?？紤]式(11)的控制器對參數攝動敏感,本節(jié)提出了一種基于積分滑模的視覺伺服控制器。定義滑模面:

(12)

(13)

(14)

設a=0.1,δ=0.1,σ∈[-5,5],符號函數與非線性函數對比如圖5所示。

圖5 符號函數與非線性函數對比

從圖中可以看出,當|σ|>δ時,非線性函數的絕對值更大,因此改進的趨近律速度更快;當|σ|≤δ時,非線性函數的波動明顯更小更平滑,相比符號函數具有更小的抖振。

對式(12)求導得:

(15)

聯立式(11)和式(15)得出控制律:

u=kJ+[-kpe-kσfal(σ,a,δ)-keσ]

(16)

式中:k,kp,kσ,ke≥0。

2.2.2 穩(wěn)定性證明

設計 Lyapunov 函數為:

(17)

對式(17)求導,將式(13)代入:

(18)

3 仿真與實驗分析

3.1 路徑規(guī)劃仿真

在本節(jié)的仿真中,利用機器人工具箱導入ABB IRB120機器人模型,其DH坐標系如表1所示。分別在三維坐標(0.18,-0.35,0.1)、(0.22,-0.25,0.355)、(0.28,0.05,0.15)、(-0.22,-0.2,0.21)建立4個直徑為0.07 m的球形障礙物,并設置機器人起始關節(jié)角度為(-96.1°,-2.6°,23.2°,0°,69.2°,0°),終點關節(jié)角度為(45.3°,10.6°,0.8°,0°,78.8°,0°)。

表1 IRB120 DH參數

將傳統(tǒng)RRT算法、文獻[10]中的RRT算法以及本文提出的自適應RRT算法對比,說明該算法的優(yōu)越性。3種算法對應的搜索節(jié)點和機器人路徑如圖6～圖8所示。

(a) 路徑搜索節(jié)點 (b) 機器人運動路徑圖6 傳統(tǒng)RRT算法

根據3種算法的對比結果可以看出,本文提出的算法相比另外兩種算法具有明顯的運動路徑短,搜索節(jié)點少等優(yōu)點,使得機器人能夠快速地從起點運動至終點。表2的數據展示了3種算法分別進行20次實驗各項指標的平均結果。

表2 3種算法對比結果

由以上數據可知,自適應RRT算法在路徑規(guī)劃上效果更好。將計算出的路徑點用五次多項式插值算法進行平滑處理,得出運行過程的關節(jié)角度如圖9所示。

圖9 關節(jié)角度

3.2 視覺伺服仿真

在本節(jié)的仿真中,采用MATLAB視覺伺服工具箱中默認的相機模型,參數如表3所示。

表3 相機模型參數

分別在空間坐標系(0.303,0.305,0.05)、(0.203,0.205,0.05)、(0.203,0.205,0.05)、(0.303,0.305,0.05)設置4個特征點。設計積分滑?？刂破鲄禐閗p=2,kσ=0.1,ke=0.001,δ=0.15,a=0.18。為了說明本文提出的視覺伺服的可靠性,將其與傳統(tǒng)視覺伺服進行對比,如圖10和圖11所示。

(a) 關節(jié)速度 (b) 特征點誤差圖10 傳統(tǒng)視覺伺服

通過對兩種控制器的比較,本文所設計的視覺伺服控制器具有更好的快速性,并且在伺服過程中波動更小,機器人能夠更加平穩(wěn)運行。

3.3 實驗平臺驗證

為了進一步驗證以上提出算法的可行性,在維視IRVS200開發(fā)平臺上進行驗證。工作站如圖12所示。主要硬件由工控機、IRB120機器人、工業(yè)相機以及傳送帶組成。

圖12 IRVS機器人工作站

整個實驗分為以下步驟:機器人首先從初始狀態(tài)通過軌跡規(guī)劃運動至傳送帶上方,當到達指定位置時,上位機接收到信號,進入視覺伺服過程;然后相機不斷地讀取圖像信息并發(fā)送給工控機,由工控機進行圖像特征提取,同時將控制信號發(fā)送給機器人控制器,直到機器人抓取到目標,一個實驗流程結束。在本實驗中使用的工業(yè)相機型號為MV-EM120M,其參數如表4所示。

表4 MV-EM120M參數

在圖像坐標系設置的4個期望特征點分別為(170,385)、(200,70)、(565,110)、(530,425)。調用OPENCV圖像處理庫,用矩形將目標包圍,將提取的4個角點坐標與期望的特征點坐標進行比較,利用本文設計的視覺伺服控制器,對機器人姿態(tài)進行校正,直到特征點精度達標。為了說明所設計的視覺伺服控制器的具有良好的跟蹤性能,在3種傳送帶速度下分別進行20次實驗,得出的跟蹤誤差及伺服過程如圖13和圖14所示。

(a) u方向跟蹤誤差 (b) v方向跟蹤誤差圖13 跟蹤誤差

圖14 視覺伺服過程

以上結果表明,機器人成功通過路徑規(guī)劃運動至工作空間,盡管傳送帶的速度發(fā)生變化,特征點的像素誤差始終穩(wěn)定在可以接受的范圍內,使得機器人能夠準確跟蹤目標,最終實現工件抓取與搬運。

4 結論

本文針對傳統(tǒng)RRT算法中采樣節(jié)點冗余、搜索路徑時間長等缺陷,提出一種自適應RRT算法。其中,自適應采樣概率策略可以根據無碰撞次數自動調整生成新點的概率,從而減少計算成本;自適應步長策略可以根據工作的環(huán)境自動調整擴展新點的步長,從而提高機器人的路徑搜索效率。實驗結果表明自適應RRT算法具有搜索時間短、節(jié)點數少等優(yōu)勢。針對傳統(tǒng)視覺伺服在伺服過程具有較大波動、速度較慢等缺陷,提出一種基于積分滑模的視覺伺服控制器?；？刂凭哂许憫俣瓤?、魯棒性強等優(yōu)勢,并在滑模面中引入積分項,用于消除系統(tǒng)跟蹤過程中的穩(wěn)態(tài)誤差。實驗結果表明該控制器能夠精確跟蹤上不同運動速度的目標。以上結果證實了本文提出的算法可以被應用于機器人控制,但是本實驗是在已知障礙物的情況下進行的,所以未來的方向將重點考慮如何躲避空間中未知障礙物。