柏維華，許行之

(桂林理工大學(xué)機(jī)械與控制工程學(xué)院，桂林 541006)

0 引言

隨著自動化技術(shù)的快速發(fā)展，機(jī)器人技術(shù)在各個領(lǐng)域的關(guān)注度越來越高，應(yīng)用場景也極大豐富[1]。實際應(yīng)用中，我們常見到一定數(shù)量的關(guān)節(jié)串聯(lián)機(jī)械臂能夠快速準(zhǔn)確地完成具有一定復(fù)雜度的工作，這對機(jī)械臂的運動控制提出了更高的要求，對機(jī)械臂進(jìn)行路徑規(guī)劃也成了控制機(jī)械臂運動的重要一環(huán)[2-3]。

運動規(guī)劃問題是機(jī)械臂規(guī)劃過程中的基礎(chǔ)。不同的運動規(guī)劃算法適用于解決不同問題?；谒阉鞯倪\動規(guī)劃方法多針對于二維空間中機(jī)器人的運動規(guī)劃問題，通常使用A*[4]、Dijkstra[5]等算法，雖然其搜索能力強但當(dāng)目標(biāo)為機(jī)械臂且其關(guān)節(jié)數(shù)增多、自由度較高時，這些算法的復(fù)雜度會大幅提升，而基于采樣的運動規(guī)劃方法則巧妙地解決了這些弊端。

基于采樣的運動規(guī)劃方法主要針對高維空間下的機(jī)械臂運動規(guī)劃問題?；诓蓸拥倪\動規(guī)劃方法即快速拓展隨機(jī)樹RRT[6-10]，具有概率完整性，快速的探索速度，無需將障礙物從任務(wù)空間映射到配置空間的優(yōu)點。同時，傳統(tǒng)的RRT算法也存在一些不足，例如在隨機(jī)樹搜索過程中的收斂速度較低，導(dǎo)向性不穩(wěn)定，在復(fù)雜環(huán)境中尋跡速度慢且不必要節(jié)點過多以及障礙物約束使其隨機(jī)采樣生成的路徑包含許多不必要的斷點，從而導(dǎo)致路徑不平滑且不連續(xù)等。因此，機(jī)械臂的運動規(guī)劃通常是不穩(wěn)定的。為了解決上述問題，可以在不同層面上做改進(jìn)[11-13]。本文采用結(jié)合目標(biāo)偏置的RRT算法，通過目標(biāo)偏置策略在采樣過程中引導(dǎo)算法以一定概率向目標(biāo)點快速拓展實現(xiàn)盡快抵達(dá)目標(biāo)點的任務(wù)以減少無用區(qū)域搜索，來加速算法收斂并達(dá)到探索空間的目的。最終在其規(guī)劃出來的路徑上采用三次B樣條函數(shù)[14]優(yōu)化其平滑性，來生成機(jī)械臂可跟蹤的光滑路徑。利用軟件仿真得到的數(shù)據(jù)，并對其進(jìn)行分析處理，驗證了所提算法的快速性以及有效性。

1 機(jī)械臂運動學(xué)分析

本文的研究對象是一個6自由度的串聯(lián)機(jī)械臂，如圖1所示。為避免碰撞檢測時，計算機(jī)運算量過大，使用機(jī)械臂連桿的凸包代替原機(jī)械臂連桿的模型。以機(jī)械臂底座坐標(biāo)系作為全局坐標(biāo)系，如圖2所示，機(jī)械臂的各個連桿初始坐標(biāo)系姿態(tài)與機(jī)械臂底座坐標(biāo)系姿態(tài)相同，其中機(jī)械臂底座視為連桿1。

圖1 6自由度機(jī)械臂及其初始姿態(tài)

圖2 機(jī)械臂底座坐標(biāo)系 圖3 機(jī)械臂各關(guān)節(jié)坐標(biāo)系示意圖

