張立彬,林后凱,譚大鵬

(浙江工業(yè)大學(xué) 機(jī)械工程學(xué)院,浙江 杭州 310014)

0 引言

面向智能制造2025，我國(guó)智能機(jī)器人技術(shù)正處于從低端制造向智能化、精益化制造轉(zhuǎn)型時(shí)期。作為智能機(jī)器人的一種，移動(dòng)機(jī)械臂可以代替人在救災(zāi)、采礦、高空裝配、檢測(cè)焊接、行星探測(cè)等危險(xiǎn)環(huán)境中工作[1-4]。由于移動(dòng)機(jī)械臂作業(yè)的工作環(huán)境并不固定，針對(duì)環(huán)境變化快速做出反應(yīng)并尋找通往目標(biāo)點(diǎn)的無(wú)碰撞較優(yōu)路徑成為當(dāng)前機(jī)械臂避障技術(shù)攻關(guān)的難點(diǎn)，開(kāi)展不同復(fù)雜障礙物環(huán)境下機(jī)械臂的快速避障路徑規(guī)劃具有重要的工程意義[5-7]。

避障路徑規(guī)劃指在工作空間存在障礙物的情況下，機(jī)器臂在較短時(shí)間內(nèi)規(guī)劃一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰路徑[8-11]。因?yàn)闄C(jī)械臂屬于高維本體結(jié)構(gòu)，所以路徑規(guī)劃時(shí)復(fù)雜度較高，目前在國(guó)內(nèi)外提出的多種規(guī)劃方法中比較優(yōu)秀的有采樣法和局部規(guī)劃法[12-13]。KALA[14]提出隨機(jī)路標(biāo)圖法(Probabilistic Roadmap, PRM)，其采用完全隨機(jī)采樣策略將連續(xù)空間轉(zhuǎn)換為離散空間來(lái)構(gòu)建路徑網(wǎng)絡(luò)圖，再采用A*等搜索算法在路線圖上快速規(guī)劃無(wú)碰路徑；曹博等[15]提出改進(jìn)人工勢(shì)場(chǎng)法(Artificial Potential Field, APF)，在機(jī)械臂關(guān)節(jié)空間內(nèi)建立目標(biāo)角度引力勢(shì)場(chǎng)，在笛卡爾空間內(nèi)建立末端引力勢(shì)場(chǎng)和障礙物斥力勢(shì)場(chǎng)，通過(guò)所有勢(shì)場(chǎng)共同作用引導(dǎo)機(jī)械臂運(yùn)動(dòng)；AKGUN等[16]結(jié)合快速搜索隨機(jī)樹(shù)算法(Rapidly exploring Random Tree star, RRT*)提出漸進(jìn)最優(yōu)的B-RRT*(bidirectional RRT*)，其用RRT*代替RRT擴(kuò)展找到初始路徑后，設(shè)定采樣約束減少無(wú)效節(jié)點(diǎn)，通過(guò)局部偏置優(yōu)化路徑上的節(jié)點(diǎn)至局部最優(yōu)；謝碧云等[17]提出雙樹(shù)隨機(jī)樹(shù)搜索(bi-directional Rapidly-exploring Random Tree, bi-RRT)算法，該算法結(jié)合末端姿態(tài)調(diào)整和關(guān)節(jié)自運(yùn)動(dòng)，利用對(duì)機(jī)械臂逆解生成目標(biāo)點(diǎn)的樹(shù)根，在給定的障礙物環(huán)境中自動(dòng)選擇某一合適的位形作為目標(biāo)節(jié)點(diǎn)來(lái)引導(dǎo)搜索樹(shù)最有效地生長(zhǎng)；李洋等[18]提出引力自適應(yīng)步長(zhǎng)RRT在工作空間對(duì)步長(zhǎng)進(jìn)行有效約束，減少了迭代次數(shù)，降低了運(yùn)行時(shí)間并縮短了路徑長(zhǎng)度；劉曉倩等[19]提出融合目標(biāo)偏向優(yōu)點(diǎn)的目標(biāo)偏向快速搜索隨機(jī)樹(shù)(Goal Bias Rapidly-exploring Random Tree, GB_RRT)，通過(guò)提升RRT的導(dǎo)向性來(lái)減少搜索樹(shù)葉節(jié)點(diǎn)的數(shù)量，從而提高RRT搜索樹(shù)的效率。

通過(guò)上述分析可以發(fā)現(xiàn)，避障路徑規(guī)劃技術(shù)在機(jī)械臂運(yùn)動(dòng)領(lǐng)域獲得了廣泛應(yīng)用，并取得較好的研究結(jié)果，然而上述研究多集中于提升某一特性，尚不能適應(yīng)不同的復(fù)雜障礙物環(huán)境。基于完全隨機(jī)采樣的RRT，PRM，RRT*，B-RRT*和引力自適應(yīng)步長(zhǎng)RRT在多障礙物環(huán)境下優(yōu)勢(shì)明顯，但在救災(zāi)、采礦、高空裝配、焊接、行星探測(cè)等簡(jiǎn)單障礙物工作環(huán)境下，因?yàn)闄C(jī)械臂采用隨機(jī)采樣策略，所以難以在較短的時(shí)間內(nèi)規(guī)劃出較優(yōu)的路徑。GB_RRT*和APF算法雖然在簡(jiǎn)單障礙物環(huán)境中的規(guī)劃性能較好，但是APF算法在多障礙物環(huán)境工作時(shí)容易陷入局部極值點(diǎn)，使所規(guī)劃的路徑代價(jià)過(guò)高，而GB_RRT*算法在多障礙物環(huán)境下因存在“徘徊探索”現(xiàn)象而導(dǎo)致搜索時(shí)間過(guò)大。

