改進RRT-人工勢場法的機械臂堆垛運動方法

王 鑫，袁慶霓，江 濤，施輝城，孫睿彤，白 歡，衣君輝

(現(xiàn)代制造技術(shù)教育部重點實驗室，貴州 貴陽 550025)

0 引 言

電子商務的高速發(fā)展帶動了中國物流業(yè)的自動化和智能化。隨著互聯(lián)網(wǎng)電商平臺訂單量的大幅增長以及產(chǎn)品種類的多元化，對倉庫中貨物的揀選、貨柜堆垛的時效及工作量的要求越來越高。而傳統(tǒng)的倉儲系統(tǒng)中，人工貨柜堆垛和分揀貨柜中物品已經(jīng)成為倉儲自動化的瓶頸，亟待解決的問題，其中貨柜物品的自動貨柜堆垛首先需解決機械臂的運動規(guī)劃問題。

在機械臂的運動規(guī)劃領域中，Mashayekhi R等人提出混合RRT、Informed RRT-Connect等算法，在算法中引入雙樹模型。Khan A T等人提出使用稱為加權(quán)雅可比快速探索隨機樹(WJRRT)的智能算法對硬性和軟性機械手的路徑進行規(guī)劃。Kai Mi等人提出了一種結(jié)合運動學控制和快速探索隨機采樣方法的運動規(guī)劃算法。Sertac Karaman等人提出一種基于RRT的隨時算法。Gitae Kang等人提出了一種機械手運動規(guī)劃的目標導向(GO)采樣方法。陳波芝等人提出基于改進RRT算法利用剪枝擇優(yōu)和設置目標區(qū)域的方式使算法快速收斂。王懷江等人提出基于遺傳算法的機械臂優(yōu)化方法為機械臂規(guī)劃抓取的最短路徑。李洋等人提出協(xié)同路徑規(guī)劃的引力自適應步長RRT算法，并在隨機樹的節(jié)點處引入引力函數(shù)提升算法的融合速度。叢明等人使用改進的遺傳算法求解，為機械臂雙臂傳接過程選擇最佳的傳接位置。馬慧麗等人在RRT的基礎上引入目標引力，同時提出自適應步長避免路徑算法陷入局部最優(yōu)。劉建宇等人在目標偏向策略引入采樣的基礎上，采用梯度下降法優(yōu)化搜索出的路徑。周恒旭等人利用Dubins曲線策略處理Informed RRT算法，解決了軌跡不穩(wěn)定和尖銳的兩個缺點。

上述算法雖能有效解決路徑規(guī)劃問題，但對于雙臂機器人或者特定場景，并不能有效地為機械臂堆垛場景規(guī)劃最優(yōu)路徑。因此，該文提出一種改進RRT-人工勢場法的路徑規(guī)劃方法，為機械臂末端執(zhí)行器規(guī)劃出一條無碰撞的貨柜自動堆垛運動軌跡，并在Python和ROS對該方法進行驗證。

1 算法原理

1.1 RRT算法基本原理

快速拓展隨機樹算法(RRT)是由La- Valle教授提出。其基本原理是將始位置點作為隨機樹的根位置點，然后，在工作空間內(nèi)隨機采樣并找到隨機樹上距離采樣點最近的節(jié)點，由最近節(jié)點沿采樣點方向拓展一個步長生成新的枝葉節(jié)點，直到末位置點成為隨機樹枝葉節(jié)點。最后，連接隨機樹枝葉節(jié)點生成一條連接始、末位置點的無規(guī)則路徑。

1.2 人工勢場法基本原理

人工勢場法是Khatib首次提出的理論，最早是針對移動機器人避障規(guī)劃。其基本思想是將機器人轉(zhuǎn)化為一個質(zhì)點，將整個環(huán)境轉(zhuǎn)化為一個人造勢場，該勢場包含末位置點對機器人的引力勢場和障礙物對機器人的斥力勢場，機器人受引力勢場產(chǎn)生的引力和斥力勢場產(chǎn)生的疊加斥力，沿引力和斥力的合力方向運動。

2 改進算法

2.1 障礙物建模

該文使用AABB包圍盒和球體包圍障礙物分別在改進RRT和改進人工勢場法下進行障礙物建模，當路徑點存在于障礙物模型內(nèi)則表示規(guī)劃路徑與障礙物發(fā)生碰撞。

AABB包圍盒數(shù)學描述為：