以李群和旋量理論[15]為基礎(chǔ)，對各關(guān)節(jié)的旋量坐標(biāo)系進(jìn)行搭建。連桿旋量坐標(biāo)系如圖3所示。慣性坐標(biāo)系S(x1,y1,z1)設(shè)置在機(jī)械臂基座上，工具坐標(biāo)系T(x7,y7,z7)設(shè)置在末端執(zhí)行器上。當(dāng)每個關(guān)節(jié)的角度為0時，機(jī)械臂的初始位形為：

(1)

各連桿坐標(biāo)系原點相對全局坐標(biāo)系位置如表1所示。

表1 各連桿坐標(biāo)系原點相對全局坐標(biāo)系位置

各關(guān)節(jié)螺旋軸原點相對相對全局坐標(biāo)系位置如表2所示。

表2 各關(guān)節(jié)螺旋軸原點相對相對全局坐標(biāo)系位置

(2)

以末端連桿位姿為例，設(shè)其初始值為矩陣gst(0)，各關(guān)節(jié)角變化量為向量θ，則經(jīng)過正運動學(xué)變換后的位姿矩陣為：

gst(θ)=e[ξ1]θ1e[ξ2]θ2…e[ξ6]θ6gst(0)

(3)

2 碰撞檢測

機(jī)械臂的碰撞檢測是機(jī)械臂運動規(guī)劃中的重要組成部分。當(dāng)隨機(jī)拓展樹向外拓展一個都需要碰撞檢測驗證節(jié)點的有效性，這一過程十分消耗CPU的計算資源，影響機(jī)械臂軌跡規(guī)劃的效率。本文通過導(dǎo)入機(jī)械臂的凸包，即能夠包含機(jī)械臂連桿的最小凸多面體，簡化碰撞檢測的難度，提升運算效率。在凸多面體碰撞檢測的過程中，利用GJK算法[16]，檢測凸多面體之間的碰撞情況。

GJK算法通過不斷驗證單純形與零點的關(guān)系判斷兩圖形是否重疊。計算機(jī)想要“看見”兩個圖形重疊的部分，可以利用自身的計算優(yōu)勢，遍歷圖形A所有坐標(biāo)減去圖形B的坐標(biāo)，得到一系列的點。將這些點全部包圍起來，如果這個新生成的圖形 包含原點，則意味著兩個圖形發(fā)生了重疊，從而判斷兩個圖形發(fā)生了碰撞。我們把計算后的點稱為閔可夫斯基差(Minkowski difference)，將生成的三角形/線形稱為單純形(simplex)。假定兩個凸體A和B，用d(A,B)來表示A與B之間的距離，則距離可以表達(dá)為：

(4)

如定義v(C)為C中的離原點最近的一個點，即滿足以下條件：

則A、B之間距離用Minkowski和A-B的形式可表示為：

d(A,B)=v(A-B)

通過如上的轉(zhuǎn)換，就可以將A、B兩凸體間的相交檢測轉(zhuǎn)化為更為簡單的形式。當(dāng)A、B碰撞發(fā)生時，A、B交集不為空集，此時單純形(A-B)中存在零點。碰撞未發(fā)生時，單純形(A-B)中不存在零點。通過迭代的方式，不斷尋找單純形(A-B)中離原點最近的點，當(dāng)誤差小于設(shè)定好的上限時，停止迭代。

(a) 凸體A和B的位置 (b) 單純形(A-B)和零點的位置圖4 發(fā)生碰撞時零點在A-B內(nèi)部

(a) 凸體A和B的位置 (b) 單純形(A-B)和零點的位置圖5 未發(fā)生碰撞時零點在A-B外部

3 RRT算法及其改進(jìn)

3.1 基本RRT算法

RRT 算法以采樣的方式進(jìn)行路徑搜索,由于性能良好，被廣泛地應(yīng)用在各種機(jī)器人的運動規(guī)劃場景中。從給定起點開始搜索，并將節(jié)點存儲在隨機(jī)樹中。隨機(jī)采樣獲取采樣點，在隨機(jī)樹中選取距離采樣點最近節(jié)點在兩點連線方向擴(kuò)展一個步長，并產(chǎn)生新節(jié)點。在擴(kuò)展出的節(jié)點到達(dá)目標(biāo)點之前，不斷進(jìn)行以上操作。在環(huán)境M中由起點xinit到目標(biāo)點xgoal構(gòu)建隨機(jī)樹T的偽代碼如表3所示。

