果園綠籬修剪機械手路徑規(guī)劃算法研究

夏長高，許秋月，韓江義

(江蘇大學(xué) 汽車與交通工程學(xué)院， 江蘇 鎮(zhèn)江 212013)

0 引言

綠籬是果園的屏障，起到了間隔保護的作用[1]，現(xiàn)有的手持式綠籬修剪機技術(shù)存在修剪工作繁重、效率低等缺點，實現(xiàn)綠籬的自動修剪已成為當下研究熱點。能夠使修剪機械手避開障礙物且能自動運行到修剪點，對提高機械手的智能化有很大的作用。

對于避障路徑規(guī)劃問題，現(xiàn)有的路徑規(guī)劃算法包括圖搜索算法、人工勢場法、智能算法和快速搜索隨機樹算法。基于圖形的搜索方法[2-4]需要遍歷有向圖中的所有頂點，搜索效率低且不適用于高維空間。當障礙物對機器人的斥力與目標點對機器人的吸引力大小相等且方向相反時，人工勢場法容易陷入局部極小值或目標不可達[5]。智能算法[6-7]通常與傳統(tǒng)路徑規(guī)劃算法結(jié)合使用，其規(guī)劃效率取決于參數(shù)的選擇和算法的特性。

機械手是一個具有高度非線性的復(fù)雜鏈式結(jié)構(gòu)，不能作為笛卡爾空間中的粒子進行路徑規(guī)劃，因此大多數(shù)路徑規(guī)劃算法不能直接應(yīng)用于機械手。快速探究隨機樹RRT算法[8]是一種通過隨機構(gòu)建空間填充樹來有效搜索非凸，高維空間的算法，可以有效避免創(chuàng)建三維障礙物模型的困難，廣泛應(yīng)用于機械手路徑規(guī)劃中，但是RRT算法存在采樣過程隨機、搜索時間長、路徑代價高，難以在復(fù)雜狹小空間快速搜索到路徑等缺點[9]。許多學(xué)者針對上述問題，在RRT算法的采樣過程[10-12]和縮短路徑長度方面[13-14]進行改進，但是改進后的算法不能兼顧搜索效率和路徑質(zhì)量，存在計算量大等問題。

本文針對RRT算法采樣的缺點，主要研究了RRT算法的采樣過程，提出了一種結(jié)合目標偏差概率和引力思想的改進采樣過程，該采樣過程使算法在采樣時偏向目標點，加快了搜索速度。為縮短路徑長度，減少路徑點數(shù)，將RRT*算法與改進的采樣方法相結(jié)合，使算法能夠快速搜索漸進最優(yōu)路徑。通過在MATLAB中的機器人系統(tǒng)工具箱中進行仿真，驗證算法的有效性。

1 算法改進

1.1 RRT*算法

RRT*算法是針對RRT算法搜索路徑長的缺點提出的一種漸進最優(yōu)快速搜索隨機樹算法，其重新選擇父節(jié)點和重新布線過程，有效縮短了搜索路徑的長度和減少了路徑點[15]，重新選擇父節(jié)點是以xnew為中心，建立半徑為R的球形搜索空間，半徑R可根據(jù)式(1)求得。在該球內(nèi)的節(jié)點稱為近鄰節(jié)點xpotentialparent，依次計算xnew經(jīng)過xpotentialparent到xstart路徑代價，并對xnew和xpotentialparent之間的路徑進行碰撞檢測，選出使路徑代價最小且無碰撞的近鄰節(jié)點作為xnew新的父節(jié)點。之后，為進一步使隨機樹節(jié)點間的路徑代價盡可能小，為擴展樹重新布線，如果近鄰節(jié)點的父節(jié)點改為新節(jié)點xnew可以使路徑代價減小，則將近鄰節(jié)點的父節(jié)點改為xnew。

(1)

式中：R為空間半徑；γ為基于環(huán)境的規(guī)劃常數(shù)；n為近鄰節(jié)點的個數(shù)；d為規(guī)劃空間維數(shù)。

