朱子祺，李創(chuàng)業(yè)，代偉

（1.國家能源集團神東煤炭集團公司 洗選中心,陜西 榆林 719300；2.鄭州機電工程研究所,河南 鄭州 450015；3.中國礦業(yè)大學 信息與控制工程學院,江蘇 徐州 221116）

0 引言

我國能源資源的特點是富煤、貧油、少氣，在已探明的一次能源儲量中，煤炭占85%以上，煤炭在我國經(jīng)濟發(fā)展中具有重大戰(zhàn)略意義[1-3]。煤矸石是在煤炭開采時產(chǎn)生的一種固體廢料，煤矸石分揀是煤炭生產(chǎn)過程中不可或缺的環(huán)節(jié)，對于提高煤炭質(zhì)量及企業(yè)收益具有重要意義[4-5]。傳統(tǒng)的人工選矸方式存在勞動強度大、生產(chǎn)效率低的缺點，惡劣的工作環(huán)境使得工傷事件頻發(fā)[6-8]。隨著機器人技術的發(fā)展，利用機器人分揀煤矸石逐漸成為研究熱點[9-11]。由于煤矸石分揀環(huán)境復雜，為了避免機器人與障礙物發(fā)生碰撞，提高分揀效率，對機器人進行路徑規(guī)劃十分必要[12-14]。

目前用于機器人路徑規(guī)劃的算法主要分為3 類：基于圖論的路徑規(guī)劃算法，基于生物啟發(fā)式的路徑規(guī)劃算法，基于離散采樣的路徑規(guī)劃算法?；趫D論的路徑規(guī)劃算法主要有A*算法[15]、D*算法[16]，該類算法需要精確定位障礙物，且計算量過大，耗時長，因此實用性不強?；谏飭l(fā)式的路徑規(guī)劃算法主要有遺傳算法[17]、粒子群優(yōu)化算法[18]，該類算法大多需要經(jīng)過多次重復訓練來確定算法參數(shù)，計算量大，搜索時間長，且運行時需要占用大量存儲空間?；陔x散采樣的路徑規(guī)劃算法的主要思想是在配置空間中選擇有限數(shù)量的點，并在這些點之間建立連接，該類算法的復雜度不依賴于地圖的復雜度，且具有概率完備性，因此具有較強實用性。

快速擴展隨機樹（Rapidly-exploring Random Trees,RRT）作為一種經(jīng)典的基于隨機采樣的路徑規(guī)劃算法，具有便捷高效、易于約束、概率完備等特性，比較適用于高維空間中的路徑規(guī)劃。但RRT 算法是在給定搜索空間內(nèi)進行全局隨機采樣，具有極大的盲目性，易在無效空間內(nèi)進行過多采樣，耗費較大計算代價。而用于煤矸石分揀的機器人需要盡可能地縮短任務周期，因此，應盡量規(guī)劃出較短路徑。

RRT*算法[19]在RRT 算法基礎上進行了改進，每次擴展時選取新節(jié)點附近的節(jié)點中與起始點距離最近的點作為父節(jié)點，確保隨機樹的節(jié)點都能收斂到當前最優(yōu)值。RRT*算法雖然具有路徑長度漸進最優(yōu)的特點，但仍存在盲目搜索的問題。為了提高搜索效率，一般的方法是引入目標偏置策略，而傳統(tǒng)目標偏置策略的概率值是固定的，無法根據(jù)實際環(huán)境進行調(diào)整，造成對可行空間進行無必要的搜索且會使路徑不夠平直。針對該問題，本文提出一種變概率的目標偏置策略，該策略在無障礙物區(qū)域增大目標偏置概率值以增強算法的目標導向性，而在障礙物區(qū)域減小目標偏置概率值以保證算法的避障能力；提出G-RRT*（Goal-RRT*）算法，將變概率的目標偏置策略與RRT*算法相結合，既保留了RRT*算法路徑長度漸進最優(yōu)的特點，也提高了算法的目標導向性。

1 煤矸石分揀機器人路徑規(guī)劃問題描述

煤矸石分揀系統(tǒng)主要由上位機、煤矸石識別子系統(tǒng)、機器人、輸送帶測速裝置等組成，通過TCP/IP通信協(xié)議實現(xiàn)連接，如圖1 所示。煤矸石識別子系統(tǒng)用于分辨煤和矸石，將矸石位置信息打包發(fā)送給上位機；上位機結合矸石位置信息和測速裝置反饋的輸送帶速度信息，計算出矸石到達抓取點的時間，給機器人下發(fā)抓取任務；機器人根據(jù)上位機分配的任務抓取矸石并送到指定位置，完成煤矸石分揀。