表3 隨機(jī)樹T的偽代碼表

圖6 RRT基本算法示意圖

3.2 RRT算法改進(jìn)

3.2.1 目標(biāo)偏置策略

傳統(tǒng)的 RRT 算法在復(fù)雜環(huán)境中存在搜索時間長、采樣點隨機(jī)性高等問題，本文采用基于概率的RRT算法。根據(jù)概率P的值來選擇樹的生長方向是隨機(jī)生長(xrand)還是朝向目標(biāo)位置(xgoal)生長，引入像目標(biāo)生長的機(jī)制可以加速路徑搜索的收斂速度。算法偽代碼如表4所示。

如圖7所示，與傳統(tǒng)RRT算法不同的是，采樣點不一定是由Sample()函數(shù)生成的隨機(jī)節(jié)點。采樣點為隨機(jī)節(jié)點和目標(biāo)點的概率分別是P和1-P，通過改變采樣點的采樣方式提升規(guī)劃效率。

圖7 基于概率的RRT算法示意圖

3.2.2 漸進(jìn)最優(yōu)機(jī)制

RRT算法效率相對較高，但是不能在運行過程中優(yōu)化路徑。RRT*算法可以隨著采樣點的增加，對路徑漸進(jìn)優(yōu)化。RRT*算法與RRT算法的區(qū)別主要在于兩個針對新節(jié)點xnew的重計算過程，分別為：重新為xnew選擇父節(jié)點的過程；重布線隨機(jī)樹的過程。

圖8 RRT*算法節(jié)點拓展

算法實現(xiàn)過程如下：

步驟1：通過基本RRT算法的得到新節(jié)點xnew和樹枝(xnear,xnew)；

步驟2：以xnew為中心，ri為半徑，在樹上搜索節(jié)點；

步驟3：找出潛在的父節(jié)點集合xpotential_parent，其目的是要更新xnew，看看有沒有比它更好的父節(jié)點；

步驟4：對每一個潛在的父節(jié)點xpotential_parent進(jìn)行檢測，找出能夠使從xinit到xnew路徑代價最小且未發(fā)生碰撞的父節(jié)點xparent；

步驟5：刪除樹枝(xnear,xnew)，添加樹枝(xparent,xnew)；

步驟6：對潛在的父節(jié)點集合xpotential_parent的剩余節(jié)點進(jìn)行檢測，假設(shè)以xnew作為父節(jié)點替換原先的父節(jié)點，計算xinit到xpotential_parent路徑代價是否小于原先的路徑代價且未發(fā)生碰撞；

步驟7：如果是，則更改父節(jié)點為xnew；如果不是，則保留原先父節(jié)點。

3.2.3 局部極小值脫離機(jī)制

RRT算法本身不能辨別已搜索的區(qū)域，這使得規(guī)劃器在擴(kuò)展區(qū)域內(nèi)重復(fù)擴(kuò)展。目標(biāo)偏好RRT算法與路徑規(guī)劃中的人工勢場方法[17]的一些特點相同。通常，簡單的加速展開法會在某些局部區(qū)域，產(chǎn)生局部極小值問題。某些已被搜索過的區(qū)域會被不斷重復(fù)搜索，這使得隨機(jī)樹很難快速逃離局部最小區(qū)域，甚至?xí)档退惴ㄟ\行成功率。因此，本文提出了一種結(jié)合碰撞信息檢測機(jī)制[18]的擴(kuò)展點選擇機(jī)制來逃避局部最小區(qū)域，彌補了原算法僅使用歐氏線性距離來選擇節(jié)點機(jī)制來選擇下一個擴(kuò)展方向，缺乏對局部約束的考慮，并能盡快擴(kuò)大以避開障礙。

圖9 向目標(biāo)點擴(kuò)展的節(jié)點與其父節(jié)點的碰撞概率 圖10 向隨機(jī)點擴(kuò)展的節(jié)點的碰撞概率

