[發(fā)明專利]基于多自由度機(jī)械臂的改進(jìn)式RRT*的避障運(yùn)動(dòng)規(guī)劃方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710163211.0
• 申請(qǐng)日: 2017-03-19
• 公開（公告）號(hào): CN106695802A
• 公開（公告）日: 2017-05-24
• 發(fā)明（設(shè)計(jì)）人: 龔道雄;李赟程;邵杰
• 申請(qǐng)（專利權(quán)）人: 北京工業(yè)大學(xué)
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 北京思海天達(dá)知識(shí)產(chǎn)權(quán)代理有限公司11203
• 代理人: 沈波

1.基于多自由度機(jī)械臂的改進(jìn)式RRT*的避障運(yùn)動(dòng)規(guī)劃方法，其特征在于：該方法的實(shí)現(xiàn)過(guò)程如下：

步驟一：搭建七連桿和六旋轉(zhuǎn)關(guān)節(jié)的六自由度機(jī)械臂模型，設(shè)定目標(biāo)到達(dá)點(diǎn)位姿值，逆解求其關(guān)節(jié)角度，確定逆解是否有解，如果有解，設(shè)定為目標(biāo)狀態(tài)x_goal，目標(biāo)狀態(tài)x_goal包含位置和姿態(tài)信息；如果逆解無(wú)解，重新采樣空間區(qū)域內(nèi)目標(biāo)位姿點(diǎn)，直到選中可達(dá)的目標(biāo)狀態(tài)為止；

步驟二：確定待搜索的空間內(nèi)參數(shù)初始化，初始化參數(shù)為：起始位型x_init，目標(biāo)位型x_goal，擴(kuò)展步長(zhǎng)λ，當(dāng)前的迭代次數(shù)n；

步驟三：在位型空間內(nèi)隨機(jī)采樣選取x_rand,選取x_rand附近臨近節(jié)點(diǎn)區(qū)域，根據(jù)節(jié)點(diǎn)區(qū)域公式得到x_near的集合X_near，其中d是空間維度，γ是選擇的常數(shù)，V是已構(gòu)成搜索樹節(jié)點(diǎn)的集合，x′是搜索域中某一節(jié)點(diǎn)；

步驟四：為了選定x_min,σ_min作為暫定的父節(jié)點(diǎn)和父路徑，遍歷集合X_near，定義最優(yōu)路徑minCost，將x_min,σ_min賦Null值，從隨機(jī)點(diǎn)x_rand向集合內(nèi)子節(jié)點(diǎn)x_near生成各自路徑σ；

步驟五：判斷Cost(x_near)+Cost(σ)<minCost是否成立，如果小于最小花費(fèi)路徑距離，則將該集合內(nèi)的附近節(jié)點(diǎn)到初始點(diǎn)和該節(jié)點(diǎn)到隨機(jī)點(diǎn)的距離暫定為最小路徑，相應(yīng)地，x_near和σ就暫定為各自的x_min,σ_min，如果大于最小花費(fèi)路徑，則遍歷其他附近節(jié)點(diǎn)，直到找到最小路徑為止；

步驟六：對(duì)新生成的σ做碰撞檢測(cè)，如果新生成的路徑?jīng)]有碰撞到障礙物區(qū)間，則將節(jié)點(diǎn)x_rand和路徑σ添加到樹中；

步驟七：將現(xiàn)有的樹即節(jié)點(diǎn)和邊和X_near集合剩余的x_near，x_rand添加到Rewire函數(shù)進(jìn)行重新焊接路徑，將冗余路徑剔除；

步驟八：遍歷剩余的x_near，從新生成的節(jié)點(diǎn)x_rand調(diào)用Steer(x_rand,x_near)步長(zhǎng)函數(shù)生成路徑σ；

步驟九：判斷Cost(x_rand)+Cost(σ)<Cost(x_near)是否成立，如果初始點(diǎn)到隨機(jī)點(diǎn)路徑加上新生成路徑的距離小于原始的初始點(diǎn)到相應(yīng)附近節(jié)點(diǎn)路徑，那么對(duì)其σ進(jìn)行碰撞檢測(cè)，如果沒有碰撞到障礙物，則定義該x_near作為父節(jié)點(diǎn)，去除所有的父節(jié)點(diǎn)和其余x_near的邊E←E/{x_parent,x_near}，E代表所構(gòu)成的邊的集合，并將隨機(jī)點(diǎn)和父節(jié)點(diǎn)的邊重新添加到樹中E←E∪{x_rand,x_near}；

步驟十：重新采樣下一個(gè)隨機(jī)點(diǎn)，根據(jù)正態(tài)分布的規(guī)律改變隨機(jī)采樣的特性μ代表平均值，σ是標(biāo)準(zhǔn)差；將均值設(shè)置為x_go_al，通過(guò)改變標(biāo)準(zhǔn)差的值來(lái)改變搜索的隨機(jī)特性，設(shè)置標(biāo)準(zhǔn)差σ的參數(shù)值；