針對(duì)上述問(wèn)題，本文采用基于柵格空間的自適應(yīng)目標(biāo)偏向快速擴(kuò)展隨機(jī)樹(shù)算法(Rapidly exploring Random Tree Star Algorithm for adaptive Goal Bias based on grid space, SAGB_RRT*)在關(guān)節(jié)空間對(duì)移動(dòng)機(jī)械臂進(jìn)行避障路徑規(guī)劃：①結(jié)合RRT*漸進(jìn)最優(yōu)思想對(duì)GB_RRT進(jìn)行優(yōu)化，使搜索路徑朝漸進(jìn)最優(yōu)解收斂；②通過(guò)柵格儲(chǔ)存樹(shù)節(jié)點(diǎn)并結(jié)合柵格快速查找相鄰節(jié)點(diǎn)來(lái)提升算法效率；③利用open表解決目標(biāo)偏向采樣策略引起的重復(fù)偏向問(wèn)題；④結(jié)合open表節(jié)點(diǎn)數(shù)量控制偏向性生長(zhǎng)值來(lái)縮減無(wú)效擴(kuò)展，降低搜索時(shí)間和路徑代價(jià)；⑤采用可變間隔的貪心算法進(jìn)一步改善規(guī)劃所得路徑的曲折性，并降低規(guī)劃所得的路徑代價(jià)。與其他避障路徑規(guī)劃算法相比，本文算法在不同復(fù)雜障礙物環(huán)境下能夠用更短的規(guī)劃搜索時(shí)間規(guī)劃出更優(yōu)的避障路徑。本文研究可以為路徑規(guī)劃領(lǐng)域的理論研究提供有益的參考，也可以為智能機(jī)器人的系統(tǒng)研發(fā)提供技術(shù)支持。

1 GB_RRT*算法原理

GB_RRT*算法是結(jié)合RRT*漸進(jìn)最優(yōu)思想對(duì)目標(biāo)偏向RRT進(jìn)行優(yōu)化，使搜索路徑朝漸進(jìn)最優(yōu)解收斂，其主要通過(guò)擴(kuò)展隨機(jī)樹(shù)、重選父節(jié)點(diǎn)和重布線對(duì)避障路徑進(jìn)行優(yōu)化[20-21]。

如圖1所示，擴(kuò)展隨機(jī)樹(shù)時(shí)首先將初始節(jié)點(diǎn)Xinit作為所構(gòu)建隨機(jī)樹(shù)T的根節(jié)點(diǎn)，在擴(kuò)展下一節(jié)點(diǎn)時(shí)主要采用目標(biāo)偏向閾值P控制采樣節(jié)點(diǎn)Xrand的生長(zhǎng)，來(lái)影響隨機(jī)樹(shù)擴(kuò)展的方向，降低了隨機(jī)樹(shù)搜索過(guò)程中的無(wú)目的性。閾值P對(duì)采樣節(jié)點(diǎn)Xrand的控制是通過(guò)比較隨機(jī)產(chǎn)生概率Prand和閾值P的大小來(lái)實(shí)現(xiàn)，主要有兩種情況：

(1)當(dāng)Prand≥P時(shí)，隨機(jī)樹(shù)傾于向未探索過(guò)的空間擴(kuò)展，即在空間中隨機(jī)選擇任意狀態(tài)點(diǎn)作為Xrand。

(2)當(dāng)Prand

通過(guò)上述方式確定Xrand后，遍歷隨機(jī)樹(shù)找到距離Xrand最近的樹(shù)節(jié)點(diǎn)Xnear，在碰撞檢測(cè)合格后經(jīng)固定步長(zhǎng)擴(kuò)展得到新節(jié)點(diǎn)Xnew，然后將Xnew添加到隨機(jī)樹(shù)中并記錄其父節(jié)點(diǎn)和節(jié)點(diǎn)權(quán)值。

如圖2所示，重選父節(jié)點(diǎn)的意義在于通過(guò)鄰域r內(nèi)的其他節(jié)點(diǎn)降低Xnew節(jié)點(diǎn)的權(quán)值。首先計(jì)算Xnew連接鄰域r內(nèi)所有節(jié)點(diǎn)的權(quán)值，若存在能使Xnew權(quán)值更小且不碰撞的Xmin節(jié)點(diǎn)，則刪除Xnew與原父節(jié)點(diǎn)的連線，同時(shí)將Xmin節(jié)點(diǎn)作為新的父節(jié)點(diǎn)，更新Xnew的權(quán)值。

如圖3所示，重布線過(guò)程的意義在于通過(guò)Xnew降低其他節(jié)點(diǎn)的權(quán)值，在不與障礙物碰撞的前提下，若其他節(jié)點(diǎn)通過(guò)將Xnew作為父節(jié)點(diǎn)來(lái)降低權(quán)值，則將Xnew節(jié)點(diǎn)作為其新父節(jié)點(diǎn)并更新節(jié)點(diǎn)權(quán)值。

2 SAGB_RRT*算法