記錄碰撞概率算法實現(xiàn)過程如下：

步驟1：隨機(jī)樹擴(kuò)展過程中發(fā)生碰撞；

3.2.4 針對機(jī)械臂改進(jìn)采樣環(huán)境

機(jī)械臂每個關(guān)節(jié)角相同的變化量對機(jī)械臂位姿的影響差別很大。將關(guān)節(jié)空間的每一個方向設(shè)置一個權(quán)重，可以大幅提升機(jī)械臂軌跡規(guī)劃的效率。

表5 機(jī)械臂關(guān)節(jié)權(quán)重

3.3 改進(jìn)后的RRT算法流程

步驟1：初始化隨機(jī)樹；

步驟2：生成一個(0,1)的隨機(jī)數(shù)rand，比較該隨機(jī)數(shù)與目標(biāo)偏置概率的大小。若rand小于等于目標(biāo)偏置概率，選取采樣點為xgoal；若rand大于目標(biāo)偏置概率，選取采樣點為xrand；

步驟3：在碰撞概率小于pthreshold的點的集合中選擇距離采樣點最近的節(jié)點xnear；

步驟4：以固定步長在xnear和采樣點連線方向上擴(kuò)展新節(jié)點xnew；

步驟5：若生成的新節(jié)點與環(huán)境M發(fā)生碰撞，則擴(kuò)展失敗并記錄碰撞概率；若生成的新節(jié)點與環(huán)境M未發(fā)生碰撞，則擴(kuò)展成功，將節(jié)點和樹枝記錄到隨機(jī)樹T中；

步驟6：利用漸進(jìn)最優(yōu)機(jī)制，重新為xnew選擇父節(jié)點的過程并重布線隨機(jī)樹，從而降低路徑代價；

步驟7：判斷xnew和xgoal之間的距離是否小于等于誤差e。若是，則停止搜索，并將目標(biāo)點添加到隨機(jī)樹T中；否則，重復(fù)步驟2～步驟6，直到隨機(jī)樹擴(kuò)展成功；

步驟8：回溯隨機(jī)樹T，得到規(guī)劃路徑。

4 仿真實驗驗證

為驗證改進(jìn)算法，在二維/三維空間中運用不同的算法進(jìn)行路徑搜索，并以6自由度機(jī)械臂作為仿真實驗對象進(jìn)行仿真。實驗在Intel(R) Core(M) i5-4210H CPU @2.90 GHz 2.90 GHz，8.00 GB內(nèi)存，實驗所用的電腦操作系統(tǒng)為Win10。使用MATLAB在二維和三維空間中對點狀機(jī)器人進(jìn)行仿真，對于6自由度機(jī)械臂的仿真則在Mathematica中進(jìn)行。

4.1 二維仿真

設(shè)置800×800的場景1，將起點設(shè)為(1,1)，目標(biāo)點設(shè)為(750,750)，設(shè)置目標(biāo)偏向概率P=0.15，步長為5，令固定的最大節(jié)點數(shù)為1000，最大迭代次數(shù)為5000。

如圖11所示，不同的算法進(jìn)行路徑搜索得到的隨機(jī)樹以及規(guī)劃出的路徑。

(a) 基本RRT算法 (b) RRT*算法 (c) 改進(jìn)后的RRT算法圖11 二維場景中測試結(jié)果

經(jīng)過100組重復(fù)實驗，實驗結(jié)果如表6所示?？梢钥闯觯蠖鄶?shù)情況下在該場景中都可以成功完成路徑搜索。本文算法相對其它算法在路徑搜索更有方向性同時也大大減少了不必要的探索節(jié)點，從而節(jié)省時間提升了搜索效率。

表6 二維場景中各算法性能對比

4.2 三維仿真

