復(fù)雜多場(chǎng)景下機(jī)械臂避障運(yùn)動(dòng)規(guī)劃方法研究

宋勇, 張蕾, 田榮, 王曉華

1.西安工程大學(xué)電子信息學(xué)院， 陜西 西安 710048；2.陜西人工智能聯(lián)合實(shí)驗(yàn)室（西安工程大學(xué)分部）， 陜西 西安 710048

工業(yè)機(jī)械臂因自身定位精度高、穩(wěn)定性強(qiáng)和執(zhí)行效率高等優(yōu)異性能,廣泛應(yīng)用在裝配、焊接、分揀等工業(yè)生產(chǎn)領(lǐng)域中,是推動(dòng)智能化工業(yè)生產(chǎn)的重要工具[1]。考慮到其工作環(huán)境復(fù)雜多樣,研究多場(chǎng)景情況下的避障運(yùn)動(dòng)規(guī)劃具有重要的工程價(jià)值。

機(jī)械臂在傳統(tǒng)運(yùn)行操作上依賴于工人的示教編程,避障規(guī)劃效率低且適應(yīng)性差。要解決這一問(wèn)題,需要機(jī)械臂能夠自主規(guī)劃出一條無(wú)碰撞路徑以適應(yīng)不同的環(huán)境。機(jī)械臂運(yùn)動(dòng)規(guī)劃是根據(jù)起始點(diǎn)到目標(biāo)點(diǎn)的信息,搜索出一組滿足碰撞檢測(cè)條件的關(guān)節(jié)角度信息,驅(qū)動(dòng)機(jī)械臂從初始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn),順利完成作業(yè)任務(wù)。采用高性能路徑規(guī)劃算法,是實(shí)現(xiàn)機(jī)械臂高效、安全、柔順作業(yè)的關(guān)鍵。

目前,路徑規(guī)劃算法的研究主要分為2類,按照是否在構(gòu)型空間中顯示障礙物區(qū)域,分為組合運(yùn)動(dòng)規(guī)劃算法和基于采樣的運(yùn)動(dòng)規(guī)劃算法。組合運(yùn)動(dòng)規(guī)劃算法包括人工勢(shì)場(chǎng)法[2]、蟻群算法[3]、遺傳算法[4]、A*算法[5]、Dijkstra算法[6]等。其中人工勢(shì)場(chǎng)法在全局路徑規(guī)劃上非常有效,但是存在局部極小值;蟻群與遺傳算法運(yùn)算量太大且耗時(shí)長(zhǎng);A*與Dijkstra算法需要較大空間存儲(chǔ)環(huán)境信息,在低維空間中的路徑規(guī)劃這些算法有一定優(yōu)勢(shì),但對(duì)于在高維空間中執(zhí)行作業(yè)任務(wù)的多自由度機(jī)械臂則不太適用[7]。

基于采樣的路徑規(guī)劃方法通過(guò)碰撞檢測(cè)模塊,不需要顯式構(gòu)建整個(gè)配置空間和邊界,主要算法包含概率路圖法(PRM)和快速擴(kuò)展隨機(jī)樹(shù)算法(rapidly-exploring random tree,RRT)[8],適用于高維空間和復(fù)雜約束下的運(yùn)動(dòng)規(guī)劃問(wèn)題。其中最典型的是基于采樣的快速擴(kuò)展隨機(jī)樹(shù),此算法具有搜索速度快、探索能力強(qiáng)、概率完備性等優(yōu)點(diǎn)[9-10],且不需要對(duì)障礙環(huán)境進(jìn)行建模,能有效解決復(fù)雜的高維空間規(guī)劃問(wèn)題,但也存在規(guī)劃路徑長(zhǎng)度不是最優(yōu)、隨機(jī)性大等缺點(diǎn)。Karaman和Frazzoli于2011年提出漸進(jìn)最優(yōu)性(最短距離)的RRT*算法[11],該算法在RRT的基礎(chǔ)上增加了父節(jié)點(diǎn)擇優(yōu)過(guò)程和鄰節(jié)點(diǎn)回溯重構(gòu)過(guò)程,使規(guī)劃的路徑具有漸進(jìn)最優(yōu)性,弊端是收斂速度慢,收斂時(shí)間逐步增長(zhǎng),產(chǎn)生的冗余節(jié)點(diǎn)過(guò)多[12],以及在避障過(guò)程中所規(guī)劃出的路徑表現(xiàn)為鋸齒狀,并不平滑,從而影響機(jī)械臂的平穩(wěn)運(yùn)行[13]。文獻(xiàn)[14]中生成的新節(jié)點(diǎn)以一定的概率向目標(biāo)方向擴(kuò)展,提升節(jié)點(diǎn)擴(kuò)展的導(dǎo)向性,同時(shí)對(duì)所規(guī)劃的路徑進(jìn)行分段優(yōu)化,優(yōu)化了路徑曲率。文獻(xiàn)[15]提出改進(jìn)的雙樹(shù)拓展策略RRT-connect算法,通過(guò)三角不等式的重接線方法,提高了搜索效率。文獻(xiàn)[16]將雙向人工勢(shì)場(chǎng)與RRT*結(jié)合提出P-RRT*算法,縮短路徑長(zhǎng)度優(yōu)化搜索效率,但是勢(shì)場(chǎng)構(gòu)建可能會(huì)導(dǎo)致局部最優(yōu)。針對(duì)陷入局部最優(yōu)問(wèn)題,文獻(xiàn)[17]引入回歸機(jī)制策略與RRT算法結(jié)合,可以逃逸極小值點(diǎn)。上述研究極大推動(dòng)了機(jī)械臂自主路徑規(guī)劃的進(jìn)程,然而在未來(lái)可預(yù)見(jiàn)的復(fù)雜多場(chǎng)景下適應(yīng)性受限。