雖然GB_RRT*算法具有漸進(jìn)最優(yōu)的特點(diǎn)，但是在為移動(dòng)機(jī)械臂規(guī)劃時(shí)無(wú)法給予CPU足夠的運(yùn)算時(shí)間，而且在不同環(huán)境下無(wú)法變換目標(biāo)偏向閾值，缺乏對(duì)環(huán)境的適應(yīng)性，造成算法無(wú)法在不同復(fù)雜障礙物環(huán)境下用較短的時(shí)間規(guī)劃出較優(yōu)的避障路徑[22]。本文基于GB_RRT*框架，提出適合移動(dòng)機(jī)械臂在救災(zāi)、采礦、高空裝配、焊接、行星探測(cè)等環(huán)境下的SAGB_RRT*(grid space-based adaptive goal bias rapidly exploring random tree star)運(yùn)動(dòng)規(guī)劃算法，該算法在原算法概率完備和漸進(jìn)最優(yōu)性不變的前提下，通過(guò)構(gòu)建全狀態(tài)空間柵格地圖的數(shù)據(jù)存儲(chǔ)結(jié)構(gòu)優(yōu)化查找相鄰節(jié)點(diǎn)的方法來(lái)提升計(jì)算速度，采用open表生長(zhǎng)策略優(yōu)化隨機(jī)樹(shù)的生長(zhǎng)方式以避免重復(fù)偏置并控制偏向性生長(zhǎng)，結(jié)合可變間隔貪婪算法以快速優(yōu)化已規(guī)劃路徑，使機(jī)械臂在適應(yīng)各種環(huán)境的同時(shí)能用較短的搜索時(shí)間規(guī)劃出較優(yōu)的避障路徑。

2.1 節(jié)點(diǎn)查找優(yōu)化

機(jī)械臂屬于高維本體結(jié)構(gòu)，為其規(guī)劃路徑時(shí)需要數(shù)量龐大的樹(shù)節(jié)點(diǎn)，造成GB_RRT*算法全遍歷查找相鄰節(jié)點(diǎn)的復(fù)雜度呈指數(shù)上升，導(dǎo)致算法搜索時(shí)間爆炸式增長(zhǎng)。為避免原算法在查找相鄰節(jié)點(diǎn)時(shí)遍歷所有樹(shù)節(jié)點(diǎn)的時(shí)間代價(jià)過(guò)大，本文算法結(jié)合工作空間的大小構(gòu)建全狀態(tài)空間的柵格地圖來(lái)存儲(chǔ)數(shù)據(jù)，通過(guò)映射方式分塊整理樹(shù)節(jié)點(diǎn)，并通過(guò)查找柵格遍歷部分樹(shù)節(jié)點(diǎn)，從而降低算法搜索時(shí)間。

在二維空間內(nèi)對(duì)柵格地圖的原理進(jìn)行表述，如圖4所示，在確保小柵格邊長(zhǎng)s大于步長(zhǎng)λ和重布線半徑r的前提下，結(jié)合工作空間大小確定柵格s的值后構(gòu)建柵格地圖。

構(gòu)建柵格地圖后，每個(gè)節(jié)點(diǎn)存儲(chǔ)時(shí)都有對(duì)應(yīng)的柵格，通過(guò)各軸向劃分柵格數(shù)和節(jié)點(diǎn)柵格坐標(biāo)得到節(jié)點(diǎn)的柵格索引值，可以實(shí)現(xiàn)節(jié)點(diǎn)和柵格索引值的映射。隨機(jī)樹(shù)任意節(jié)點(diǎn)的柵格坐標(biāo)均由其已知工作空間坐標(biāo)換算得到，具體為：

(1)

(2)

式中：函數(shù)int表示對(duì)所求值取整；(x0，y0)為節(jié)點(diǎn)位置坐標(biāo)，(x，y)為樹(shù)節(jié)點(diǎn)柵格坐標(biāo)；s為每個(gè)小柵格的邊長(zhǎng)。

通過(guò)構(gòu)建映射關(guān)系快速查找相鄰節(jié)點(diǎn)的原理如圖5所示，在擴(kuò)展生長(zhǎng)和重選父節(jié)點(diǎn)查找Xnear時(shí)，改進(jìn)算法只需通過(guò)映射關(guān)系得到其柵格索引值，即可快速查找Xrand周邊的柵格群，然后在Xrand周邊柵格群遍歷查找樹(shù)節(jié)點(diǎn)Xnear。如圖6所示，在重布線階段查找χnear時(shí)，改進(jìn)算法通過(guò)映射關(guān)系得到的柵格索引值快速查找到Xnew周邊柵格群，然后在Xrand周邊柵格群遍歷查找樹(shù)節(jié)點(diǎn)群χnear。

本文通過(guò)優(yōu)化節(jié)點(diǎn)查找方式提升規(guī)劃效率。因?yàn)闃?gòu)建柵格地圖只是簡(jiǎn)單地根據(jù)工作空間確定每個(gè)小存儲(chǔ)柵格的大小，相比全遍歷樹(shù)節(jié)點(diǎn)查找相鄰節(jié)點(diǎn)的方式，其在時(shí)間和空間的復(fù)雜度基本忽略不計(jì)，所以對(duì)算法規(guī)劃性能的分析更多在查找相鄰節(jié)點(diǎn)的時(shí)間和空間復(fù)雜度上。然而，隨著隨機(jī)樹(shù)的不斷生長(zhǎng)，樹(shù)節(jié)點(diǎn)不斷增多，原算法在擴(kuò)展生長(zhǎng)、重選父節(jié)點(diǎn)和重布線時(shí)查找相鄰節(jié)點(diǎn)所采用的方式是遍歷所有樹(shù)節(jié)點(diǎn)，其時(shí)間復(fù)雜度與樹(shù)節(jié)點(diǎn)個(gè)數(shù)為指數(shù)倍關(guān)系，改進(jìn)算法只需從當(dāng)前節(jié)點(diǎn)所在柵格與其周邊柵格中的樹(shù)節(jié)點(diǎn)遍歷查找相鄰節(jié)點(diǎn)，時(shí)間復(fù)雜度維持在一定范圍內(nèi)，大大降低了時(shí)間復(fù)雜度。