圖1 煤矸石分揀系統(tǒng)Fig.1 Coal gangue sorting system

在煤矸石分揀過程中，輸送帶是一直運動的，即煤矸石識別子系統(tǒng)識別到矸石后，矸石繼續(xù)向前運動。因此，在目標矸石到達機器人的可行工作區(qū)間之前，機器人應完成上一次的分揀任務并提前回到初始位置，準備抓取目標矸石。

煤矸石分揀機器人路徑規(guī)劃問題可歸結為在障礙物環(huán)境下規(guī)劃出一條從給定起點到目標點的無碰撞路徑。煤矸石分揀機器人路徑規(guī)劃需要滿足以下條件：

（1）速度快：規(guī)劃的路徑應盡可能短，使得機器人執(zhí)行任務的時間盡可能小于2 個抓取任務的時間間隔。

（2）約束性：路徑規(guī)劃算法要滿足工作環(huán)境的約束。輸送帶作為煤和矸石的載體，給路徑規(guī)劃算法增加了位置約束，即機器人在運動過程中要避免與輸送帶和其他障礙物接觸。

本文主要針對煤矸石分揀機器人路徑規(guī)劃算法部分進行研究，因此，針對煤矸石分揀系統(tǒng)的其他部分作如下假設：

（1）煤矸石識別子系統(tǒng)能夠提供精確的矸石位置和對應的機器人關節(jié)角度值。

（2）輸送帶測速裝置能夠?qū)崟r反饋輸送帶的速度、加速度。

（3）煤矸石識別子系統(tǒng)識別出矸石位置后，矸石相對于輸送帶的位置不再發(fā)生變化。

（4）使用球形或方形包絡體代替障礙物，使用圓柱體代替機器人連桿，使用球體代替機器人末端執(zhí)行器。

2 煤矸石分揀機器人路徑規(guī)劃方案設計

機器人路徑規(guī)劃一般在笛卡爾空間或關節(jié)空間中進行。笛卡爾空間即三維空間，在該空間進行路徑規(guī)劃的優(yōu)點是障礙物容易表達、空間維度低，缺點是需要通過求逆解得到關節(jié)路徑，但求機器人的逆解計算量大且解一般不唯一。關節(jié)空間即驅(qū)動機器人運動的關節(jié)角空間，在該空間中進行路徑規(guī)劃的優(yōu)點是規(guī)劃出的路徑即為機器人能夠直接運行的路徑，缺點是需將障礙物從笛卡爾空間映射到關節(jié)空間，技術難度較大。本文結合笛卡爾空間和關節(jié)空間的優(yōu)點，提出一種在關節(jié)空間進行路徑規(guī)劃、在笛卡爾空間進行碰撞檢測的方案，該方案不需要對機器人進行運動學求逆，且可避免在關節(jié)空間中描述障礙物。

機器人路徑規(guī)劃方案分為機器人建模、路徑規(guī)劃算法、任務空間轉(zhuǎn)換及碰撞檢測4 個模塊，如圖2所示。

圖2 機器人路徑規(guī)劃方案Fig.2 Robot path planning scheme

要實現(xiàn)機器人路徑規(guī)劃，首先要針對機器人的機械結構進行建模。本文選用AUBO-i5 機器人，其結構如圖3 所示。采用D-H 參數(shù)法對機器人進行建模。根據(jù)實物機器人結構尺寸，得到機器人的D-H 參數(shù)，根據(jù)D-H 參數(shù)可推導出基坐標系到各關節(jié)坐標系的轉(zhuǎn)換關系，進而得到末端執(zhí)行器的位姿矩陣。

圖3 機器人機械結構Fig.3 Robot mechanical structure

實際煤矸石分選環(huán)境中的機器人和障礙物都是不規(guī)則物體，很難假定它們之間的碰撞條件，因此，簡化機器人連桿和障礙物模型對于提高碰撞檢測效率具有重要意義。根據(jù)機器人連桿結構和障礙物特點，將連桿簡化為圓柱體，將障礙物簡化為球體或長方體，如圖4 所示。機器人與障礙物之間的碰撞檢測問題轉(zhuǎn)換為一系列判斷規(guī)則幾何體之間是否交涉的問題。