q

=[

x

,

y

,

z

,

x

,

y

,

z

]

(1)

AABB包圍盒表示為每一邊都平行于坐標面的長方體。

球體包圍盒數(shù)學描述為：

q

=[

X

,

Y

,

Z

,

R

]

(2)

人工勢場法需要考慮當前路徑點與障礙物距離因素，因此，在AABB包圍盒的基礎上引入球體包圍盒對障礙物作進一步描述。(

X

,

Y

,

Z

)為障礙物中心的世界坐標，

R

為球形半徑，分別表示為：

(3)

(4)

2.2 改進RRT算法原理

針對傳統(tǒng)隨機樹生長方向具有無向性，隨著路徑采樣點的增加，冗余路徑也隨之增加，該文引入目標搜索。該方法能夠有效減少隨機樹無用生長，減小冗余路徑，提高算法規(guī)劃效率(如圖1)。

具體算法描述如下：

(1)將始位置點

q

設置為隨機樹根節(jié)點

T

；(2)令

q

=

q

；(3)生成隨機采樣點

q

；(4)遍歷隨機樹，搜索隨機樹距離

q

最近的隨機樹節(jié)點，將其命名為

q

；(5)由

q

沿

q

方向拓展生成新的枝葉節(jié)點

q

，且連接

q

和

q

并進行碰撞檢測，若碰撞，返回步驟(2)，若無碰撞，令

q

為

q

子節(jié)點，并進入下一步；(6)連接

q

和末位置點

q

。并進行碰撞檢測，若碰撞，返回步驟(2)，若無碰撞，進入下一步；(7)連接

q

和

q

，令

q

為

q

子節(jié)點，由末位置點

q

開始，根據(jù)隨機樹中父子關系搜索路徑節(jié)點，生成路徑PATH。其中

q

是隨機樹始位置點，

q

是隨機樹的末位置點，

q

是隨機樹新增枝葉節(jié)點，

q

是隨機樹上距離隨機采樣點

q

的最近節(jié)點，

S

是步長。

q

的計算如式(5)～式(7)所示。

(5)

q

-

q

=[(

x

-

x

),(

y

-

y

),(

z

-

z

)]

(6)

‖

q

,

q

‖=

(7)

圖1是隨機樹生長圖，其中黑點是RRT隨機樹節(jié)點和隨機采樣點；

q

是隨機樹始位置點；

q

是隨機樹的末位置點；

q

是隨機樹新增枝葉節(jié)點；

q

是隨機樹上距離隨機采樣點

q

的最近節(jié)點；

S

是步長。改進RRT算法偽代碼如算法1所示：

圖1 隨機樹生長圖

算法1: 改進RRT算法。

Input:始、末位置：

q

、

q

；工作空間(障礙空間)：

X

；步長：

S

；

Output:路徑PATH。

Initialize all Parameters;

1.Add_vertex(tree,

q

)

2.While Connect=0 do:

3.For 1 To N:

4.

q

←Sample_free(X);5.

q

,

q

←Get_nearest(tree,

q

);6.if Collision_free(

q

,

q

) then:7.Connect_to_point(tree,

q

,

q

,

S

);

8.End if

9.if Collision_free(

q

,

q

) then:

10.PATH←Get_path( );

11.Connect=1;

12.End if

13.End For

14.End While

15.Final

16.Return PATH

2.3 改進人工勢場法原理

人工勢場法全部信息來源引力勢場和斥力勢場，當機械臂遠離末位置點時：引力遠大于斥力，規(guī)劃路徑易與障礙物發(fā)生碰撞；當障礙物靠近末位置點時：斥力遠大于引力，易發(fā)生目標不可達。針對上述問題，對人工勢場法進行改進，解決路徑與障礙碰撞和目標不可達等問題。

人工勢場主要由末位置點對機械臂末端執(zhí)行器產(chǎn)生的引力勢場和障礙物產(chǎn)生的斥力勢場組成。故人工勢場

U

可定義為：

U

(

q

)=

U

(

q

)+

U

(

q

)

(8)

其中，

q

為機械臂末端執(zhí)行器當前位置點，

U

(

q

)表示引力勢場，

U

(

q

)表示斥力勢場。相應的合力

F

為：

F

(

q

)=

F

(

q

)+

F

(

q

)

(9)

針對當前位置與末位置距離過大而導致受力不平衡的問題，以障礙物斥力范圍作為標準給定閾值