為了驗(yàn)證查找節(jié)點(diǎn)優(yōu)化方法的有效性，在障礙物、步長(zhǎng)等條件均相同的情況下，分別采用原算法和GB_RRT*算法對(duì)6關(guān)節(jié)機(jī)械臂進(jìn)行規(guī)劃。比較相同迭代次數(shù)所用的CPU計(jì)算時(shí)間如圖7所示，可見(jiàn)在相同迭代次數(shù)下，基于柵格化RRT*所需的搜索時(shí)間明顯小于RRT*算法，其中當(dāng)?shù)螖?shù)為30 000時(shí)所需的搜索時(shí)間從1 695.84 s下降到679.17 s，降低了65%，可知結(jié)合工作空間的大小構(gòu)建全狀態(tài)空間的柵格地圖進(jìn)行節(jié)點(diǎn)的相互映射，通過(guò)映射關(guān)系快速查找相鄰節(jié)點(diǎn)的方法能夠有效降低算法因樹(shù)節(jié)點(diǎn)數(shù)量增加而導(dǎo)致的計(jì)算量指數(shù)爆炸問(wèn)題，從而證明本文查找節(jié)點(diǎn)優(yōu)化方法的有效性。

2.2 生長(zhǎng)方式優(yōu)化

GB_RRT*算法在規(guī)劃時(shí)由于目標(biāo)偏向會(huì)出現(xiàn)重復(fù)，造成時(shí)間浪費(fèi)。當(dāng)選擇的P值較低時(shí)，由于隨機(jī)樹(shù)生長(zhǎng)偏向性弱，GB_RRT*算法在簡(jiǎn)單環(huán)境中會(huì)進(jìn)行大量無(wú)效搜索；當(dāng)P值較高時(shí)，由于隨機(jī)樹(shù)生長(zhǎng)偏向性強(qiáng)，GB_RRT*算法在復(fù)雜情況下，尤其是障礙物遮擋目標(biāo)時(shí)，極易陷入局部偏向而重復(fù)產(chǎn)生大量失敗的生長(zhǎng)，使算法對(duì)環(huán)境的適應(yīng)性較差。為避免重復(fù)偏向并保證隨機(jī)樹(shù)生長(zhǎng)時(shí)對(duì)環(huán)境有較強(qiáng)的適應(yīng)性，引入自適應(yīng)學(xué)習(xí)的思想并構(gòu)建可存儲(chǔ)目標(biāo)偏向生長(zhǎng)點(diǎn)的open表，將成功擴(kuò)展所得的Xnew作為目標(biāo)偏向生長(zhǎng)點(diǎn)存入open表，在進(jìn)行隨機(jī)樹(shù)擴(kuò)展時(shí)，從open表中查找離Xgoal最近的Xnear節(jié)點(diǎn)生長(zhǎng),不管生長(zhǎng)能否成功均將Xnear節(jié)點(diǎn)作為不可偏向生長(zhǎng)點(diǎn)從open表中刪除，通過(guò)節(jié)點(diǎn)擴(kuò)展時(shí)的搜索方向和反饋的搜索狀態(tài)控制open表中的節(jié)點(diǎn)數(shù)量來(lái)控制目標(biāo)偏向閾值的變化，從而自適應(yīng)調(diào)整目標(biāo)偏向閾值P。

通過(guò)自適應(yīng)調(diào)整P值來(lái)增強(qiáng)算法對(duì)環(huán)境的感知能力，其原理在于，open表中的節(jié)點(diǎn)數(shù)量越大，可偏向生長(zhǎng)的樹(shù)節(jié)點(diǎn)越多，應(yīng)該縮減隨機(jī)樹(shù)探索未知空間的能力，而將主要精力放在目標(biāo)偏向生長(zhǎng)上，即增大P值；open表中的節(jié)點(diǎn)數(shù)量越少，可偏向生長(zhǎng)的樹(shù)節(jié)點(diǎn)越少，繼續(xù)向目標(biāo)偏向生長(zhǎng)極易造成堆積在障礙物附近的樹(shù)節(jié)點(diǎn)過(guò)多，因此當(dāng)節(jié)點(diǎn)數(shù)量較少時(shí)，應(yīng)增強(qiáng)隨機(jī)樹(shù)探索未知空間的能力，避免隨機(jī)樹(shù)陷入局部障礙物處難以自拔，即減小P值。圖8所示為自適應(yīng)調(diào)整目標(biāo)偏向閾值P的基本框架。

由于SAGB_RRT*算法在規(guī)劃時(shí)對(duì)open表中節(jié)點(diǎn)的數(shù)量完全可知，可以通過(guò)調(diào)節(jié)open表中的節(jié)點(diǎn)數(shù)量在某一范圍內(nèi)自適應(yīng)調(diào)整目標(biāo)偏向閾值P，即

(3)

式中：Pmin為目標(biāo)偏向的最小閾值，Pmax為目標(biāo)偏向的最大閾值；Knode為open表中當(dāng)前節(jié)點(diǎn)的數(shù)量；Kmax為open表中所能存放節(jié)點(diǎn)數(shù)量的最大值，