設(shè)置600×400×400的三維場景，將起點設(shè)為(50,50,50)，終點設(shè)為(450,350,300)。經(jīng)過20組重復(fù)實驗，實驗結(jié)果如圖12和表7所示。從圖12和表7中可以看出，在具有障礙物的三維環(huán)境中，基本RRT算法以及RRT*算法在達(dá)到最大迭代次數(shù)時并不一定能成功搜索出從起點到終點的路徑。相對基本RRT算法，RRT*算法搜索出的路徑代價更小而改進(jìn)后的RRT算法在每次實驗中都能在較短的時間內(nèi)找到路徑代價相對較小的路徑。

(a) 基本RRT算法 (b) RRT*算法

表7 三維場景中各算法性能對比

4.3 6自由度機(jī)械臂仿真

在MATLAB中完成了二維/三維空間的路徑搜索仿真，并對比了不同算法之間的性能。接下來本文在Mathematica中，運行改進(jìn)后的RRT算法，可以生成一條較為平滑且不產(chǎn)生碰撞的機(jī)械臂運行路徑。

由于我們生活在三維空間，沒有辦法看到六維關(guān)節(jié)空間，為了將隨機(jī)樹的搜索以更加直觀的方式表達(dá)出來，將六維關(guān)節(jié)空間拆分成兩個三維空間，分別對應(yīng)前3個關(guān)節(jié)和后3個關(guān)節(jié)。為了提高軌跡規(guī)劃的效率，機(jī)械臂的關(guān)節(jié)空間在不同維度上賦予了不同的權(quán)重。如圖13所示，不同算法在關(guān)節(jié)空間中進(jìn)行路徑搜索。

(a) 基本RRT算法

經(jīng)過20組重復(fù)實驗，實驗結(jié)果如表8所示。由表8分析可知，在進(jìn)行機(jī)械臂運行路徑規(guī)劃時，由于關(guān)節(jié)空間維度較高，經(jīng)常存在規(guī)劃失敗的情況。相較于RRT和RRT*算法，改進(jìn)后的RRT算法進(jìn)行規(guī)劃時，平均運行時間顯著縮短，成功率大幅提高。同時，改進(jìn)后的RRT算法平均路徑節(jié)點數(shù)更少，這意味著機(jī)械臂運行過程中的位姿變化量更小。

表8 機(jī)械臂軌跡規(guī)劃過程中各算法性能對比

利用改進(jìn)后的RRT算法進(jìn)行運動規(guī)劃后得到的關(guān)節(jié)角變化曲線如圖14所示。

圖14 機(jī)械臂關(guān)節(jié)角變化數(shù)據(jù) 圖15 三次B樣條插值后機(jī)械臂關(guān)節(jié)角變化數(shù)據(jù)

由于RRT算法規(guī)劃出的路徑是相鄰的點以線段的方式連接形成的，是一條折線，其中一個特征便是路徑不夠平滑。利用三次B樣條曲線對得到的一串機(jī)械臂關(guān)節(jié)角進(jìn)行插值，可以平滑機(jī)械臂關(guān)節(jié)角變化速率，如圖15所示。

利用改進(jìn)后的RRT算法結(jié)合三次B樣條曲線對機(jī)械臂關(guān)節(jié)角變化數(shù)據(jù)進(jìn)行插值，可以得到一條較為平滑且路徑代價較小的機(jī)械臂運行軌跡。機(jī)械臂末端運行軌跡如圖16所示。

圖16 機(jī)械臂末端運行軌跡

5 結(jié)論

本文以RRT算法為基礎(chǔ)，利用目標(biāo)偏置提升規(guī)劃效率，使得算法在運行過程中具有導(dǎo)向性。同時為了降低算法陷入局部極小值的幾率，提出利用碰撞信息檢測機(jī)制降低隨機(jī)樹向局部極小值擴(kuò)展的概率。

在不同實驗場景下，改進(jìn)后的算法具有以下幾個優(yōu)勢：①運行效率更高；②占用的計算資源更少；③更好地避開障礙物，同時消耗的路徑代價也相對較小。

在機(jī)械臂仿真實驗中，改進(jìn)后的RRT算法能夠快速有效地生成可以運行的機(jī)械臂運動軌跡。同時利用三次B樣條函數(shù)對運行軌跡進(jìn)行處理，保證機(jī)械臂在運動過程中保持平穩(wěn)。