本發(fā)明屬于機械臂路徑規(guī)劃，具體涉及一種基于改進雙向rrt*算法的機械臂避障路徑規(guī)劃方法。

背景技術(shù)：

1、伴隨著工業(yè)機器人技術(shù)的迅速發(fā)展，其應(yīng)用場景也日益多樣化，如工件裝配、金屬焊接、物料搬運等。路徑規(guī)劃作為工業(yè)機器人系統(tǒng)化智能化的重要技術(shù)手段，良好的路徑規(guī)劃算法能夠保障工業(yè)機器人在運行過程中的安全性和可靠性。目前，移動機器人的路徑規(guī)劃方法通常是基于圖搜索的dijkstra、a*算法和基于勢場的apf算法。雖然a*等算法能夠找到可行路徑，但在高維空間下，這些算法的復(fù)雜度與計算量會大幅增加；而apf算法則容易陷入局部最優(yōu)解，無法確保機器人達到目標(biāo)點。基于隨機采樣的運動規(guī)劃方法能夠較好的解決這些問題。

2、基于隨機采樣的快速搜索隨機樹算法rrt(rapidly?exploring?random?tree)是一種現(xiàn)有的機器人路徑規(guī)劃算法，由于其在高維空間探索的高效性、全局空間內(nèi)的強探索性，被廣泛用于機械臂避障路徑規(guī)劃。而rrt*算法是rrt算法的改進優(yōu)化版本，一種漸近最優(yōu)的路徑規(guī)劃算法，屬于本技術(shù)領(lǐng)域常用技術(shù)術(shù)語，為公知常識。rrt*算法具有漸近最優(yōu)性，隨著采樣點數(shù)的增多，算法找到的路徑會接近最優(yōu)路徑。但rrt*算法同樣存在一些難以克服的局限性，由于采樣點是通過隨機均勻采樣算法生成，沒有任何導(dǎo)向，多數(shù)采樣點可能背離目標(biāo)，導(dǎo)致算法收斂速度較慢；rrt*算法采用固定步長搜索，在較大空曠區(qū)域會導(dǎo)致擴展速度變慢，而在狹窄通道處，算法則易跳過可行路徑，出現(xiàn)鎖路現(xiàn)象；rrt*算法生成的初始路徑節(jié)點與彎折較多，需要進一步優(yōu)化減少冗余節(jié)點和彎折，使路徑更符合實際執(zhí)行的需求。

技術(shù)實現(xiàn)思路

1、為解決上述技術(shù)問題，本發(fā)明提供了一種基于改進雙向rrt*算法的機械臂避障路徑規(guī)劃方法，能夠在復(fù)雜環(huán)境下，較短時間內(nèi)規(guī)劃出一條避開障礙物的高質(zhì)量路徑。

2、本發(fā)明采用的技術(shù)方案為：一種基于改進雙向rrt*算法的機械臂避障路徑規(guī)劃方法，具體步驟如下：

3、s1、構(gòu)建并初始化起始樹和目標(biāo)樹，同時確定非障礙物空間為隨機采樣空間，再依據(jù)環(huán)境信息確定初始搜索步長；

4、首先初始化路徑規(guī)劃所需要的參數(shù)信息，機械臂根據(jù)規(guī)劃空間及障礙物信息構(gòu)建地圖模型，確定目標(biāo)偏置采樣的可行空間為非障礙物空間，以及機械臂運動的起始點xinit和目標(biāo)點xgoal，并以起始點和目標(biāo)點分別構(gòu)建起始樹ta和目標(biāo)樹tb，然后計算初始搜索步長step0與環(huán)境系數(shù)ε。

5、其中，所述參數(shù)信息包括：路徑規(guī)劃的空間范圍，障礙物信息，機械臂的起始點xinit和目標(biāo)點xgoal，地圖的空間大小mapl、maph，障礙物的個數(shù)nob，障礙物總面積sob，采樣目標(biāo)引導(dǎo)概率閾值p0，重選父節(jié)點范圍半徑r，角度旋轉(zhuǎn)步長stepangle，交點集合i，碰撞狀態(tài)集合c。

6、其中，環(huán)境系數(shù)ε與初始搜索步長step0計算表達式如下：