(4)

式中：α為系數(shù)；‖Xgoal-Xstart‖為歐拉范數(shù)定義的目標(biāo)點(diǎn)到起點(diǎn)的距離；γ為機(jī)械臂關(guān)節(jié)維度的權(quán)值；θ為工作空間的維度；λ為隨機(jī)樹(shù)擴(kuò)展的步長(zhǎng)。

由式(3)和式(4)可知，引入自適應(yīng)學(xué)習(xí)思想生成目標(biāo)偏向閾值P的表達(dá)式為

P=Pmin+(Pmax-Pmin)·

(5)

采用上述方式，結(jié)合open表使隨機(jī)樹(shù)在未偏向生長(zhǎng)的節(jié)點(diǎn)中選擇偏向生長(zhǎng)，以避免隨機(jī)樹(shù)重復(fù)偏向生長(zhǎng)，從而縮減無(wú)效擴(kuò)展，提升算法效率。通過(guò)自適應(yīng)調(diào)整目標(biāo)偏向閾值P，使算法在規(guī)劃時(shí)能夠用更少的迭代次數(shù)、更強(qiáng)的偏向性令路徑快速收斂。

為進(jìn)一步證明優(yōu)化SAGB_RRT*算法生長(zhǎng)方式的效果，在5種不同環(huán)境下分別采用SAGB_RRT*算法和不同閾值的GB_RRT*算法進(jìn)行規(guī)劃，如圖9所示。由于兩種算法均基于采樣使規(guī)劃結(jié)果具有不確定性，為使實(shí)驗(yàn)結(jié)果更具說(shuō)服力，每個(gè)算法均仿真分析15次，結(jié)果取平均值。由圖9a和圖9c可見(jiàn)，在5種環(huán)境下，SAGB_RRT*算法更優(yōu)的原因在于引入open表解決了偏向重復(fù)，同時(shí)采用目標(biāo)閾值P自適應(yīng)優(yōu)化了生長(zhǎng)方式；由圖9b和圖9d可見(jiàn)，相比GB_RRT*算法，SAGB_RRT*算法所需的樹(shù)節(jié)點(diǎn)和失敗生長(zhǎng)點(diǎn)均較少，證明了本文SAGB_RRT*算法的有效性。

2.3 路徑優(yōu)化

GB_RRT*是基于隨機(jī)采樣的搜索算法，雖然利用目標(biāo)偏向生長(zhǎng)和重布線策略進(jìn)行優(yōu)化，但是仍然存在曲折路徑，造成最終規(guī)劃的路徑代價(jià)較高。貪婪算法能夠?qū)σ阎窂竭M(jìn)行貪心優(yōu)化，使其不斷接近最優(yōu)解，為了能在較短時(shí)間內(nèi)使路徑總代價(jià)最低，引入可變間隔m的貪心算法對(duì)SAGB_RRT*算法規(guī)劃得到的路徑進(jìn)行快速優(yōu)化，其中路徑代價(jià)目標(biāo)函數(shù)

(6)

式中：C為機(jī)械臂相鄰關(guān)節(jié)角的路徑代價(jià)；θi為機(jī)械臂的第i關(guān)節(jié)角。

利用可變間隔m的貪心算法對(duì)已知路徑進(jìn)行優(yōu)化，首先將Xinit作為起始節(jié)點(diǎn)，為保證算法的時(shí)效性，依次優(yōu)化從起始節(jié)點(diǎn)到相隔m值的后續(xù)節(jié)點(diǎn)的路徑，將起始節(jié)點(diǎn)和后續(xù)節(jié)點(diǎn)通過(guò)正運(yùn)動(dòng)學(xué)求解后得到機(jī)械臂的運(yùn)動(dòng)狀態(tài)，并在笛卡爾空間求得機(jī)械臂在該運(yùn)動(dòng)路徑下的碰撞情況。當(dāng)與障礙物碰撞時(shí)，結(jié)合起始節(jié)點(diǎn)的當(dāng)前后續(xù)節(jié)點(diǎn)和上個(gè)后續(xù)節(jié)點(diǎn)依次逆向查找，若從起始節(jié)點(diǎn)到所查節(jié)點(diǎn)的路徑與障礙物不碰撞則停止搜索。將起始節(jié)點(diǎn)與該節(jié)點(diǎn)相連作為新的路徑，以該節(jié)點(diǎn)為起始節(jié)點(diǎn)繼續(xù)進(jìn)行間隔m的貪婪搜索，如此反復(fù)直到終點(diǎn)，停止搜索。

通過(guò)上述方法優(yōu)化已知路徑，當(dāng)已知路徑中的障礙物較少時(shí)采用較大的可變間隔，以減少碰撞檢測(cè)次數(shù)，加快規(guī)劃速度，因此應(yīng)根據(jù)起始目標(biāo)距離、所得路徑和搜索時(shí)的失敗生長(zhǎng)點(diǎn)數(shù)選擇m值，具體為

(7)

式中：dist(path)為已規(guī)劃路徑；dist(Xgoal,Xstart)為起點(diǎn)到終點(diǎn)的距離；Qinvalid為失敗生長(zhǎng)節(jié)點(diǎn)數(shù)。

3 算法實(shí)現(xiàn)

