本發(fā)明公開一種基于改進(jìn)rrt算法的機(jī)械臂避障路徑規(guī)劃方法

背景技術(shù)：

1、機(jī)械臂作為機(jī)器人技術(shù)研究中不可或缺的一部分，其具有工作空間大、操作簡(jiǎn)便、靈活性和自由度高等優(yōu)點(diǎn)。當(dāng)機(jī)械臂執(zhí)行任務(wù)時(shí)，運(yùn)動(dòng)是否連續(xù)、運(yùn)行是否高效平穩(wěn)、規(guī)劃出的路徑是否會(huì)與障礙物發(fā)生碰撞等直接影響了作業(yè)質(zhì)量與效率，因此對(duì)機(jī)械臂的運(yùn)動(dòng)規(guī)劃相關(guān)算法展開研究極具意義。

2、路徑規(guī)劃顧名思義，即規(guī)劃出機(jī)器人運(yùn)動(dòng)的路徑，它可以幫助機(jī)器人在復(fù)雜環(huán)境下自主移動(dòng)，提高其自動(dòng)化水平。機(jī)械臂的路徑規(guī)劃和移動(dòng)機(jī)器人底盤類似，均是以機(jī)器人當(dāng)前位置為起點(diǎn)，規(guī)劃出一條運(yùn)動(dòng)到終點(diǎn)且無碰撞的路徑。但是機(jī)械臂的路徑規(guī)劃主要應(yīng)用于多維空間，對(duì)環(huán)境中障礙物的精確描述與建模相較移動(dòng)底盤更為困難；并且機(jī)械臂本身擁有多自由度，在機(jī)械臂的關(guān)節(jié)空間中進(jìn)行路徑規(guī)劃的難度更大。因此在過去的幾十年中，國(guó)內(nèi)外學(xué)者嘗試過諸多方法來解決機(jī)械臂路徑規(guī)劃領(lǐng)域的問題，主要可以分為四類：基于圖搜索算法的路徑規(guī)劃方法、基于勢(shì)場(chǎng)的路徑規(guī)劃方法、基于智能算法的路徑規(guī)劃方法和基于隨機(jī)采樣的路徑規(guī)劃方法。

3、以快速擴(kuò)展隨機(jī)樹(rapidly-exploring?random?tree,rrt)算法為代表的基于隨機(jī)采樣的路徑規(guī)劃算法無需預(yù)處理，也無需提前建圖，適用于高維空間中的路徑規(guī)劃，在機(jī)械臂避障路徑規(guī)劃中得到廣泛的應(yīng)用。但是rrt算法依然存在算法收斂速度較慢、規(guī)劃出的路徑代價(jià)較大、復(fù)雜環(huán)境難以規(guī)劃出路徑等不足，并且該算法本身在搜索路徑的過程中具有隨機(jī)性，多數(shù)情況規(guī)劃出的只是可行路徑并不是最優(yōu)路徑。雖然各種研究學(xué)者基于rrt算法提出了諸多改進(jìn)算法，但是均不具備絕對(duì)的普適性，由于機(jī)械臂在三維空間下規(guī)劃路徑時(shí)通常是不穩(wěn)定的，需在不同層面上做出改進(jìn)。

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

1、為克服上述問題，本發(fā)明的目的在于提供了一種基于改進(jìn)rrt算法的機(jī)械臂避障路徑規(guī)劃方法。該方法應(yīng)用目標(biāo)偏置加強(qiáng)算法向目標(biāo)點(diǎn)搜索的趨向性、引入貪婪思想提高算法效率、結(jié)合三次b樣條曲線對(duì)規(guī)劃出的路徑平滑優(yōu)化，此外，在搜索路徑過程通過保持碰撞檢測(cè)以達(dá)到避障效果，并且由于該方法經(jīng)過不斷地重選父節(jié)點(diǎn)和重新布線，最終生成的路徑趨近最優(yōu)路徑，使得該方法兼具搜索效率和最優(yōu)性。

2、為達(dá)到上述目的，本發(fā)明的基于改進(jìn)rrt算法的機(jī)械臂避障路徑規(guī)劃方法，包括以下步驟：

3、步驟1：初始化地圖信息，設(shè)置算法參數(shù)；

4、步驟2：分別以起點(diǎn)和終點(diǎn)作為根節(jié)點(diǎn)向空間中采樣生成擴(kuò)展樹t1和t2；