圖4 機器人連桿和障礙物簡化模型Fig.4 Simplified model of robot link and obstacle

3 路徑規(guī)劃算法

3.1 RRT 算法原理

RRT 算法是一種類似于樹生長的空間搜索算法。從給定起點xinit開始進行擴展搜索，并將搜索節(jié)點存儲于一個樹狀結構（隨機樹）中，如圖5 所示。擴展新節(jié)點時，通過在可行空間中隨機采樣獲取采樣點xsample；在隨機樹中尋找距離采樣點xsample歐氏距離最近的點作為最近節(jié)點xnearest，并以一定步長s生成新節(jié)點xnew；判斷xnew是否處于障礙物中，若是則舍棄該節(jié)點，重新進行采樣，反之則將xnew加入隨機樹中；如此循環(huán)搜索，直至到達目標點xgoal。

圖5 RRT 算法節(jié)點擴展Fig.5 Node extension of RRT algorithm

RRT 算法在給定空間中進行全空間的盲目隨機采樣，搜索效率較低，且得到的路徑一般不是最優(yōu)路徑，不能滿足機器人路徑規(guī)劃要求。

3.2 RRT*算法原理

RRT*算法由RRT 算法改進而來，是一種路徑長度漸進最優(yōu)的算法，其節(jié)點擴展如圖6 所示。生成新節(jié)點xnew后，以xnew為圓心、一定長度r為半徑畫圓，得到xnew周圍的節(jié)點xnear1，xnear2；分別以xnearest，xnear1，xnear2為xnew的父節(jié)點，計算3 條路徑的長度并選出一條從xinit到xnew的最短路徑，圖6 中以xnear2為父節(jié)點的路徑最短，因此用xnear2替換xnearest作為xnew的父節(jié)點。按照上述策略進行擴展，保證了RRT*算法路徑長度的漸進最優(yōu)性，但仍不能解決RRT 算法盲目搜索導致效率低的問題，即雖然RRT*算法能夠?qū)崿F(xiàn)最優(yōu)解收斂，但收斂速度緩慢。

圖6 RRT*算法節(jié)點擴展Fig.6 Node extension of RRT* algorithm

3.3 目標偏置策略

3.3.1 固定概率的目標偏置策略

為了解決RRT*算法缺乏目標性的問題，對RRT*算法的樣本函數(shù)進行改進。傳統(tǒng)RRT*算法在整個空間中隨機選取樣本，其延伸方向是全方位的。如果不使用整個空間的樣本，而是有目標地選擇樣本，則可以決定RRT*搜索樹的擴展方向。

為了使節(jié)點擴展具有一定方向性，設定一個概率值p，使采樣點以概率p等于目標點，引導節(jié)點向目標點搜索[20-21]。目標偏置策略具體實現(xiàn)過程：首先生成一個隨機數(shù)rand，判斷rand 與概率值p的大小，若rand≤p則直接令采樣點等于目標點，不再進行隨機采樣，否則進行隨機采樣。目標偏置策略使得RRT*算法具有一定的目標導向性，同時，以1-p的隨機概率進行采樣，使RRT*算法具有一定避障性能。p值的選取會影響搜索過程和結果，當環(huán)境的障礙物密集時應使p值較小，保持算法對未知空間的探索能力，反之應使p值較大，增強算法的目標導向性。

3.3.2 變概率的目標偏置策略

目標偏置策略可以有效地將隨機樹向目標點引導，但在傳統(tǒng)的目標偏置策略中，目標偏置概率值是在算法執(zhí)行前設定的，不能根據(jù)當前節(jié)點所處環(huán)境進行調(diào)整。當目標點與當前節(jié)點之間沒有障礙物時，若進行大概率的隨機采樣，會造成不必要的曲折路徑；此外，當目標點與當前節(jié)點之間有障礙物且當前節(jié)點在障礙物附近時，若使采樣點等于目標點并引導新節(jié)點向目標點擴展，勢必會造成新節(jié)點與障礙物發(fā)生碰撞。

為了避免產(chǎn)生不必要的曲折路徑及與障礙物的碰撞，本文提出了一種變概率的目標偏置策略：設置目標偏置概率的初始值為p0，在向目標方向擴展時，如果新節(jié)點與障礙物沒有發(fā)生碰撞，則使目標偏置概率值逐步變大，以增強在目標方向上的搜索能力；如果新節(jié)點與障礙物發(fā)生碰撞，則需要進行避障，應減小目標偏置概率值，使目標偏置概率值重新等于p0，以增強隨機采樣能力，避開障礙物。根據(jù)多次實驗結果，目標偏置概率增加值padd取0.1 時效果較好。