綜上所述,為了解決RRT*算法在不同復(fù)雜的二維和三維等多場(chǎng)景下,規(guī)劃效率不高、冗余節(jié)點(diǎn)多、路徑曲折程度大,導(dǎo)致機(jī)械臂運(yùn)行不平穩(wěn)的問(wèn)題,本文提出一種啟發(fā)式概率融合人工勢(shì)場(chǎng)的改進(jìn)型RRT*算法(PAPF-RRT*),并在剔除冗余節(jié)點(diǎn)后進(jìn)行B樣條插值平滑優(yōu)化,規(guī)劃出一條滿足運(yùn)動(dòng)學(xué)約束的平滑路徑,使機(jī)械臂可以平穩(wěn)運(yùn)行。

1 機(jī)械臂建模與碰撞檢測(cè)

1.1 機(jī)械臂運(yùn)動(dòng)學(xué)原理及其模型構(gòu)建

本文以工業(yè)六自由度機(jī)械臂puma560為研究對(duì)象,其URDF模型如圖1所示,其D-H參數(shù)如表1所示。

表1 D-H參數(shù)表

圖1 puma560機(jī)械臂模型

表1中各個(gè)參數(shù)的含義為當(dāng)前關(guān)節(jié)標(biāo)號(hào)為i,逐次遞推其余關(guān)節(jié)標(biāo)號(hào)為i-1,di表示2個(gè)相鄰連桿之間的距離,ai-1代表連桿長(zhǎng)度,αi-1代表連桿i的扭角,θi代表關(guān)節(jié)轉(zhuǎn)角的范圍,其中d2=148.09 mm,d4=433.07 mm,a2=431.8 mm,a3=20.32 mm。

連桿i相對(duì)于連桿i-1的坐標(biāo)變換通式為

(1)

式中:根據(jù)D-H參數(shù)及公式(1)可以得出機(jī)械臂末端位姿變換矩陣如下所示。

(2)

式中:向量n,o,a代表機(jī)械臂末端姿態(tài);x,y,z代表機(jī)械臂末端坐標(biāo)位置;p代表三維坐標(biāo)。

1.2 機(jī)械臂連桿與障礙物空間位置關(guān)系的描述

機(jī)械臂路徑規(guī)劃的前提是要具備精準(zhǔn)有效的碰撞檢測(cè)。但是在實(shí)際運(yùn)行的環(huán)境中,機(jī)械臂本體和障礙物都是不規(guī)則的物體,兩者之間的碰撞檢測(cè)條件很難設(shè)定,因此需要對(duì)機(jī)械臂和障礙物進(jìn)行模型簡(jiǎn)化[18-19]。本文用幾何包絡(luò)法簡(jiǎn)化機(jī)械臂與障礙之間的碰撞模型,用球型包圍盒對(duì)障礙物進(jìn)行包絡(luò),為了精確逼近空間障礙物的占位,包絡(luò)球的直徑定義為障礙物徑向最長(zhǎng)的線段距離,其表達(dá)式為