5、步驟3：采用目標(biāo)偏置策略加強(qiáng)向目標(biāo)點(diǎn)搜索的趨向性，以減少無用搜索，即人為引導(dǎo)采樣過程中隨機(jī)點(diǎn)的生成，在生成一個(gè)隨機(jī)節(jié)點(diǎn)時(shí)，以概率p讓該節(jié)點(diǎn)生成朝向目標(biāo)點(diǎn)的鄰近節(jié)點(diǎn)，即擴(kuò)展樹t1向目標(biāo)點(diǎn)f擴(kuò)展生長(zhǎng)；

6、步驟4：在擴(kuò)展樹t1和/或t2的搜索路徑過程中不斷重選父節(jié)點(diǎn)并重新布線；

7、步驟5：路徑規(guī)劃過程應(yīng)用貪婪思想保持判斷，直到擴(kuò)展樹t1和t2中存在兩個(gè)節(jié)點(diǎn)間不存在障礙物且滿足設(shè)定的閾值時(shí)，將這兩個(gè)節(jié)點(diǎn)直接相連并生成從起點(diǎn)通往終點(diǎn)的路徑。

8、進(jìn)一步地，所述步驟1需設(shè)置的參數(shù)包括起點(diǎn)s、終點(diǎn)f、目標(biāo)偏置概率p、重連半徑r、步長(zhǎng)es、節(jié)點(diǎn)連接閾值α以及起點(diǎn)與終點(diǎn)間的障礙物信息。

9、進(jìn)一步地，所述步驟3的具體內(nèi)容為：生成一個(gè)范圍在(0,1)的隨機(jī)數(shù)xrand，擴(kuò)展樹t1生成任一子節(jié)點(diǎn)tn1時(shí)都按設(shè)定的目標(biāo)偏置概率p進(jìn)行概率判斷，并且為保持搜索樹對(duì)于未知空間的搜索能力，概率p的值一般設(shè)定在0.05～0.10之間；若0<xrand<p，那么tn1生成朝向f的鄰近節(jié)點(diǎn)trand，若p<xrand<1，隨機(jī)生成鄰近節(jié)點(diǎn)tn1+1并重復(fù)步驟3。

10、進(jìn)一步地，所述步驟3進(jìn)行的同時(shí)還包括：擴(kuò)展樹t2隨機(jī)生成候選節(jié)點(diǎn)tn2，其相鄰節(jié)點(diǎn)tn2+1的生成不再隨機(jī)，而是以trand作為目標(biāo)，不斷生成向目標(biāo)節(jié)點(diǎn)靠近的相鄰節(jié)點(diǎn)tn2+1。

11、進(jìn)一步地，每次生成鄰近節(jié)點(diǎn)時(shí)均引入碰撞檢測(cè)，若鄰近節(jié)點(diǎn)trand與tn1間不存在障礙物，則將trand加入t1，反之則放棄此次獲得的鄰近節(jié)點(diǎn)trand；若鄰近節(jié)點(diǎn)tn2+1與tn2間不存在障礙物，則將tn2+1加入t2，反之則放棄此次獲得的鄰近節(jié)點(diǎn)tn2+1。

12、進(jìn)一步地，所述步驟4的具體內(nèi)容包括：

13、步驟4-1：t1擴(kuò)展采樣生成節(jié)點(diǎn)tn1并以固定步長(zhǎng)es生成朝向f的鄰近節(jié)點(diǎn)trand，此時(shí)tn1即為trand的鄰近父節(jié)點(diǎn)；

14、步驟4-2：在trand附近在規(guī)定的半徑范圍r內(nèi)尋找鄰近節(jié)點(diǎn)，并將尋得的鄰近節(jié)點(diǎn)作為替換原父節(jié)點(diǎn)的備選；

15、步驟4-3：依次計(jì)算鄰近節(jié)點(diǎn)到起點(diǎn)s加上該鄰近節(jié)點(diǎn)到節(jié)點(diǎn)trand的路徑代價(jià)，選擇相對(duì)最小路徑代價(jià)的鄰近節(jié)點(diǎn)來替換tn1作為trand新的父節(jié)點(diǎn)；

16、步驟4-4：在更換trand父節(jié)點(diǎn)后重新布線，重新生成擴(kuò)展樹t1。

17、進(jìn)一步地，所述的碰撞檢測(cè)模型將機(jī)械臂簡(jiǎn)化成一個(gè)圓柱體，將空間障礙物簡(jiǎn)化成一個(gè)球體，并為了簡(jiǎn)化計(jì)算將連桿抽象成一條沒有寬度的直線；所述機(jī)械臂和障礙物在經(jīng)過模型簡(jiǎn)化后，通過判斷圓柱體和球體間的距離即可判斷是否發(fā)生碰撞，若距離小于0視為發(fā)生碰撞，相反則未發(fā)生碰撞。