固定概率和變概率的目標偏置策略效果對比如圖7 所示。相比于圖7（a），圖7（b）在目標方向上搜索時樹枝更少，且路徑更為平直，靠近障礙物時隨機樹的樹枝也能夠發(fā)散開，以避開障礙物。相比于固定概率的目標偏置策略，變概率的目標偏置策略能夠在保證算法避障性能的前提下，提高算法的目標導向性，避免對可行空間的非必要搜索。

當前，農(nóng)產(chǎn)品質(zhì)量追溯功能實現(xiàn)的關鍵環(huán)節(jié)包括信息采集、信息傳遞、信息載體和信息查詢4個方面。信息采集是實現(xiàn)追溯的基礎，農(nóng)產(chǎn)品質(zhì)量追溯需要采集的信息主要包括農(nóng)產(chǎn)品個體（批次）信息、生產(chǎn)過程信息、投入品使用信息、生產(chǎn)者信息、產(chǎn)地信息等[6]。當農(nóng)產(chǎn)品在市場上流通時，上游生產(chǎn)經(jīng)營主體采集到的信息還需要準確無誤傳遞到下游生產(chǎn)經(jīng)營主體，實現(xiàn)信息傳遞和交換的無縫對接。否則，任何一個環(huán)節(jié)斷了，整個鏈條就斷了，也就無法實現(xiàn)追溯。在信息傳遞過程中，需要有信息載體來承載所采集到的信息，以便于信息在各個主體之間的傳遞和交換，追溯功能的實現(xiàn)還需要公共信息平臺，便于農(nóng)產(chǎn)品質(zhì)量信息匯集和查詢。

圖7 目標偏置策略效果對比Fig.7 Comparison of target bias strategy effects

3.4 G-RRT*算法實現(xiàn)

將變概率的目標偏置策略引入RRT*算法中，得到G-RRT*算法。給定路徑起點xinit和目標點xgoal，設置步長為s，目標偏置概率為p，允許誤差為E，隨機樹為T，G-RRT*算法步驟如下：

（1）初始化隨機樹T，將起點xinit加入T中。

（2）生成一個（0,1）的隨機數(shù)rand，比較rand 與偏置概率p的大小。如果rand≤p，則令采樣點xsample=xgoal；否則在可行空間中進行隨機采樣，得到采樣點xsample。

（3）在T中選擇距離采樣點xsample最近的點作為最近節(jié)點xnearest。

（4）以步長s在xnearest與xsample連線方向上擴展新節(jié)點xnew。

（5）以xnew為圓心、r為半徑畫圓，統(tǒng)計該圓中的節(jié)點集合，并判斷節(jié)點集合中是否存在路徑長度小于當前路徑長度的節(jié)點，若存在則將該節(jié)點作為父節(jié)點并進行碰撞檢測。

（7）判斷xnew與xgoal之間的距離是否小于等于誤差E。若是則停止搜索，并將目標點加入T中，否則重復步驟（2）-步驟（6），直到滿足條件。

（8）回溯父子關系，得到規(guī)劃路徑。

上述過程為G-RRT*算法的一般實現(xiàn)過程，而在機器人上應用時，路徑規(guī)劃是在關節(jié)空間中進行的，而障礙物處于笛卡爾空間中，因此，進行碰撞檢測時需要將得到的新節(jié)點從關節(jié)空間轉(zhuǎn)換到笛卡爾空間中，即通過機器人正運動學求出機器人位姿，再執(zhí)行機器人各連桿與障礙物的碰撞檢測。

4 實驗驗證

為驗證G-RRT*算法的避障和路徑搜索能力，在Matlab 環(huán)境中搭建煤矸石分揀機器人實驗場景。根據(jù)實際機器人運行需求設置相關參數(shù)：機器人每個關節(jié)角的運動范圍均為（-35π/36，35π/36）；輸送帶寬度為0.8 m，運行速度為0.6 m/s；矸石粒度為0.1～0.3 m；以矸石儲放處為分揀機器人位姿起點，矸石正上方0.5 m 處某點為位姿終點，對應的關節(jié)角分別為（-5π/9，7π/18，5π/18，3π/20，0，0）和（π/18，π/6，-4π/9，π/4，0，0）。計算機配置為Intel（R）Core（TM）i5-10400F CPU @2.9 GHz，RAM 為8 GB。