(3)

式中:(a,b,c)為包絡(luò)球的球心坐標(biāo);r為包絡(luò)球的半徑;(x,y,z)為障礙物的任意一點(diǎn)坐標(biāo)。

將機(jī)械臂連桿用圓柱體完全包裹,如圖2所示,其徑向最大包絡(luò)半徑為R1;將障礙物用球體進(jìn)行完全包裹, 其徑向最大包絡(luò)半徑為R2;進(jìn)而將機(jī)械臂與障礙物的碰撞檢測(cè)轉(zhuǎn)化成判斷規(guī)則幾何體是否相交問(wèn)題。經(jīng)過(guò)包絡(luò)化處理,機(jī)械臂各連桿與障礙物可以簡(jiǎn)化為線段與球體的空間位置關(guān)系,如圖3所示。根據(jù)末端位姿的信息,通過(guò)逆運(yùn)動(dòng)學(xué)求出各個(gè)關(guān)節(jié)的最優(yōu)逆解,再進(jìn)行正解求出空間位置坐標(biāo),即連桿AB的狀態(tài)方程。設(shè)點(diǎn)Q到線段AB的直線距離為dmin,設(shè)R=R1+R2,根據(jù)線段AB與球體的空間位置關(guān)系歸納如下幾種情況:

圖2 模型簡(jiǎn)化示意圖

圖3 碰撞檢測(cè)判斷示意圖

1) 如圖3a)所示,dmin>R,則不發(fā)生碰撞。

2) 如圖3b)所示,dmin

3) 如圖3c)所示,dminR并且|QB|>R,點(diǎn)A、B在點(diǎn)Q的兩側(cè),有2個(gè)交點(diǎn),則發(fā)生碰撞。

4) 如圖3d)所示,dminR并且|QB|>R,點(diǎn)A、B在點(diǎn)Q的同側(cè),無(wú)交點(diǎn),則不發(fā)生碰撞。

2 基本算法原理簡(jiǎn)介

2.1 RRT及RRT*算法原理簡(jiǎn)介

快速擴(kuò)展隨機(jī)樹(shù)算法(RRT)是一種基于隨機(jī)采樣的單步查詢搜索算法,對(duì)環(huán)境障礙物無(wú)需建模,因此很適合高維空間路徑規(guī)劃。

RRT*算法在RRT節(jié)點(diǎn)擴(kuò)展的基礎(chǔ)上,增加了路徑代價(jià)函數(shù),依靠父節(jié)點(diǎn)優(yōu)選過(guò)程和鄰節(jié)點(diǎn)回溯重構(gòu),重新計(jì)算新節(jié)點(diǎn),具備漸進(jìn)最優(yōu)性,且收斂到當(dāng)前最優(yōu),具體過(guò)程如圖4所示。

圖4 父節(jié)點(diǎn)優(yōu)選過(guò)程

父節(jié)點(diǎn)擇優(yōu)選擇過(guò)程,如圖4所示,其目的是通過(guò)鄰域r內(nèi)某個(gè)備選節(jié)去替換新節(jié)點(diǎn),從而降低新節(jié)點(diǎn)的路徑代價(jià),并更新其權(quán)重。

鄰節(jié)點(diǎn)回溯重構(gòu)過(guò)程,如圖5所示,在沒(méi)有發(fā)生碰撞的情況下,若存在其他節(jié)點(diǎn)以Xnew做為父節(jié)點(diǎn)并索引至根節(jié)點(diǎn)的代價(jià)減小,則用Xnew替換該節(jié)點(diǎn)作為新的父節(jié)點(diǎn)。

圖5 鄰節(jié)點(diǎn)回溯重構(gòu)過(guò)程

2.2 改進(jìn)型RRT*算法(PAPF-RRT*)

機(jī)械臂路徑規(guī)劃問(wèn)題是滿足自身運(yùn)動(dòng)學(xué)約束和工作空間的條件下,規(guī)劃從指定起點(diǎn)到目標(biāo)點(diǎn)的路徑,該路徑?jīng)]有發(fā)生碰撞,使機(jī)械臂能高效完成任務(wù)。該問(wèn)題特征總結(jié)為3類:

1) 在復(fù)雜受限等特殊環(huán)境下,RRT*算法規(guī)劃效率低,產(chǎn)生過(guò)多冗余節(jié)點(diǎn);

