基于G-RRT*算法的煤矸石分揀機(jī)器人路徑規(guī)劃

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

（1）煤矸石識(shí)別子系統(tǒng)能夠提供精確的矸石位置和對(duì)應(yīng)的機(jī)器人關(guān)節(jié)角度值。

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

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

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

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

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

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

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

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

圖3 機(jī)器人機(jī)械結(jié)構(gòu)Fig.3 Robot mechanical structure

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

圖4 機(jī)器人連桿和障礙物簡(jiǎn)化模型Fig.4 Simplified model of robot link and obstacle

3 路徑規(guī)劃算法

3.1 RRT 算法原理

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

圖5 RRT 算法節(jié)點(diǎn)擴(kuò)展Fig.5 Node extension of RRT algorithm

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

3.2 RRT*算法原理

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

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

3.3 目標(biāo)偏置策略

3.3.1 固定概率的目標(biāo)偏置策略

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

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

3.3.2 變概率的目標(biāo)偏置策略

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

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

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

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

圖7 目標(biāo)偏置策略效果對(duì)比Fig.7 Comparison of target bias strategy effects

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

將變概率的目標(biāo)偏置策略引入RRT*算法中，得到G-RRT*算法。給定路徑起點(diǎn)xinit和目標(biāo)點(diǎn)xgoal，設(shè)置步長(zhǎng)為s，目標(biāo)偏置概率為p，允許誤差為E，隨機(jī)樹(shù)為T，G-RRT*算法步驟如下：

（1）初始化隨機(jī)樹(shù)T，將起點(diǎn)xinit加入T中。

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

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

（4）以步長(zhǎng)s在xnearest與xsample連線方向上擴(kuò)展新節(jié)點(diǎn)xnew。

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

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

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

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

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

圖8 引入固定概率目標(biāo)偏置的RRT 算法實(shí)驗(yàn)結(jié)果Fig.8 Experimental results of RRT algorithm introducing fixed probability target bias

圖9 引入固定概率目標(biāo)偏置的RRT-Connect 算法實(shí)驗(yàn)結(jié)果Fig.9 Experimental results of RRT-Connect algorithm introducing fixed probability target bias

圖10 G-RRT*算法實(shí)驗(yàn)結(jié)果Fig.10 Experimental results of G-RRT* algorithm

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

圖11 3 種算法實(shí)驗(yàn)結(jié)果對(duì)比Fig.11 Comparison of experimental results of three algorithms

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

5 結(jié)論

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

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

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

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