基于上述改進(jìn)思想，為保證移動(dòng)機(jī)械臂在救災(zāi)、采礦、高空裝配、檢測(cè)焊接、行星探測(cè)等危險(xiǎn)環(huán)境中能夠用較短的搜索時(shí)間快速規(guī)劃較優(yōu)的避障路徑，提出一種避障規(guī)劃SAGB_RRT*算法，其偽代碼如下：

算法1SAGB_RRT*。

1. Wg←Workspace_Grid(W)

2. T←Initialize_Tree()

3. For k=1:k do

4. P←Update_P(Kopen,Kmax)

5. Prand←rand(0～1)

6. If Prand>P

7. Xrand←Random_State()

8. else

9. Xrand←Xgoal

10. χrandgrids←Find_Grids_Near(Xrand,Wg)

11. Xcloset←Find_nodes_Near(χrandgrids,Xrand)

12. Xnew←Steer(Xrand,Xcloset,λ)

13. If Obstacle_Free(Xnew)

14. Xnew←New_State(Xcloset,λ),Open←Add(Xnew)

15. If Xrand=Xgoal

16. Open←Delete(Xcloset)

17. Xmin=Xnear,Cmin=cost(Xnear)+dist(Xnear,Xnew)，Xnewparent←Xcloset

18. χnewgrids←Find_Grids_Near(Xnear,Wg)

19. χnear←Find_nodes_Near(χnewgrids,Xnew,r)

20. For Xnear∈χneardo

21. Cnew=cost(Xnear)+dist(Xnear,Xnew)

22. If Cnew

23. Cmin=Cnew,Xmin=Xnear,Xnewparent←Xnear

24. For Xnear∈χneardo

25. Cold=cost(Xnew),Cnew=cost(Xnear)+dist(Xnear,Xnew)

26. If Cnew

27. Cmin=Cnew,Xmin=Xnew,Xnewparent←Xnew

28. If dist(Xnew,Xgoal)≤r and line(Xgoal,Xnew)∈Xfree

29. path←Getpath()

30. Break

31.Path←GreedyOptimize(path)

SAGB_RRT*算法進(jìn)行路徑規(guī)劃的主要步驟為：第1步結(jié)合工作空間劃分柵格空間，并利用柵格存儲(chǔ)和映射關(guān)系快速查找相鄰節(jié)點(diǎn)；第2步初始化時(shí)，定義6關(guān)節(jié)移動(dòng)機(jī)械臂的起始關(guān)節(jié)數(shù)組Xstart、目標(biāo)關(guān)節(jié)數(shù)組Xgoal和關(guān)節(jié)搜索步長(zhǎng)λ；第3步～第9步通過(guò)反饋open表中的可擴(kuò)展節(jié)點(diǎn)數(shù)來(lái)更新目標(biāo)偏向閾值P，比較隨機(jī)概率Prand和P的大小確定隨機(jī)節(jié)點(diǎn)的Xrand，通過(guò)Xrand控制隨機(jī)樹(shù)的生長(zhǎng)；第10步～第12步為了快速擴(kuò)展節(jié)點(diǎn)Xnew，利用Xrand所在柵格的索引值在柵格空間快速查找離Xrand所在柵格最近且含有樹(shù)節(jié)點(diǎn)的柵格群，在這些柵格群中迅速找到離Xrand最近的節(jié)點(diǎn)Xcloset，利用Xcloset生長(zhǎng)一個(gè)步長(zhǎng)得到Xnew；第13步～第16步對(duì)Xcloset～Xnew的運(yùn)動(dòng)進(jìn)行碰撞檢測(cè)，在碰撞檢測(cè)合格的前提下按照Xnew生長(zhǎng)結(jié)果更新open表，從而避免隨機(jī)樹(shù)重復(fù)偏向生長(zhǎng)；第17步～第23步通過(guò)Xnew重選父節(jié)點(diǎn)策略來(lái)優(yōu)化路徑，其關(guān)鍵為在節(jié)點(diǎn)Xnew通過(guò)其他節(jié)點(diǎn)獲得更低權(quán)值時(shí)，更新父節(jié)點(diǎn)并更新權(quán)值，主要?jiǎng)?chuàng)新為結(jié)合柵格群χnewgrids快速找到Xnew相鄰節(jié)點(diǎn)群χnear；第24步～第27步通過(guò)重布線優(yōu)化路徑，然后結(jié)合相鄰節(jié)點(diǎn)群χnear找到Xnew相鄰半徑r內(nèi)通過(guò)Xnew獲得更低權(quán)值的節(jié)點(diǎn)，更新父節(jié)點(diǎn)和權(quán)值；第28步～第30步為算法循環(huán)停止的條件，當(dāng)Xnew距離Xgoal小于r且與Xgoal連線和障礙物不碰撞時(shí)得到避障路徑，循環(huán)停止；第31步用間隔m貪心算法快速優(yōu)化SAGB_RRT*已規(guī)劃出的路徑，使所規(guī)劃的路徑代價(jià)更加接近最優(yōu)解。

4 仿真與實(shí)驗(yàn)

為驗(yàn)證算法性能，以日本三菱公司RV-2F為研究對(duì)象，采用改進(jìn)D-H(Denavit-Hartenberg)法建立機(jī)械臂運(yùn)動(dòng)學(xué)模型，通過(guò)逆運(yùn)動(dòng)學(xué)求解確立機(jī)械臂在關(guān)節(jié)空間的起始位置和目標(biāo)位置后，在關(guān)節(jié)空間對(duì)機(jī)械臂路徑進(jìn)行采樣，并在笛卡爾空間對(duì)采樣結(jié)果進(jìn)行碰撞檢測(cè)。