步驟十一：判斷搜索空間的大小以及對(duì)稱程度，將正態(tài)分布的采樣區(qū)域重新更改規(guī)劃，采用-φ(x_lower)是采樣可達(dá)最大區(qū)域，將概率密度函數(shù)更改以適應(yīng)不同的實(shí)際環(huán)境，f′(x|μ,σ)是修正后的采樣區(qū)域概率密度；

步驟十二：重復(fù)步驟三到步驟十一，一直到最優(yōu)路徑搜索到。

2.根據(jù)權(quán)利要求1所述的基于多自由度機(jī)械臂的改進(jìn)式RRT*的避障運(yùn)動(dòng)規(guī)劃方法，其特征在于：

首先建立七連桿六旋轉(zhuǎn)關(guān)節(jié)的機(jī)械臂，選擇二指夾持器作為末端執(zhí)行器，(x0，y0，z0)作為基坐標(biāo)系，其余關(guān)節(jié)圍繞旋轉(zhuǎn)的軸作為z軸，根據(jù)DH參數(shù)值計(jì)算出末端的位姿矩陣其中A1到A6為各關(guān)節(jié)旋轉(zhuǎn)矩陣，通過(guò)逆運(yùn)動(dòng)學(xué)分析求解目標(biāo)狀態(tài)下的各個(gè)關(guān)節(jié)角度；完成算法路徑的搜索，具體為：在100*100*100的三維立體空間內(nèi)，起始位型x_init設(shè)定為(5,5,5),在不考慮機(jī)械臂姿態(tài)，只考慮空間位置的情況下，目標(biāo)位型x_goal(95,95,95),擴(kuò)展步長(zhǎng)λ設(shè)為1.5,當(dāng)前迭代次數(shù)為0次，在搜索空間內(nèi)隨機(jī)采樣選取x_rand,根據(jù)公式得到x_near的集合X_near,其中d設(shè)定為3，_γ選擇0.618，在該集合里查看是否有搜索樹上的節(jié)點(diǎn)，如果沒有進(jìn)一步擴(kuò)展迭代次數(shù)增加搜索域直到集合X_near包含附近節(jié)點(diǎn)；選定x_min,σ_min作為暫定的父節(jié)點(diǎn)和父路徑，遍歷集合X_near，定義最優(yōu)路徑minCost，將x_min,σ_min賦Null值，從隨機(jī)點(diǎn)x_rand向集合內(nèi)子節(jié)點(diǎn)x_near生成各自路徑σ；

判斷Cost(x_near)+Cost(σ)<minCost是否成立，如果小于最小花費(fèi)路徑距離，則將該集合內(nèi)的附近節(jié)點(diǎn)到初始點(diǎn)和該節(jié)點(diǎn)到隨機(jī)點(diǎn)的距離暫定為最小路徑，相應(yīng)地，x_near和σ就暫定為各自的x_min,σ_min，如果大于最小花費(fèi)路徑，則遍歷其他附近節(jié)點(diǎn)，直到找到最小路徑為止；對(duì)新生成的σ做碰撞檢測(cè)，如果新生成的路徑?jīng)]有碰撞到障礙物區(qū)間，則將節(jié)點(diǎn)x_rand和路徑σ添加到樹中；將現(xiàn)有的樹(節(jié)點(diǎn)和邊)和X_near集合剩余的x_near，x_rand添加到Rewire函數(shù)進(jìn)行重新焊接路徑，將冗余路徑剔除；遍歷剩余的x_near，從新生成的節(jié)點(diǎn)x_rand調(diào)用Steer(x_rand,x_near)步長(zhǎng)函數(shù)生成路徑σ，判斷Cost(x_rand)+Cost(σ)<Cost(x_near)是否成立，如果初始點(diǎn)到隨機(jī)點(diǎn)路徑加上新生成路徑的距離小于原始的初始點(diǎn)到相應(yīng)附近節(jié)點(diǎn)路徑，那么對(duì)其σ進(jìn)行碰撞檢測(cè)，如果沒有碰撞到障礙物，則定義該x_near作為父節(jié)點(diǎn)，祛除所有的父節(jié)點(diǎn)和其余x_near的邊E←E/{x_parent,x_near}，并將隨機(jī)點(diǎn)和父節(jié)點(diǎn)的邊重新添加到樹中E←E∪{x_rand,x_near}，拓展節(jié)點(diǎn)的方式和重新焊接已有節(jié)點(diǎn)和邊是RRT*不同于原始RRT的一種搜索方法，重新采樣下一個(gè)隨機(jī)點(diǎn)，根據(jù)正態(tài)分布的規(guī)律改變隨機(jī)采樣的特性將均值設(shè)置為x_go_al(95,95,95)，通過(guò)改變標(biāo)準(zhǔn)差的值來(lái)改變搜索的隨機(jī)特性，設(shè)置標(biāo)準(zhǔn)差σ＝1；判斷搜索空間的大小以及對(duì)稱程度，將正態(tài)分布的采樣區(qū)域重新更改規(guī)劃，采用將概率密度函數(shù)更改以適應(yīng)不同的實(shí)際環(huán)境，重復(fù)如說(shuō)明圖的步驟，直到找到一條合適的搜索路徑為止。