D

，用于修正引力函數(shù)，防止引力遠大于斥力而導致路徑與障礙物發(fā)生碰撞。將

U

(

q

)表示為：

(10)

其中，

ξ

為常量；

ρ

(

q

,

q

)為當前位置到末位置的距離；

D

為給定的當前位置與末位置距離的閾值。

算法2：改進人工勢場法。

Input:始、末位置：

q

、

q

；工作空間(障礙空間)：

X

；步長：

S

；

Output:路徑Path。

Initialize all Parameters;

1.Path = [

q

];2.

q

=

q

;3.While Dis(

q

,

q

) > S do:4.

F

←Attractive( ) + Repulsion( );5.

q

←

F

.direction( )*

S

;6.path←Get_path(

q

);7.if Dis(

q

,

q

) <

S

then:8.path←Get_path(

q

);

9.End if

10.End While

11.Final

12.Return path

由引力勢場引發(fā)的引力相應表示為：

(11)

針對目標不可達的問題，在原有斥力場的基礎上加強機械臂末端執(zhí)行器與末位置距離的影響，機械臂末端執(zhí)行器靠近末位置時，斥力隨著位置靠近而相應減小，避免斥力遠大于引力而導致目標不可達。

U

(

q

)表示為：

(12)

其中，num=

ρ

(

q

,

q

)。

由斥力勢場引發(fā)的斥力相應表示為：

(13)

F

(

q

)、

F

(

q

)分別為：

(14)

(15)

其中，

η

為斥力場常量;

ρ

(

q

,

q

)為當前位置到障礙物的距離；

ρ

為斥力勢場范圍；人工勢場法偽代碼如算法2所示。

2.4 非均勻B樣條曲線

機械臂工作空間出現(xiàn)數(shù)目較多的隨機障礙物時，為滿足避障需求，規(guī)劃路徑往往會出現(xiàn)路徑轉(zhuǎn)折。在此情況下，機械臂會因為加速度突變而引起機體振動，對機體造成傷害。因此，該文引入非均勻B樣條曲線平滑路徑，為機械臂在復雜的障礙環(huán)境中提供有效解決方案。

B樣條曲線方程為：

(16)

其中，

p

為控制點；

N

,為

n

次B樣條的基函數(shù)；

M

為區(qū)間數(shù)；

n

為樣條曲線的次數(shù)。

B樣條的基本函數(shù)Cox-deBoor遞歸公式定義為：

(17)

從式(17)可知，第

j

個控制點對應的

n

次B樣條基數(shù)只與[

t

,

t

++1]個參數(shù)域有關。該文定義的是3次4階非均勻B樣條曲線，即當路勁節(jié)點為

m

時，該樣條曲線共有

m

+3個參數(shù)區(qū)間，其合法的定義區(qū)域為[

U

,

U

]。

2.5 改進 RRT-人工勢場法混合算法

該文提出的改進RRT-人工勢場法混合算法，是利用改進RRT算法對全局路徑進行規(guī)劃，當RRT規(guī)劃存在冗余路徑時，使用人工勢場法對局部進行優(yōu)化，并用優(yōu)化路徑取代冗余路徑。其具體流程如圖2所示。

圖2 改進RRT-人工勢場法流程圖

算法描述如下:

(1)初始化機械臂工作空間及工作空間中環(huán)境信息，給定機械臂移動的始、末位置點

q

、

q

和機械臂移動步長

S

;

(2)利用改進RRT算法為機械臂規(guī)劃全局路徑，其具體流程如改進RRT算法描述；

(3)將改進RRT規(guī)劃全局路徑PATH設為當前路徑，并對當前路徑進行判斷。若存在冗余路徑，將冗余節(jié)點設為人工勢場法始、末位置點

q

、

q

，開始改進人工勢場法局部路徑規(guī)劃；否則執(zhí)行攻進RPT算法步驟6)；(4)將始、末位置點

q

、

q

為當前位置點和末位置點代入式(11)計算引力，式(13)計算斥力，并根據(jù)式(9)求得當前位置點合力大小及方向。由當前位置點沿合力方向拓展步長

S

生成新節(jié)點

q

。同理，重復上述步驟直到Dis(

q

,

q

)<

S

?，令

q

為

q

;使用三次B樣條曲線光滑路徑并生成最終路徑；

3 實 驗

3.1 實驗任務