如圖10所示為RV-2F的三維模型，其有6個(gè)自由轉(zhuǎn)動(dòng)關(guān)節(jié)，屬于一種常見(jiàn)的連桿串聯(lián)機(jī)器人結(jié)構(gòu)。RV-2F關(guān)節(jié)1的軸線垂直基座朝上；關(guān)節(jié)2和關(guān)節(jié)3的軸線水平且平行，距離為a2；關(guān)節(jié)3和關(guān)節(jié)4的軸線相互垂直，距離為a3。圖11所示為采用改進(jìn)D-H法建立的RV-2F運(yùn)動(dòng)學(xué)坐標(biāo)系。

表1所示為采用改進(jìn)D-H對(duì)RV-2F運(yùn)動(dòng)學(xué)建模后的尺寸參數(shù)，通過(guò)測(cè)量機(jī)械臂可知d1=295 mm，d4=270 mm，d6=70 mm，a2=230 mm，a3=50 mm。

表1 RV-2F機(jī)械臂的D-H參數(shù)

表2所示為仿真實(shí)驗(yàn)時(shí)的機(jī)械臂運(yùn)動(dòng)控制參數(shù)，其中起點(diǎn)位置和終點(diǎn)位置是通過(guò)對(duì)機(jī)器臂進(jìn)行逆運(yùn)動(dòng)學(xué)求解得到的關(guān)節(jié)空間內(nèi)的坐標(biāo)加權(quán)，系數(shù)是多次試驗(yàn)得到的較優(yōu)值。

表2 機(jī)械臂運(yùn)動(dòng)控制參數(shù)表

本次仿真實(shí)驗(yàn)采用的計(jì)算機(jī)處理器為Inter(R) Core(TM) i5-8500 CPU @ 3.00 GHz 3.00 GHz，內(nèi)存為8 G，實(shí)驗(yàn)平臺(tái)為MATLAB R2016b。

4.1 仿真分析

4.1.1 SAGB_RRT*算法適應(yīng)性

為驗(yàn)證SAGB_RRT*算法在不同環(huán)境下的適應(yīng)性，選取5種典型環(huán)境進(jìn)行多次仿真實(shí)驗(yàn)，如圖12所示。環(huán)境1是有一個(gè)半徑為70 mm的障礙物，圓心在空間的坐標(biāo)為(75,54,395)；環(huán)境2有2個(gè)半徑為45 mm的障礙物，圓心在空間的坐標(biāo)為(75,14,395)，(75,104,395)；環(huán)境3有3個(gè)半徑為45 mm的障礙物，圓心在空間的坐標(biāo)為(75,14,395)，(75,104,335)，(75,84,475)；環(huán)境4有4個(gè)半徑為45 mm的障礙物，圓心在空間的坐標(biāo)為(55,14,395)，(75,104,335)，(75,74,475)，(235,134,365)；環(huán)境5有5個(gè)半徑為45 mm的障礙物，圓心在空間的坐標(biāo)為(5,14,395)，(75,104,335)，(75,74,475)，(235,134,365)，(0,74,550)。采用SAGB_RRT*算法在這5種復(fù)雜環(huán)境下為機(jī)械臂進(jìn)行路徑規(guī)劃?？梢?jiàn)，不同復(fù)雜度環(huán)境下，SAGB_RRT*均能快速規(guī)劃出符合機(jī)械臂運(yùn)動(dòng)限制條件的避障路徑。

4.1.2 SAGB_RRT*算法的有效性

為進(jìn)一步驗(yàn)證SAGB_RRT*算法的有效性，在4.1.1節(jié)環(huán)境下，分別采用SAGB_RRT*算法和不同閾值的GB_RRT*算法進(jìn)行15次仿真規(guī)劃，結(jié)果取平均值。如圖13a和圖13b所示，在不同環(huán)境下，SAGB_RRT*算法比不同固定閾值P的GB_RRT*算法規(guī)劃得到的路徑均更優(yōu)，路徑長(zhǎng)度降低約30%，而且在多次仿真實(shí)驗(yàn)規(guī)劃出的路徑中，SAGB_RRT*算法比GB_RRT*算法的穩(wěn)定性更好。如圖14c所示，在5種不同的環(huán)境中，由于引入柵格法存儲(chǔ)、open表存儲(chǔ)和自適應(yīng)調(diào)整目標(biāo)閾值，SAGB_RRT*算法比GB_RRT*算法規(guī)劃所需的搜索時(shí)間更具優(yōu)勢(shì)，降低約50%。如圖14d所示，在不同環(huán)境下的15次仿真實(shí)驗(yàn)中，SAGB_RRT*比GB_RRT*所需的搜索時(shí)間更穩(wěn)定。綜上所述，相比固定閾值P的GB_RRT*算法，SAGB_RRT*算法對(duì)環(huán)境具有較強(qiáng)的感知能力，其能根據(jù)環(huán)境調(diào)整隨機(jī)樹(shù)擴(kuò)展的偏向性，保證機(jī)械臂在各種復(fù)雜的工程環(huán)境下以較短的時(shí)間規(guī)劃出最優(yōu)且最穩(wěn)定的避障路徑。

4.1.3 不同算法對(duì)比