將G-RRT*算法與加入固定概率目標偏置策略的RRT-Connect 算法和RRT 算法進行對比。RRTConnect 是一種雙樹搜索算法，從起點和目標點開始生成2 棵樹同時進行搜索，因此搜索效率更高。本文所提G-RRT*算法與RRT-Connect 算法都由RRT算法改進而來，兩者進行對比，可從橫向說明所提算法的優(yōu)越性。由于未加入目標偏置策略的RRTConnect 算法和RRT 算法盲目性太大，且機器人關節(jié)空間是六維的高維空間，使得2 種算法在有限時間內(nèi)無法搜索到一條可行路徑，所以本文在RRTConnect 算法和RRT 算法基礎上加入固定概率目標偏置策略后再與G-RRT*算法進行對比。

設置3 種算法的目標偏置值均為0.5，選取100 次實驗中路徑長度最接近平均數(shù)的1 次實驗結果，如圖8-圖10 所示，其中X，Y，Z為機器人所在空間的三維坐標軸。在圖8（a）、圖9（a）、圖10（a）中，樹枝形狀的線段表示算法搜索過程，紅色曲線為映射到笛卡爾空間中的機器人末端路徑。由于RRT 算法本身的隨機性，搜索出的路徑并不平滑，難以被機器人直接執(zhí)行，所以本文采用基于五次多項式的最小二乘法對關節(jié)角度進行后處理，使路徑更加光滑，這也是最終路徑?jīng)]有經(jīng)過所有路徑點的原因。由于第1 關節(jié)在笛卡爾空間中的位置是不變的，所以圖8（b）、圖9（b）、圖10（b）中只展示了關節(jié)2-關節(jié)6 的末端路徑，可以看出，各關節(jié)均不與障礙物交涉。由圖8（c）、圖9（c）、圖10（c）可看出，機器人按照規(guī)劃路徑運動可以完美避開障礙物，到達目標點。

圖8 引入固定概率目標偏置的RRT 算法實驗結果Fig.8 Experimental results of RRT algorithm introducing fixed probability target bias

圖9 引入固定概率目標偏置的RRT-Connect 算法實驗結果Fig.9 Experimental results of RRT-Connect algorithm introducing fixed probability target bias

圖10 G-RRT*算法實驗結果Fig.10 Experimental results of G-RRT* algorithm

使用3 種算法進行100 次實驗的結果對比如圖11所示，算法性能參數(shù)對比見表1。在關節(jié)空間中，用關節(jié)角弧度衡量路徑長度。從實驗結果可看出，RRT-Connect 算法搜索效率比其他2 種算法高，但得到的路徑最長。本文的目的是盡可能求得最短路徑，采用G-RRT*算法得到的路徑長度平均值小于其他2 種算法，因而更適用于煤矸石分揀機器人路徑規(guī)劃。

圖11 3 種算法實驗結果對比Fig.11 Comparison of experimental results of three algorithms

表1 算法性能參數(shù)對比Table 1 Comparison of algorithm performance parameters

5 結論

（1）分析了煤矸石分揀系統(tǒng)原理，設計了煤矸石分揀機器人路徑規(guī)劃方案。該方案融合了笛卡爾空間和關節(jié)空間的優(yōu)點，不需要對機器人進行運動學求逆，且可避免在關節(jié)空間中描述障礙物。

（2）針對RRT*路徑規(guī)劃算法存在盲目性的問題，提出一種變概率的目標偏置策略。該策略在無障礙物區(qū)域增大目標偏置概率值，以增強算法的目標導向性，而在障礙物區(qū)域減小目標偏置概率值，以保證算法的避障能力。將變概率的目標偏置策略與RRT*算法相結合，提出G-RRT*算法，既保留了RRT*算法路徑長度漸進最優(yōu)的特點，也提高了算法的目標導向性。

（3）實驗結果表明，與加入固定概率目標偏置策略的RRT-Connect 算法和RRT 算法相比，采用GRRT*算法得到的路徑長度平均值最小，更適用于煤矸石分揀機器人路徑規(guī)劃。

（4）考慮到路徑規(guī)劃過程中步長變化會影響算法效率，在未來的研究工作中將針對步長隨概率變化情況進行相關實驗，以期進一步提高算法效率。同時，考慮將G-RRT*算法應用于煤礦現(xiàn)場真實場景中，以檢測該算法存在的問題，便于提出改進策略。