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

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

0 引 言

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

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

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

1 算法原理

1.1 RRT算法基本原理

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

1.2 人工勢場法基本原理

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

2 改進(jìn)算法

2.1 障礙物建模

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

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

q

=[

x

,

y

,

z

,

x

,

y

,

z

]

(1)

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

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

q

=[

X

,

Y

,

Z

,

R

]

(2)

人工勢場法需要考慮當(dāng)前路徑點(diǎn)與障礙物距離因素，因此，在AABB包圍盒的基礎(chǔ)上引入球體包圍盒對(duì)障礙物作進(jìn)一步描述。(

X

,

Y

,

Z

)為障礙物中心的世界坐標(biāo)，

R

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

(3)

(4)

2.2 改進(jìn)RRT算法原理

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

具體算法描述如下：

(1)將始位置點(diǎn)

q

設(shè)置為隨機(jī)樹根節(jié)點(diǎn)

T

；(2)令

q

=

q

；(3)生成隨機(jī)采樣點(diǎn)

q

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

q

最近的隨機(jī)樹節(jié)點(diǎn)，將其命名為

q

；(5)由

q

沿

q

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

q

，且連接

q

和

q

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

q

為

q

子節(jié)點(diǎn)，并進(jìn)入下一步；(6)連接

q

和末位置點(diǎn)

q

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

q

和

q

，令

q

為

q

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

q

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

q

是隨機(jī)樹始位置點(diǎn)，

q

是隨機(jī)樹的末位置點(diǎn)，

q

是隨機(jī)樹新增枝葉節(jié)點(diǎn)，

q

是隨機(jī)樹上距離隨機(jī)采樣點(diǎn)

q

的最近節(jié)點(diǎn)，

S

是步長。

q

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

(5)

q

-

q

=[(

x

-

x

),(

y

-

y

),(

z

-

z

)]

(6)

‖

q

,

q

‖=

(7)

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

q

是隨機(jī)樹始位置點(diǎn)；

q

是隨機(jī)樹的末位置點(diǎn)；

q

是隨機(jī)樹新增枝葉節(jié)點(diǎn)；

q

是隨機(jī)樹上距離隨機(jī)采樣點(diǎn)

q

的最近節(jié)點(diǎn)；

S

是步長。改進(jìn)RRT算法偽代碼如算法1所示：

圖1 隨機(jī)樹生長圖

算法1: 改進(jìn)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 改進(jìn)人工勢場法原理

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

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

U

可定義為：

U

(

q

)=

U

(

q

)+

U

(

q

)

(8)

其中，

q

為機(jī)械臂末端執(zhí)行器當(dāng)前位置點(diǎn)，

U

(

q

)表示引力勢場，

U

(

q

)表示斥力勢場。相應(yīng)的合力

F

為：

F

(

q

)=

F

(

q

)+

F

(

q

)

(9)

針對(duì)當(dāng)前位置與末位置距離過大而導(dǎo)致受力不平衡的問題，以障礙物斥力范圍作為標(biāo)準(zhǔn)給定閾值

D

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

U

(

q

)表示為：

(10)

其中，

ξ

為常量；

ρ

(

q

,

q

)為當(dāng)前位置到末位置的距離；

D

為給定的當(dāng)前位置與末位置距離的閾值。

算法2：改進(jìn)人工勢場法。

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ā)的引力相應(yīng)表示為：

(11)

針對(duì)目標(biāo)不可達(dá)的問題，在原有斥力場的基礎(chǔ)上加強(qiáng)機(jī)械臂末端執(zhí)行器與末位置距離的影響，機(jī)械臂末端執(zhí)行器靠近末位置時(shí)，斥力隨著位置靠近而相應(yīng)減小，避免斥力遠(yuǎn)大于引力而導(dǎo)致目標(biāo)不可達(dá)。

U

(

q

)表示為：

(12)

其中，num=

ρ

(

q

,

q

)。

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

(13)

F

(

q

)、

F

(

q

)分別為：

(14)

(15)

其中，

η

為斥力場常量;

ρ

(

q

,

q

)為當(dāng)前位置到障礙物的距離；

ρ

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

2.4 非均勻B樣條曲線

機(jī)械臂工作空間出現(xiàn)數(shù)目較多的隨機(jī)障礙物時(shí)，為滿足避障需求，規(guī)劃路徑往往會(huì)出現(xiàn)路徑轉(zhuǎn)折。在此情況下，機(jī)械臂會(huì)因?yàn)榧铀俣韧蛔兌饳C(jī)體振動(dòng)，對(duì)機(jī)體造成傷害。因此，該文引入非均勻B樣條曲線平滑路徑，為機(jī)械臂在復(fù)雜的障礙環(huán)境中提供有效解決方案。

B樣條曲線方程為：

(16)

其中，

p

為控制點(diǎn)；

N

,為

n

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

M

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

n

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

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

(17)

從式(17)可知，第

j

個(gè)控制點(diǎn)對(duì)應(yīng)的

n

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

t

,

t

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

m

時(shí)，該樣條曲線共有

m

+3個(gè)參數(shù)區(qū)間，其合法的定義區(qū)域?yàn)閇

U

,

U

]。

2.5 改進(jìn) RRT-人工勢場法混合算法

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

圖2 改進(jìn)RRT-人工勢場法流程圖

算法描述如下:

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

q

、

q

和機(jī)械臂移動(dòng)步長

S

;

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

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

q

、

q

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

q

、

q

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

S

生成新節(jié)點(diǎn)

q

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

q

,

q

)<

S

?，令

q

為

q

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

3 實(shí) 驗(yàn)

3.1 實(shí)驗(yàn)任務(wù)

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

3.2 Python實(shí)驗(yàn)

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

N

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

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

圖3 算法對(duì)比實(shí)驗(yàn)

當(dāng)

N

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

圖4 路徑規(guī)劃場景模擬實(shí)驗(yàn)

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

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

3.3 ROS實(shí)驗(yàn)仿真

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

圖5 貨柜堆垛運(yùn)動(dòng)

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

4 結(jié)束語

該文提出改進(jìn)RRT-人工勢場法混合算法對(duì)機(jī)械臂堆垛進(jìn)行運(yùn)動(dòng)規(guī)劃，其主要改進(jìn)點(diǎn)如下：

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

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

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

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