2) 盡可能優(yōu)化規(guī)劃時(shí)間和路徑長(zhǎng)度;

3) 規(guī)劃出來(lái)的路徑必須滿足機(jī)械臂運(yùn)動(dòng)學(xué)約束,機(jī)械臂才能夠平穩(wěn)運(yùn)行。

2.2.1 混合約束采樣策略

針對(duì)RRT*算法在復(fù)雜受限環(huán)境下搜索效率低的問(wèn)題以及避免無(wú)效區(qū)域的搜索,借鑒啟發(fā)式思想,提出目標(biāo)偏向采樣策略來(lái)優(yōu)化采樣過(guò)程。預(yù)先設(shè)置一個(gè)目標(biāo)偏向閾值Pbias(設(shè)置為0.3),然后由均勻概率分布隨機(jī)產(chǎn)生一個(gè)概率值P,將隨機(jī)樹(shù)擴(kuò)展分為2個(gè)階段,分別為探索階段和收斂階段。核心思想是在隨機(jī)樹(shù)生長(zhǎng)過(guò)程中,如果P≥Pbias,隨機(jī)樹(shù)處于探索階段如圖6a)所示,通過(guò)RandomState函數(shù)隨機(jī)生成采樣點(diǎn)Xsample;隨著迭代次數(shù)的增加,如果P

圖6 概率指數(shù)示意圖

(4)

如果隨機(jī)值小于預(yù)設(shè)的目標(biāo)偏向閾值,則通過(guò)位置約束來(lái)優(yōu)化采樣點(diǎn)。在采樣過(guò)程中建立采樣區(qū),每次采樣時(shí)可將多組采樣點(diǎn)放入采樣區(qū)中,定義采樣區(qū)中距離目標(biāo)點(diǎn)最近且生成的節(jié)點(diǎn)不與障礙物發(fā)生碰撞的點(diǎn)為最優(yōu)點(diǎn)。建立采樣區(qū)過(guò)程示意圖如圖7所示。判斷條件為:計(jì)算當(dāng)前采樣點(diǎn)與目標(biāo)點(diǎn)的距離是否小于上一個(gè)采樣點(diǎn)與目標(biāo)點(diǎn)的距離。若是,則符合判斷條件,采樣成功;否則,繼續(xù)循環(huán)采樣直到滿足條件。

圖7 采樣點(diǎn)優(yōu)選示意圖

圖7設(shè)置第一個(gè)參考采樣點(diǎn)為Xsample1,圖中按照采樣次序以及采樣點(diǎn)到目標(biāo)點(diǎn)的歐氏距離進(jìn)行大小排序Xsample1>Xsample2>Xsample3

2.2.2 隨機(jī)樹(shù)生長(zhǎng)優(yōu)化策略

采樣優(yōu)化函數(shù)雖然能提高采樣的導(dǎo)向性和收斂速度,但未改變新節(jié)點(diǎn)生長(zhǎng)過(guò)程的盲目性,在環(huán)境復(fù)雜時(shí)可能陷入局部極小點(diǎn)和重復(fù)偏置問(wèn)題,降低算法尋跡效率。本文融合人工勢(shì)場(chǎng)法,將目標(biāo)引力函數(shù)G(n)和障礙物斥力函數(shù)R(n)疊加到隨機(jī)樹(shù)節(jié)點(diǎn)Xnear上,則隨機(jī)樹(shù)節(jié)點(diǎn)生長(zhǎng)受采樣點(diǎn)函數(shù)P(n)的約束,同時(shí)也受到目標(biāo)引力和障礙物斥力合力的約束。APF對(duì)隨機(jī)樹(shù)節(jié)點(diǎn)擴(kuò)展的影響如圖8所示,生長(zhǎng)函數(shù)由(5)式表示。

圖8 APF對(duì)節(jié)點(diǎn)生長(zhǎng)的影響

F(n)=P(n)+G(n)+R(n)

(5)

人工勢(shì)場(chǎng)法是局部路徑規(guī)劃方法,基本思想在目標(biāo)點(diǎn)設(shè)置作用于全局的引力場(chǎng),對(duì)擴(kuò)展樹(shù)節(jié)點(diǎn)產(chǎn)生引力作用,障礙物設(shè)置作用于局部范圍的斥力場(chǎng),對(duì)擴(kuò)展樹(shù)節(jié)點(diǎn)產(chǎn)生斥力作用,引力與斥力的合力方向決定隨機(jī)樹(shù)生長(zhǎng)方向,從而降低隨機(jī)樹(shù)生長(zhǎng)的盲目性,加快算法收斂速度。(6)～(7)式為引力場(chǎng)和斥力場(chǎng)勢(shì)函數(shù)