7、

8、step0＝(mapl·maph)1/2·ε

9、s2、基于步驟s1，起始樹和目標(biāo)樹初始化完成后，開始進行目標(biāo)偏置采樣，采樣區(qū)域為非障礙物空間，起始樹和目標(biāo)樹輪流采樣，生成隨機概率p，并判斷隨機概率是否小于目標(biāo)引導(dǎo)概率，若是則隨機生成采樣點，若否則以另一棵樹的起點為采樣點；

10、其中，隨機樹為xrand，起始樹和目標(biāo)樹的采樣點分別為xranda、xrandb。起始樹和目標(biāo)樹的采樣點xrand(xranda、xrandb)的生成方式具體如下：

11、

12、s3、基于步驟s2，起始樹和目標(biāo)樹在獲取各自的采樣點后，遍歷隨機樹，分別尋找隨機樹上距離采樣點xrand最近的節(jié)點xnearest，像素化xnearest附近空間并取矩形范圍，由xnearest周圍障礙物矩形范圍重合面積s與矩形范圍面積srange，確定節(jié)點環(huán)境系數(shù)η和動態(tài)擴展步長step；

13、其中，矩形范圍的長xrange、寬yrange計算表達式如下：

14、

15、其中，分別表示xnearest的橫縱坐標(biāo)。

16、則節(jié)點環(huán)境系數(shù)η，動態(tài)擴展步長step計算表達式如下：

17、

18、step＝floor(step0·η)

19、s4、基于步驟s3，確定擴展方向為xnearest向xrand，由xnearest向xrand按照動態(tài)擴展步長step生長得到新節(jié)點xnew；

20、s5、使用xnew進行碰撞檢測，若不發(fā)生碰撞，則將xnew添加到隨機樹中，進入步驟s6，若發(fā)生碰撞，返回步驟s2重新進行目標(biāo)偏置采樣；

21、s6、對新節(jié)點xnew進行重選父節(jié)點和重新排線；

22、針對以xnew為圓心，r為半徑的圓內(nèi)節(jié)點，比較從隨機樹的起點經(jīng)圓內(nèi)每一節(jié)點到xnew的路徑代價，選取路徑代價最小的圓內(nèi)節(jié)點作為xnew的新父節(jié)點xnewparent，并改變樹的結(jié)構(gòu)；針對改變后的樹結(jié)構(gòu)和在xnew為圓心，r為半徑的圓內(nèi)節(jié)點，若存在節(jié)點xnear，xnear以xnew為父節(jié)點能進一步減小路徑代價，則令xnear的父節(jié)點為xnew，對隨機樹進行重新排線。

23、其中，起始樹起點為xinit，目標(biāo)樹起點為xgoal。

24、s7、基于步驟s6，判斷起始樹與目標(biāo)樹是否能直接相連；

25、應(yīng)用雙向末端強化機制，若起始樹產(chǎn)生的新節(jié)點xnewa或目標(biāo)樹產(chǎn)生的新節(jié)點xnewb，能與另一棵樹上的某一節(jié)點直接相連，算法收斂，生成初始路徑，進入步驟s8，否則回到步驟s2。

26、s8、基于步驟s7中所獲得的最終路徑，進行路徑優(yōu)化，移除不必要的中間節(jié)點，簡化路徑并減少冗余，在路徑轉(zhuǎn)折處應(yīng)用基于旋轉(zhuǎn)向量引導(dǎo)的路徑優(yōu)化方法生成新節(jié)點，進一步優(yōu)化轉(zhuǎn)折處以減少路徑代價，輸出最終優(yōu)化路徑，即適合機械臂運動的實際路徑。

27、進一步地，所述步驟s8具體如下：

28、s81、輸入待優(yōu)化路徑，進行路徑剪枝，從路徑起點開始依次與后續(xù)節(jié)點連接，若不發(fā)生碰撞則去除中間節(jié)點，獲得剪枝后路徑，然后判斷是否遍歷路徑所有節(jié)點，若否則進入步驟s82，若是則路徑已完成初次優(yōu)化，進入步驟s85；