RRT*算法選擇父節(jié)點和重新布線的擴展過程如圖1所示。

圖1中，xpotentialparent代表鄰近節(jié)點，xparent代表父節(jié)點。

圖1 RRT*算法選擇父節(jié)點和重新布線過程

雖然RRT*算法可以得到漸進最優(yōu)路徑，但是RRT*算法采樣過程與RRT算法相同，采樣過程具有隨機性，算法效率低，不利于機器的實時控制，因此需要對算法進行改進，降低規(guī)劃時間，提高規(guī)劃效率。

1.2 a-bRRT*算法

人工勢場法(APF)是Khatib[16]提出的一種機器人路徑規(guī)劃算法。該算法將目標點和障礙物分別看作對機器人有引力和斥力的物體，機器人沿引力與斥力的合力來進行運動。傳統(tǒng)APF中機器人與目標點之間的引力場定義為[17]：

(2)

Fatt(X)=-▽Uatt(X)=-katt|X-Xg|

(3)

式中：Uatt為引力場；Fatt為引力；katt為增益系數(shù)；X為機器人的當前位置；Xg為目標點位置。

通常情況下，為了加快RRT算法的隨機搜索速度，通常設(shè)置目標偏移概率λ，即bias-RRT[18]，本文稱為bRRT。雖然bRRT算法相對于RRT算法的路徑規(guī)劃速度有所加快，但是當生成的隨機數(shù)大于偏轉(zhuǎn)概率時，采樣依然隨機。如果在RRT*中引入目標偏轉(zhuǎn)概率λ和APF法的引力思想，對RRT*算法的采樣過程進行改進。算法中的rand函數(shù)隨機生成0～1的隨機數(shù)，如果隨機數(shù)小于λ，則將目標點xgoal設(shè)置為采樣點Xrand，如果隨機數(shù)大于λ，則在采樣空間中隨機采樣之后得到xrad，目標點對擴展樹上距離采樣點最近的點xnear產(chǎn)生吸引力，根據(jù)目標點對最近點xnear的吸引力，得到臨時路徑點xtp,由隨機采樣點xrand和臨時路徑點xtp求取新的采樣點Xrand，a-bRRT*算法采樣過程如圖2所示，臨時路徑點xtp和新采樣點Xrand可以用以下公式進行表示：

圖2 a-RRT*算法采樣過程

xtp=-Δl·katt·|xnear-xgoal|=

(4)

(5)

則新節(jié)點xnew可通過式(5)求得：

(6)

式中：xnear為最近樹節(jié)點；xtp為臨時路徑點；xrand為隨機采樣點；Xrand為新采樣點；xnew為新節(jié)點；xgoal為目標點；Δl為擴展步長。

將改進后的采樣過程與RRT*算法中的重新選擇父節(jié)點和重新布線相結(jié)合，給出a-bRRT*算法流程，如圖3所示。

圖3 a-bRRT*算法流程框圖

2 算法驗證

為了驗證改進算法的搜索和尋優(yōu)能力，創(chuàng)建包括9個邊長為2 cm正方體障礙物的驗證環(huán)境，它們位于起始點(5，5，2)和目標點之間。為了驗證算法在復(fù)雜空間中的有效性，將目標點設(shè)置為(11，10，13)。為了驗證算法在狹窄空間中的尋優(yōu)能力，將目標設(shè)置在2個障礙物之間的狹窄空間中，目標點為(11，8，10)，3種算法在邊長為20 cm的正方體空間中進行隨機采樣，步長設(shè)置為1，當目標點與擴展樹xnear節(jié)點距離小于1時，將目標點添加到擴展樹中，算法結(jié)束。模擬結(jié)果如圖4—6所示，其中紅線表示搜索無碰撞路徑，綠線表示擴展的樹枝，紅色點表示擴展樹節(jié)點。