(6)

(7)

Uatt代表引力函數(shù),Ka代表引力常數(shù),Xnear表示隨機(jī)樹(shù)擴(kuò)展的當(dāng)前節(jié)點(diǎn),Xgoal為目標(biāo)點(diǎn),Urep表示斥力函數(shù),Kr為斥力系數(shù),l表示采樣點(diǎn)與障礙物的距離,l0為斥力場(chǎng)作用的范圍,‖Xgoal-Xnear‖=ρ。

目標(biāo)點(diǎn)對(duì)隨機(jī)樹(shù)節(jié)點(diǎn)Xnear的引力為其對(duì)應(yīng)引力場(chǎng)的負(fù)梯度

FA=-Uatt=-Ka‖Xgoal-Xnear‖Δρ

(8)

由(8)式構(gòu)造出節(jié)點(diǎn)Xnear處的目標(biāo)引力函數(shù)G(n),其中FA表示引力方向上的擴(kuò)展系數(shù)。

(9)

同理,環(huán)境中障礙物對(duì)隨機(jī)樹(shù)節(jié)點(diǎn)Xnear產(chǎn)生的斥力為對(duì)應(yīng)斥力場(chǎng)的負(fù)梯度

(10)

由(10)式構(gòu)造出節(jié)點(diǎn)Xnear處的障礙物斥力函數(shù)R(n),其中P0表示環(huán)境中障礙物位置信息,FR表示斥力方向上的擴(kuò)展系數(shù)。

(11)

通過(guò)引入人工勢(shì)場(chǎng)法將斥力與引力疊加到隨機(jī)樹(shù)擴(kuò)展方向上,對(duì)節(jié)點(diǎn)擴(kuò)展方向進(jìn)行修正,引導(dǎo)隨機(jī)樹(shù)生長(zhǎng)的方向和擴(kuò)展步長(zhǎng)大小,促使每一次擴(kuò)展過(guò)程更接近目標(biāo)點(diǎn),加快收斂,增加了算法搜索的靈活性。因此,新節(jié)點(diǎn)被重新配置如公式(12)所示。

(12)

式中:δ為擴(kuò)展固定步長(zhǎng);Xnear為距離采樣點(diǎn)最近鄰節(jié)點(diǎn);‖Xrand-Xnear‖代表Xrand與Xnear之間的歐氏距離。

2.2.3 自適應(yīng)步長(zhǎng)策略

基于隨機(jī)樹(shù)生長(zhǎng)優(yōu)化策略的算法沒(méi)有有效利用擴(kuò)展的節(jié)點(diǎn)信息,忽略了碰撞概率較小的方向,如果算法繼續(xù)朝這個(gè)方向擴(kuò)展,成功擴(kuò)展的可能性會(huì)更大。此外,當(dāng)隨機(jī)樹(shù)向目標(biāo)方向生長(zhǎng)時(shí),如果擴(kuò)展方向上存在障礙物,與隨機(jī)擴(kuò)展相比,該策略將顯著降低算法的避障能力。因此,本文引入了一種自適應(yīng)步長(zhǎng)策略,新節(jié)點(diǎn)的生成方法配置為

(13)

當(dāng)機(jī)械臂的操作環(huán)境比較復(fù)雜或者有較大障礙物時(shí),融合了人工勢(shì)場(chǎng)法的RRT*算法,若始終是固定步長(zhǎng)進(jìn)行搜索,則不能有效利用擴(kuò)展過(guò)程中收集到的信息提高尋跡效率。因此,本文采用自適應(yīng)步長(zhǎng)策略,可以有效利用擴(kuò)展過(guò)程中所收集到的障礙物和環(huán)境信息,更好適應(yīng)節(jié)點(diǎn)的生長(zhǎng)狀況,使得算法更快地穿越障礙物較少的區(qū)域。在隨機(jī)樹(shù)進(jìn)行拓展時(shí)首先采用初始步長(zhǎng),在每一次擴(kuò)展生成新節(jié)點(diǎn)Xnew時(shí),進(jìn)行碰撞檢測(cè),如檢測(cè)到附近沒(méi)有障礙物,則調(diào)整步長(zhǎng)。具體過(guò)程:

