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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

7、

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

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

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

11、

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

13、其中，矩形范圍的長xrange、寬yrange計(jì)算表達(dá)式如下：

14、

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

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

17、

18、step＝floor(step0·η)

19、s4、基于步驟s3，確定擴(kuò)展方向?yàn)閤nearest向xrand，由xnearest向xrand按照動(dòng)態(tài)擴(kuò)展步長step生長得到新節(jié)點(diǎn)xnew；

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

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

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

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

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

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

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

27、進(jìn)一步地，所述步驟s8具體如下：

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

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

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

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

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

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

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

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