從圖4—6可以看出，bRRT、RRT*和a-bRRT*都可以在復(fù)雜環(huán)境和狹窄空間中搜索無碰撞路徑。在圖4中，由于bRRT算法采樣方向的隨機性，擴展樹充滿采樣空間，規(guī)劃效率低，路徑最長。從圖5可以看出，RRT*的路徑長度明顯短于bRRT算法，在復(fù)雜環(huán)境中，RRT*算法可以搜索到漸進最優(yōu)路徑。由于其采樣過程的隨機性，重新選擇父節(jié)點和重新布線的過程致使當目標點在狹小空間時，算法很難搜索到路徑，搜索效率低。從圖6可以看出，改進算法a-bRRT*可以有效減少擴展樹節(jié)點且向目標點方向進行擴展。在復(fù)雜的環(huán)境和狹小的空間中，該算法都能夠高效地搜索路徑，且搜索路徑較短。

圖4 bRRT算法驗證仿真圖

圖5 RRT*算法驗證仿真圖

圖6 a-bRRT*算法驗證仿真圖

為了詳細比較bRRT、RRT*和a-bRRT*3種算法，在Matlab中對3種算法進行了多次仿真，記錄其搜索時間、路徑長度、采樣點和樹節(jié)點的數(shù)量，其平均數(shù)據(jù)如表1所示。

表1 3種算法的仿真平均數(shù)據(jù)

2種空間條件下，與bRRT算法相比，a-bRRT*算法的路徑長度分別縮短了29.9%和23.8%；與RRT*算法相比，時間分別減少了52.8%和98.6%。改進后的算法提高了規(guī)劃效率，縮短了路徑長度，采樣點和樹節(jié)點數(shù)量顯著減少，加入引力思想算法的采樣點數(shù)量減少了約90.44%。改進后的算法采樣方向性強，易于快速搜索無碰撞路徑。

3 機械手避障路徑規(guī)劃仿真

在關(guān)節(jié)空間中進行路徑規(guī)劃，機械臂自初始位姿至目標位姿的運動不可預(yù)知，且搜索速度遠慢于笛卡爾空間規(guī)劃法，但是計算量小，不需要時刻進行機械臂逆運動學(xué)求解，路徑是在機械手各個關(guān)節(jié)范圍內(nèi)規(guī)劃出的，因此關(guān)節(jié)角度變化必定連貫，排除出現(xiàn)奇異點的可能[19]。本文在關(guān)節(jié)空間中進行路徑規(guī)劃，在笛卡爾空間中進行碰撞檢測。

3.1 碰撞檢測

為避免機械手從起始位姿到達修剪位姿觸碰綠籬，需要進行機械手避障。本文中綠籬修剪機械手的修剪狀況有2種：修剪綠籬頂面時，機械手由初始位姿運行至綠籬頂面修剪位姿；完成頂面修剪后，機械手從頂面修剪位姿運行至側(cè)面修剪位姿。

機械手碰撞包括機械手連桿之間的碰撞以及整個機械手和障礙物之間的碰撞[20]，果園綠籬墻高度一般在1.2～2 m，寬0.8 m，以綠籬墻為障礙物，采用矩形包絡(luò)法對障礙物進行簡化。將機械手連桿的最大半徑疊加在障礙物上，將連桿與障礙物的空間碰撞檢測問題轉(zhuǎn)化為空間直線與平面位置關(guān)系的檢測，如圖7所示。

圖7 機械手連桿和障礙物簡化過程

3.2 仿真結(jié)果

機械手在Matlab中的機器人系統(tǒng)工具箱(RST)中進行仿真。為了更好地驗證算法，在SolidWorks中建立了四自由度修剪機械手模型，如圖8所示。將機械手模型通過插件轉(zhuǎn)化為URDF格式，導(dǎo)入到RST工具箱中，進行機械手路徑規(guī)劃仿真。

圖8 修剪機械手三維結(jié)構(gòu)示意圖