1) 預(yù)設(shè)一個(gè)φ值,取值范圍在[0,1],初值設(shè)為零,表示隨機(jī)樹(shù)向通過(guò)采樣獲得的節(jié)點(diǎn)方向擴(kuò)展。

2) 當(dāng)機(jī)械臂操作環(huán)境比較簡(jiǎn)單,增大φ值以加快偏向目標(biāo)生長(zhǎng)。但是φ值不宜設(shè)置過(guò)大,以避免陷入局部最優(yōu),根據(jù)實(shí)際的任務(wù)場(chǎng)景,合理設(shè)置[20]。

3) 當(dāng)機(jī)械臂操作環(huán)境復(fù)雜或需要跨過(guò)較大障礙物時(shí),降低φ值,從而使擴(kuò)展步長(zhǎng)減小并保持?jǐn)U展的隨機(jī)性,使得隨機(jī)樹(shù)繞開(kāi)障礙物生長(zhǎng)。

2.2.4 路徑優(yōu)化策略

RRT*算法及其衍生算法在三維空間中搜索得到一條由離散點(diǎn)所組成的連續(xù)線段,其路徑的形狀比較曲折,平滑性極差,產(chǎn)生大量冗余節(jié)點(diǎn),不利于機(jī)械臂平穩(wěn)運(yùn)行。假設(shè)RRT*算法產(chǎn)生的路徑節(jié)點(diǎn)向量q[q1,q2,q3…qn],對(duì)節(jié)點(diǎn)qa和qb之間路徑進(jìn)行碰撞檢測(cè)(a

圖9 優(yōu)化冗余節(jié)點(diǎn)原理圖

刪除冗余節(jié)點(diǎn)之后,得到的路徑長(zhǎng)度較短,路徑曲率小,且轉(zhuǎn)折點(diǎn)較少。但在轉(zhuǎn)折點(diǎn)處還需要進(jìn)行平滑處理[21],以提高機(jī)械臂運(yùn)行時(shí)的柔順性。三次B樣條曲線具有局部可控的特點(diǎn),能較好滿足路徑規(guī)劃中對(duì)拐點(diǎn)的局部擬合,且不影響整條路徑的基本形狀,增加了軌跡平滑性,保證了各關(guān)節(jié)速度、加速度變化連續(xù)性,且不發(fā)生突變,提高軌跡柔順性。因此,選用B樣條函數(shù)構(gòu)造軌跡,其K階B樣條曲線公式為

(14)

式中:di表示給定曲線的控制點(diǎn);Ni,k(u)表示K階B樣條基函數(shù)。三次B樣條基函數(shù)為

(15)

式中,u表示均勻節(jié)點(diǎn)向量,當(dāng)給定控制頂點(diǎn)后,可以求出一些列滿足B樣條插值的空間序列點(diǎn),從而構(gòu)造出插值曲線。

3 算法仿真分析

為了驗(yàn)證算法PAPF-RRT*的有效性和可靠性,與RRT*、GB-RRT*算法分別在二維和三維場(chǎng)景下進(jìn)行仿真與實(shí)驗(yàn)分析。

3.1 二維仿真

仿真環(huán)境設(shè)置為[100,100],起點(diǎn)設(shè)為[30,80],目標(biāo)點(diǎn)為[70,10],隨機(jī)設(shè)置障礙物,對(duì)RRT*、GB-RRT*和PAPF-RRT*算法進(jìn)行100余次仿真實(shí)驗(yàn),迭代次數(shù)設(shè)置為100,步長(zhǎng)設(shè)置為5,3種算法的仿真結(jié)果和各項(xiàng)指標(biāo)如圖10和表2所示。

表2 二維場(chǎng)景中各算法性能對(duì)比

圖10 二維環(huán)境中各算法性能對(duì)比

紅色線條表示算法尋跡的最終路徑,黑色線條表示拓展樹(shù)分支。通過(guò)對(duì)比可以看出PAPF-RRT*算法相對(duì)于RRT*、GB-RRT*算法在復(fù)雜狹窄環(huán)境中生成的節(jié)點(diǎn)數(shù)較少。通過(guò)圖12和表2數(shù)據(jù)對(duì)比可以看出PAPF-RRT*算法相比RRT*算法,擴(kuò)展無(wú)效結(jié)點(diǎn)數(shù)變少,其節(jié)點(diǎn)使用率由11.89%提升到35.62%,因此占用的內(nèi)存更少;在搜索時(shí)間層面上由2.874 s降低到1.243 s,降低了約56.75%,提高了路徑規(guī)劃效率;平均路徑長(zhǎng)度上從124.63 cm縮短到102.51 cm,減少了約17.74%,且路徑轉(zhuǎn)折程度較低,有利于路徑平滑優(yōu)化;相比GB-RRT*算法,PAPF-RRT*算法在路徑代價(jià)上減少了16.41%,節(jié)點(diǎn)使用率提高了24.50%,搜索時(shí)間上減少了13.5%。相比以上2種算法,本文所提PAPF-RRT*算法在復(fù)雜多障礙環(huán)境中具有良好的路徑規(guī)劃能力。

3.2 三維仿真

為了驗(yàn)證本文所提算法PAPF-RRT*在三維環(huán)境中的有效性,在MATLAB環(huán)境下搭建三維避障環(huán)境,仿真實(shí)驗(yàn)中設(shè)置算法搜索步長(zhǎng)為5,迭代次數(shù)設(shè)置為100,設(shè)置搜索起點(diǎn)為[20,55,10],目標(biāo)點(diǎn)為[40,15,40],仿真環(huán)境地圖設(shè)置為[60×60×60]的三維空間,考慮到算法的隨機(jī)性,重復(fù)進(jìn)行實(shí)驗(yàn)100余次,實(shí)驗(yàn)結(jié)果和各項(xiàng)指標(biāo)如圖11和表3所示。

表3 三維場(chǎng)景中各算法性能對(duì)比

圖11 三維環(huán)境中各算法仿真結(jié)果

藍(lán)色線條表示隨機(jī)樹(shù)的分支,紅色線條表示最終尋跡的路徑,如圖11所示,本文所提算法PAPF-RRT*在三維空間的采樣點(diǎn)比傳統(tǒng)RRT*減少了很多,且規(guī)劃的路徑長(zhǎng)度短、曲折度低。結(jié)合表3中的數(shù)據(jù)可得,本文所提算法相比基本RRT*算法平均路徑長(zhǎng)度由42.73 cm降低到32.93 cm,減少了約22.93%;平均運(yùn)行時(shí)間由6.743 s降低到1.548 s,降低了約77.04%;節(jié)點(diǎn)使用率由9.54%提升到63.21%,從而說(shuō)明規(guī)劃效率的提升,且路徑中折點(diǎn)數(shù)較少,曲線轉(zhuǎn)折程度較低,有利于路徑平滑優(yōu)化。雖然GB-RRT*算法尋跡的路徑代價(jià)相對(duì)較短,因?yàn)槟繕?biāo)偏向方法在三維空間中起到良好作用,但是該算法易陷入局部極小。然而本文所提PAPF-RRT*算法,在搜索路徑的同時(shí)提高了節(jié)點(diǎn)使用率,降低路徑規(guī)劃長(zhǎng)度,且規(guī)劃效率較高。

