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

夏長高，許秋月，韓江義

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

0 引言

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

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

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

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

1 算法改進(jìn)

1.1 RRT*算法

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

(1)

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

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

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

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

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

1.2 a-bRRT*算法

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

(2)

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

(3)

式中：Uatt為引力場；Fatt為引力；katt為增益系數(shù)；X為機(jī)器人的當(dāng)前位置；Xg為目標(biāo)點(diǎn)位置。

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

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

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

(4)

(5)

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

(6)

式中：xnear為最近樹節(jié)點(diǎn)；xtp為臨時路徑點(diǎn)；xrand為隨機(jī)采樣點(diǎn)；Xrand為新采樣點(diǎn)；xnew為新節(jié)點(diǎn)；xgoal為目標(biāo)點(diǎn)；Δl為擴(kuò)展步長。

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

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

2 算法驗(yàn)證

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

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

圖4 bRRT算法驗(yàn)證仿真圖

圖5 RRT*算法驗(yàn)證仿真圖

圖6 a-bRRT*算法驗(yàn)證仿真圖

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

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

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

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

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

3.1 碰撞檢測

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

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

圖7 機(jī)械手連桿和障礙物簡化過程

3.2 仿真結(jié)果

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

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

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

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

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

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

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

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

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

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

表3 各算法仿真數(shù)據(jù)的標(biāo)準(zhǔn)差

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

4 結(jié)論

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

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

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

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