機械手第一個修剪狀況是從收縮狀態(tài)到達頂部修剪位置，起始點的關(guān)節(jié)坐標為(0，2.094 4，-2.094 4，-1.047 2)，目標坐標為(0,1.275 4，-0.850 1，-1.996 0)。第二個修剪工況為機械手頂端修剪完畢，從頂端運行到側(cè)面修剪點的位置，起點的關(guān)節(jié)坐標為(0，1.275 4，-0.850 1，-1.996 0)，終點的關(guān)節(jié)坐標為(0，1.807 0，-2.443 5，0.636 5)，機械手路徑規(guī)劃采樣范圍限定在機械手各個關(guān)節(jié)轉(zhuǎn)動范圍內(nèi)。

以從修剪頂面運行至修剪側(cè)面為例，設(shè)置高 2 m，長2 m，寬0.8 m的綠籬墻障礙物，在RST工具箱中進行三維仿真，獲得機械手末端執(zhí)行器路徑，驗證a-bRRT*算法可以在起始點和終點之間搜索到一條無碰撞路徑，且路徑長度得到了優(yōu)化，如圖9—11所示，圖中綠色立方體為障礙物模型。

圖9 bRRT算法末端執(zhí)行器路徑

圖10 RRT*算法末端執(zhí)行器路徑

圖11 a-bRRT*算法末端執(zhí)行器路徑

為驗證改進a-bRRT*算法的優(yōu)越性，在關(guān)節(jié)空間對3種路徑規(guī)劃算法進行多次仿真驗證，結(jié)果如表2所示。

表2 機械手路徑規(guī)劃仿真平均數(shù)據(jù)

從表2可以看出，修剪頂面時，與bRRT相比，a-bRRT*的平均路徑長度減少了66.32%，規(guī)劃時間減少44.19%。修剪側(cè)面時，a-bRRT*算法的路徑長度比bRRT*算法短67.17%，規(guī)劃時間比RRT*算法少73.87%。結(jié)果表明，改進后的算法兼顧路徑長度和規(guī)劃效率，達到了預(yù)期目的。每個算法模擬數(shù)據(jù)的標準差用于評估算法的穩(wěn)定性。各算法數(shù)據(jù)的標準差如表3所示，在仿真結(jié)果中，改進后的a-bRRT*算法在2種修剪條件下的時間標準差和長度標準差都是最小，因此改進后的算法比原算法在保證規(guī)劃效率和路徑長度方面更加穩(wěn)定。

表3 各算法仿真數(shù)據(jù)的標準差

從圖9—11，以及表3可以看出，這3種算法都可以順利完成避障路徑規(guī)劃，但bRRT算法搜索時間過長，路徑長。雖然RRT*算法的路徑長度縮短，但搜索效率低。改進的a-bRRT*算法偏向目標點采樣，不僅搜索速度快，規(guī)劃效率高，而且繼承了RRT*算法搜索路徑長度短的優(yōu)點，兼顧了搜索效率和路徑長度。

4 結(jié)論

1) 針對RRT算法采樣隨機、效率低、規(guī)劃路徑長的缺點，對算法采樣過程進行改進，結(jié)合目標偏移概率和引力的思想，提出了一種改進的RRT*算法。改進后的算法采樣偏向標點，規(guī)劃效率高、路徑短，采樣點數(shù)減少約90.44%。

2) 在RST工具箱中驗證a-bRRT*算法在機械手路徑規(guī)劃中的有效性。結(jié)果表明，a-bRRT*算法能夠成功規(guī)劃無碰撞路徑，效率高、路徑短，且改進后的算法更穩(wěn)定。

3) 針對修剪機械手避障路徑規(guī)劃問題，僅在靜態(tài)已知障礙物環(huán)境下進行路徑規(guī)劃，因此在后續(xù)研究中，需要考慮動態(tài)障礙物的實時避障，以適應(yīng)更多的修剪場景。

4) 由于條件有限，本文的研究僅在MATLAB/RST中進行仿真驗證，后期還需進行實驗驗證。