3.3 路徑平滑性分析

為了進(jìn)一步驗(yàn)證本文所提算法PAPF-RRT*規(guī)劃出的路徑是否滿足機(jī)械臂的運(yùn)動(dòng)學(xué)約束,將機(jī)械臂模型導(dǎo)入,進(jìn)行運(yùn)動(dòng)規(guī)劃仿真實(shí)驗(yàn)。因?yàn)镻APF-RRT*算法所規(guī)劃出來(lái)的路徑是由空間一系列離散的采樣點(diǎn)組成,路徑平滑性差,所以,本文采用基于三次B樣條對(duì)所規(guī)劃出來(lái)的路徑映射到關(guān)節(jié)空間進(jìn)行插值優(yōu)化,平滑優(yōu)化效果如圖12所示。

圖12 機(jī)械臂末端軌跡優(yōu)化效果

圖12表示在局部空間內(nèi),機(jī)械臂末端路徑規(guī)劃圖,其中黑色實(shí)線代表未進(jìn)行插值的路徑,紅色實(shí)線代表經(jīng)過(guò)三次B樣條插值平滑處理后末端的路徑。由仿真結(jié)果發(fā)現(xiàn),插值擬合處理后末端軌跡光滑性明顯增強(qiáng),各關(guān)節(jié)運(yùn)動(dòng)學(xué)曲線如圖13所示。