18、進(jìn)一步地，所述步驟5的具體內(nèi)容包括：當(dāng)擴(kuò)展樹t1和t2中存在trand和tn2+1間的距離滿足設(shè)定的節(jié)點(diǎn)連接閾值α?xí)r，判斷這兩個(gè)節(jié)點(diǎn)直接相連的可行性，若兩點(diǎn)間存在障礙物，則兩點(diǎn)分別繼續(xù)采樣擴(kuò)展生成新的節(jié)點(diǎn)；若兩點(diǎn)間不存在障礙物，則停止采樣，且不受固定步長(zhǎng)es的限制，將兩個(gè)節(jié)點(diǎn)直接相連，并將兩點(diǎn)間的路徑放入最終規(guī)劃出的路徑中。

19、進(jìn)一步地，還包括步驟6：使用三次b樣條插值函數(shù)對(duì)路徑進(jìn)行平滑處理，生成最終路徑。

20、進(jìn)一步地，所述的步驟6為：在不改變整條路徑的情況下使用三次b樣條插值函數(shù)只對(duì)局部關(guān)鍵位置點(diǎn)進(jìn)行平滑優(yōu)化，進(jìn)而優(yōu)化整條路徑，使機(jī)械臂運(yùn)動(dòng)過程更為平穩(wěn)高效。

21、本發(fā)明具有以下優(yōu)點(diǎn)：

22、(1)rrt算法在隨機(jī)采樣的過程中，擴(kuò)展樹時(shí)常會(huì)擴(kuò)展到離目標(biāo)很遠(yuǎn)的地方，即“無用區(qū)域”，導(dǎo)致搜索路徑過程產(chǎn)生大量無用節(jié)點(diǎn)和路徑。因此本發(fā)明應(yīng)用目標(biāo)偏置的思想，人為引導(dǎo)采樣過程中隨機(jī)點(diǎn)的生成，在生成一個(gè)隨機(jī)節(jié)點(diǎn)時(shí)，以一定概率讓該節(jié)點(diǎn)生成朝向目標(biāo)點(diǎn)的鄰近節(jié)點(diǎn)，即使擴(kuò)展樹向目標(biāo)點(diǎn)擴(kuò)展生長(zhǎng)。此方法加強(qiáng)了向目標(biāo)點(diǎn)搜索的趨向性，減少了無用搜索，既保持了采樣生成子節(jié)點(diǎn)的隨機(jī)性，又在一定程度上降低了采樣過程的盲目性，提高了算法的收斂速度，使rrt算法在多維空間下的搜索能力得到最大化的利用。

23、(2)rrt算法在隨機(jī)采樣到目標(biāo)點(diǎn)附近區(qū)域時(shí)，受固定步長(zhǎng)的限制，即使采樣點(diǎn)與目標(biāo)點(diǎn)之間無障礙物，擴(kuò)展樹也無法直接到達(dá)目標(biāo)點(diǎn)，轉(zhuǎn)而會(huì)繼續(xù)進(jìn)行大量無效搜索，使路徑復(fù)雜、規(guī)劃時(shí)長(zhǎng)增加。針對(duì)上述問題，本發(fā)明引入貪婪思想，在搜索路徑過程中保持判斷，直到兩棵擴(kuò)展樹中存在兩個(gè)節(jié)點(diǎn)間不存在障礙物且滿足設(shè)定的閾值時(shí)，將這兩個(gè)節(jié)點(diǎn)直接相連并生成從起點(diǎn)通往終點(diǎn)的路徑，較大幅度地提高了算法搜索路徑的速度。

24、(3)rrt算法規(guī)劃出的路徑若不經(jīng)過平滑優(yōu)化，往往會(huì)彎折幅度較大的拐點(diǎn)，導(dǎo)致機(jī)械臂運(yùn)行過程中容易出現(xiàn)抖動(dòng)振蕩的情況，進(jìn)而影響機(jī)械臂的工作精度與效率。本發(fā)明使用三次b樣條插值函數(shù)對(duì)局部關(guān)鍵位置點(diǎn)進(jìn)行平滑優(yōu)化，這樣做的好處是在不改變整條路徑的情況下對(duì)路徑進(jìn)行優(yōu)化，使彎折幅度大的路徑拐點(diǎn)變得平緩，保證了機(jī)械臂運(yùn)動(dòng)過程的平穩(wěn)高效。