在4.1.2節(jié)的5種障礙物環(huán)境中，分別比較SAGB_RRT*算法與蟻群優(yōu)化(Ant Colony Optimization, ACO)算法、PRM、APF+RRT混合算法、GB_RRT*算法的規(guī)劃能力。圖14所示為將5種復(fù)雜環(huán)境中采用不同算法仿真分析15次后得到的結(jié)果取平均值進(jìn)行比較，數(shù)據(jù)顯示SAGB_RRT*算法規(guī)劃路徑所需的平均搜索時(shí)間和平均代價(jià)均優(yōu)于其他算法。

如圖14a所示，SAGB_RRT*算法在5種環(huán)境下均能穩(wěn)定地用較少的搜索時(shí)間規(guī)劃出路徑，其優(yōu)勢(shì)相比ACO和PRM尤為明顯，主要原因?yàn)锳CO和PRM在保證采樣次數(shù)的基礎(chǔ)上才能搜索出較優(yōu)路徑，導(dǎo)致其在有限搜索時(shí)間內(nèi)的算法性能不佳。如圖14b所示，SAGB_RRT*算法在5種環(huán)境下均能穩(wěn)定地規(guī)劃出較優(yōu)的路徑,其優(yōu)勢(shì)相比APF+RRT尤為明顯，主要原因?yàn)锳PF在高維環(huán)境下規(guī)劃時(shí)極易陷入局部極值點(diǎn)，RRT將其從局部極值點(diǎn)引出時(shí)會(huì)“繞路”，導(dǎo)致APF+RRT規(guī)劃的路徑較長(zhǎng)。SAGB_RRT*算法也明顯優(yōu)于GB_RRT*，主要原因在于GB_RRT*采用固定的目標(biāo)偏向閾值，當(dāng)初始化的目標(biāo)偏向閾值較高時(shí)，雖然可以保證其規(guī)劃的路徑較短，但是卻極易因陷入局部搜索而增大搜索時(shí)間；當(dāng)初始化的目標(biāo)偏向閾值較低時(shí)，會(huì)因搜索漫無(wú)目的而增大路徑代價(jià)和搜索時(shí)間。

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

為進(jìn)一步驗(yàn)證SAGB_RRT*路徑規(guī)劃方法的有效性和可行性，通過(guò)實(shí)驗(yàn)對(duì)仿真結(jié)果進(jìn)行驗(yàn)證，如圖16所示。圖中采用SAGB_RRT*算法為機(jī)械臂規(guī)劃起始位姿到目標(biāo)位姿的避障路徑，將規(guī)劃所得的機(jī)械臂運(yùn)動(dòng)路徑導(dǎo)入機(jī)器人示教器，并設(shè)置機(jī)械臂的速度比率為2。重復(fù)上述操作并進(jìn)行20次實(shí)驗(yàn)，機(jī)械臂都能避開(kāi)障礙物且平均運(yùn)行用時(shí)25.8 s。再采用ACO，PRM，APF+RRT，GB_RRT*進(jìn)行實(shí)驗(yàn)，平均運(yùn)行時(shí)間分別為28.5 s，26.6 s，28.7 s，27.3 s，可見(jiàn)SAGB_RRT*算法所規(guī)劃路徑的平均運(yùn)行時(shí)間小于其他算法。通過(guò)實(shí)驗(yàn)可知，采用SAGB_RRT*算法對(duì)機(jī)械臂進(jìn)行路徑規(guī)劃不但有效，而且所規(guī)劃出的路徑比其他算法更優(yōu)更短。

5 結(jié)束語(yǔ)

本文通過(guò)在救災(zāi)、采礦、高空裝配、檢測(cè)焊接和行星探測(cè)等環(huán)境下分析移動(dòng)機(jī)械臂的作業(yè)需求，提出在關(guān)節(jié)空間內(nèi)采用SAGB_RRT*算法進(jìn)行路徑避障規(guī)劃，該算法結(jié)合RRT*漸進(jìn)最優(yōu)思想對(duì)GB_RRT*算法進(jìn)行優(yōu)化，借助柵格儲(chǔ)存數(shù)據(jù)快速查找相鄰節(jié)點(diǎn)，并引入open表解決目標(biāo)偏向擴(kuò)展帶來(lái)的重復(fù)偏向問(wèn)題，提出通過(guò)open表節(jié)點(diǎn)數(shù)量變化自適應(yīng)控制偏向性生長(zhǎng)，并利用可變間隔的貪心算法對(duì)已規(guī)劃的路徑進(jìn)行優(yōu)化，從而在復(fù)雜障礙物環(huán)境下用較短的搜索時(shí)間快速規(guī)劃出較優(yōu)的避障路徑。仿真實(shí)驗(yàn)顯示，相比改進(jìn)前，SAGB_RRT*算法進(jìn)行規(guī)劃時(shí)所需的平均搜索時(shí)間降低了50%，規(guī)劃的平均路徑降低了30%，而且大幅提升了規(guī)劃的穩(wěn)定性；相比其他算法，SAGB_RRT*算法能夠很好地兼顧搜索時(shí)間和路徑代價(jià)，在進(jìn)行避障的同時(shí)保證規(guī)劃所需的平均搜索時(shí)間和所規(guī)劃出的平均路徑均為最優(yōu)，因此采用本文所提的SAGB_RRT*算法對(duì)移動(dòng)機(jī)械臂進(jìn)行路徑避障規(guī)劃是有效的?；赟AGB_RRT*算法的路徑避障規(guī)劃方法，能夠在較短的搜索時(shí)間內(nèi)規(guī)劃出較優(yōu)的避障路徑，為移動(dòng)機(jī)械臂在危險(xiǎn)環(huán)境下進(jìn)行快速路徑規(guī)劃提供技術(shù)支撐。