圖13 各關(guān)節(jié)擬合后角度、速度及加速度曲線

圖13為機(jī)械臂避障運(yùn)行過(guò)程中各關(guān)節(jié)位移、速度、加速度變化曲線,其中各關(guān)節(jié)的起始速度、加速度和終止速度、加速度數(shù)值大小都為零,符合實(shí)際工作情況。在平滑處理機(jī)械臂末端軌跡的同時(shí),機(jī)械臂各關(guān)節(jié)角度、速度和角加速度3項(xiàng)運(yùn)動(dòng)學(xué)參數(shù)曲線變化連續(xù)、穩(wěn)定,無(wú)跳變,降低了各關(guān)節(jié)在運(yùn)行過(guò)程中的沖擊和抖動(dòng),保證了機(jī)臂在規(guī)劃路徑上的高效、平穩(wěn)運(yùn)行,并且各連桿與障礙之間擁有安全裕度,能夠滿足機(jī)械臂的實(shí)際應(yīng)用需求。

為了進(jìn)一步驗(yàn)證本文所提算法對(duì)不同環(huán)境的適應(yīng)性,設(shè)置障礙物個(gè)數(shù)不同的3種環(huán)境。仿真結(jié)果如圖14所示,紅色線條表示本文所提算法插值擬合后規(guī)劃出來(lái)的路徑,黑色線條表示未進(jìn)行路徑優(yōu)化的軌跡,可以看出改進(jìn)后算法規(guī)劃出來(lái)的路徑明顯光滑,路徑中的震蕩現(xiàn)象也有效得到改善;并且隨著障礙物個(gè)數(shù)的增加,本文所提PAPF-RRT*依然能夠引導(dǎo)機(jī)械臂順利避障,證明了算法的有效性。

圖14 算法對(duì)環(huán)境復(fù)雜程度的適應(yīng)性實(shí)驗(yàn)

4 結(jié) 論

本文以六自由度工業(yè)機(jī)械臂在智能工廠操作業(yè)務(wù)為背景,在不同復(fù)雜程度的的多場(chǎng)景中,針對(duì)傳統(tǒng)RRT*算法進(jìn)行路徑規(guī)劃時(shí)存在隨機(jī)性強(qiáng)、冗余節(jié)點(diǎn)過(guò)多、路徑不平滑等問(wèn)題進(jìn)行改進(jìn),提出一種基于PAPF-RRT*結(jié)合B樣條插值的機(jī)械臂運(yùn)動(dòng)規(guī)劃算法。本文主要改進(jìn)有如下三點(diǎn):

1) 在隨機(jī)樹(shù)擴(kuò)展過(guò)程中引入混合約束采樣策略,在采樣點(diǎn)偏向擴(kuò)展的基礎(chǔ)上,通過(guò)采樣區(qū)優(yōu)選策略,提高采樣點(diǎn)的導(dǎo)向性與質(zhì)量。

2) 將節(jié)點(diǎn)擴(kuò)展方向與人工勢(shì)場(chǎng)合力方向進(jìn)行融合,使得擴(kuò)展方向具備目標(biāo)點(diǎn)吸引和障礙物排斥的趨勢(shì),同時(shí)引入自適應(yīng)步長(zhǎng)策略,減少擴(kuò)展的盲目性,加快收斂速度。

3) 通過(guò)去除冗余節(jié)點(diǎn)并進(jìn)行二次插值優(yōu)化,改善路徑平滑性,以滿足機(jī)械臂運(yùn)動(dòng)學(xué)約束。

仿真結(jié)果表明PAPF-RRT*算法在有效避障的同時(shí),提高了規(guī)劃效率以及節(jié)點(diǎn)使用率,使規(guī)劃的路徑長(zhǎng)度更優(yōu)。同時(shí)在規(guī)劃后的路徑點(diǎn)間進(jìn)行B樣條插值構(gòu)造軌跡,提升路徑平滑性。導(dǎo)入機(jī)械臂模型進(jìn)行仿真,關(guān)節(jié)空間的各項(xiàng)運(yùn)動(dòng)學(xué)參數(shù)變化連續(xù)、穩(wěn)定,同時(shí)也優(yōu)化了機(jī)械臂末端執(zhí)行器的軌跡,降低了規(guī)劃時(shí)間,使規(guī)劃的軌跡更有利于機(jī)器人的跟蹤執(zhí)行,具有較好的發(fā)展應(yīng)用前景。