為了驗證改進RRT-人工勢場法混合算法的有效性和可行性，分別在Python和ROS上對改進算法進行比較和仿真實驗。所有實驗均在16 G內(nèi)存的Intel i7-9750H DELL Y7000計算機上進行。

3.2 Python實驗

為驗證文中所提算法是否具有改進意義，將改進RRT-人工勢場法同人工勢場法、RRT和Bi-RRT算法進行對比分析。實驗在地圖大小為100×100×100，障礙物數(shù)目

N

為30的隨機障礙場景中進行，始位置點為(0, 0, 0)，末位置點為(100, 100, 100)。

圖3是四種算法對比實驗。細線為隨機樹，粗線為算法規(guī)劃的路徑曲線。通過對比分析可知，RRT與Bi-RRT規(guī)劃路徑時不能有效解決因隨機策略產(chǎn)生的隨機樹生長無向性問題，人工勢場法在規(guī)劃路徑時能夠有效地靠近末位置點，改進RRT-人工勢場法混合算法能夠有效地將兩種算法的優(yōu)點進行融合，規(guī)劃出一條逐漸靠近末位置點的無碰光滑路徑。

圖3 算法對比實驗

當

N

=30，40，50時，對該算法實驗50次，結(jié)果如表1所示。隨著環(huán)境復雜程度的增加，其平均路徑長度無明顯變化。由此可知，改進RRT-人工勢場法混合算法不受復雜程度影響，且該算法能夠穩(wěn)定有效地為機械臂規(guī)劃出一條無碰撞的光滑路徑。為驗證該算法是否能夠有效為機械臂在工作空間中規(guī)劃路徑，故使用Python中模擬機械臂實驗場景。該場景包含一個模擬傳送帶AABB包圍盒和由若干AABB包圍盒組成的立式貨架，具體如圖4所示。

圖4 路徑規(guī)劃場景模擬實驗

表1 障礙物數(shù)量不同時各算法數(shù)據(jù)分析

圖4(a)～圖4(d)為不同末位置點路徑規(guī)劃圖，始位置位于傳送帶上，且坐標為(10, 25, 11)。結(jié)果表明，提出的改進RRT-人工勢場法混合算法能夠為機械臂規(guī)劃出由傳送帶至貨架任意格子的無碰光滑路徑。

3.3 ROS實驗仿真

在Ubuntu16.04LTS、ROS Kinetic平臺中使用Gazebo模擬現(xiàn)實仿真，如圖5所示，主要包含:傳送帶、塊狀物、UR5機械臂、貨架。要求UR5機械臂在運動狀態(tài)下傳送帶上，抓取塊狀物放置在貨架的指定位置，實現(xiàn)機械臂由抓取的初始位置，避開貨架邊緣側(cè)壁，為機械臂抓取至放置運動規(guī)劃出一條至指定目標位置的無碰撞路徑。

圖5 貨柜堆垛運動

圖5(a)為機械臂在運動狀態(tài)的傳送帶上抓取塊狀物；圖5(b)為機械臂將塊狀物送至貨架指定位置的行進運動；圖5(c)為機械臂將塊狀物放置指定位置的貨柜堆垛運動。由圖可知，該算可以完成貨柜堆垛場景下的機械臂運動仿真，實現(xiàn)為機械臂抓取、貨柜堆垛運動，驗證了改進RRT-人工勢場法的有效性與可靠性。

4 結(jié)束語

該文提出改進RRT-人工勢場法混合算法對機械臂堆垛進行運動規(guī)劃，其主要改進點如下：

(1)針對傳統(tǒng)RRT算法存在的路徑節(jié)點多、冗余路徑長等問題，引入目標搜索。該方法能夠有效探索空間，減少隨機樹無效生長，提高算法規(guī)劃效率；

(2)針對人工勢場法受力不平衡和目標不可達等問題，分別利用斥力范圍作為閾值修正引力函數(shù)、當前位置點和末位置點距離作為影響參數(shù)修正斥力函數(shù)。

(3)利用RRT算法的概率完備性規(guī)劃全局路徑，利用人工勢場法的有向性對RRT算法進行了補充。同時，RRT算法也為改進算法的避障能力提供了有效保證。

最后，將改進RRT-人工勢場法混合算法分別在Python和ROS系統(tǒng)中對機械臂貨柜堆垛運動進行了模擬仿真。驗證結(jié)果表明，該算法能夠為機械臂堆垛運動提供有效支持。