29、s82、基于步驟s81獲得的剪枝后路徑，從起點qj開始，取節(jié)點qj、qj+1和qj+2，由qj、qj+1的向量方向生成一條足夠長的射線，逆時針旋轉(zhuǎn)射線，每次旋轉(zhuǎn)stepangle，并判斷是否已經(jīng)旋轉(zhuǎn)360°，若否則存儲每一次旋轉(zhuǎn)時射線和qj+1與qj+2連線的所有交點，把交點位置儲存在交點集合i中，檢測并儲存qj與交點集合i中每一點進行碰撞檢測的結(jié)果，儲存在碰撞狀態(tài)集合c中，直至射線旋轉(zhuǎn)至360°進入步驟s83；

30、s83、基于步驟s82得到的碰撞狀態(tài)集合c中的布爾值，檢測碰撞狀態(tài)集合的突變，當(dāng)碰撞狀態(tài)發(fā)生突變時，儲存突變時不碰撞時的交點坐標(biāo)qnc；

31、其中，所述碰撞狀態(tài)發(fā)生突變即由碰撞變?yōu)椴慌鲎不蛴刹慌鲎沧優(yōu)榕鲎病?/p>

32、s84、去除節(jié)點qj+1并創(chuàng)建新節(jié)點qnc添加到路徑中，以qnc為起點，令qj＝qnc，重復(fù)進行基于旋轉(zhuǎn)向量引導(dǎo)的路徑優(yōu)化生成新節(jié)點，即重復(fù)步驟s82-s83，直至遍歷步驟s81剪枝后路徑中所有節(jié)點，生成初次優(yōu)化后路徑，進入步驟s85；

33、s85、對步驟s84得到的初次優(yōu)化后路徑進行剪枝，隨后對剪枝后路徑進行反轉(zhuǎn)并再次進行基于旋轉(zhuǎn)向量引導(dǎo)的路徑優(yōu)化，即重復(fù)步驟s82-s84，得到二次優(yōu)化路徑；

34、s86、基于步驟s85得到的二次優(yōu)化路徑，繼續(xù)進行剪枝，隨后對路徑進行反轉(zhuǎn)，輸出最終優(yōu)化路徑。

35、本發(fā)明的有益效果：本發(fā)明的方法首先獲取路徑規(guī)劃所需要參數(shù)信息，構(gòu)建起始樹和目標(biāo)樹，依據(jù)環(huán)境信息確定初始搜索步長，然后進行目標(biāo)偏置采樣，獲得隨機樹采樣點，找到隨機樹上距離采樣點最近節(jié)點并像素化其周圍空間，計算動態(tài)擴展步長，由最近節(jié)點向隨機樹采樣點按照動態(tài)擴展步長生長得到新節(jié)點，并進行碰撞檢測，若碰撞則重新采樣，若不碰撞則新節(jié)點添加到隨機樹并，重選父節(jié)點和重新排線，應(yīng)用雙向末端強化機制得到初始路徑，最后基于旋轉(zhuǎn)向量引導(dǎo)的路徑優(yōu)化方法優(yōu)化路徑轉(zhuǎn)折處，輸出最終優(yōu)化路徑。本發(fā)明的方法所提出的目標(biāo)偏置采樣策略在保持采樣點隨機性的同時，增加了采樣點向目標(biāo)偏置的概率，能夠加速算法收斂，減少迂回和不必要路徑，提高路徑規(guī)劃質(zhì)量；所提出的動態(tài)步長調(diào)整策略能根據(jù)節(jié)點周圍障礙物信息動態(tài)調(diào)整步長長度，從而提高路徑規(guī)劃的靈活性，減少碰撞次數(shù)，節(jié)省碰撞檢測時間；所提出的雙向末端強化機制能夠顯著加快算法的路徑的生成速度，降低算法陷入局部最優(yōu)的可能性，增強算法對復(fù)雜環(huán)境，尤其是具有狹窄通道環(huán)境的適應(yīng)性；基于旋轉(zhuǎn)向量引導(dǎo)的路徑優(yōu)化方法能夠顯著縮短路徑長度和提高路徑質(zhì)量。此外，本發(fā)明的方法簡單易行，適用于復(fù)雜環(huán)境下的機械臂路徑規(guī)劃。