本發(fā)明涉及到機械臂避障環(huán)節(jié)中運動規(guī)劃方法，具體是一種改進式快速隨機搜索的方法應用到全局空間，屬于機械臂運動規(guī)劃領(lǐng)域

背景技術(shù)：

機器人的運動規(guī)劃問題早在上世紀六十年代提出，早期的運動規(guī)劃只是涉及到路徑規(guī)劃，機器人被看成一點，運動規(guī)劃就被看成在位姿空間中尋找一條從起始位姿點到目標位姿點的連續(xù)路徑，路徑規(guī)劃經(jīng)過一個搜索的過程，根據(jù)世界環(huán)境的建模和搜索方法的不同主要分成自由空間幾何構(gòu)造的規(guī)劃，智能法規(guī)劃和基于隨機采樣的規(guī)劃方法。然而早期的路徑規(guī)劃不能適應于當前機械臂的高自由度和障礙物空間復雜的環(huán)境，早期的規(guī)劃方法只能增加大量的計算，降低搜索的效率。

機械臂的避障規(guī)劃目標是規(guī)劃處一條滿足各類指標的最優(yōu)路徑，針對這一問題，已采用的C空間法、A*搜索法、人工試場法、遺傳算法都存在一定的局限性，C空間法的計算時間要比機械臂的響應時間長，限制其在避障中的應用，蟻群算法是本質(zhì)上并行的算法，它在問題空間多點同時開始進行多線獨立的解搜索，但改算法搜索時間較長，易陷入停滯問題。

考慮到現(xiàn)在搜索算法存在的諸多效率低下的問題，本發(fā)明旨在提出一種改進的RRT*(rapidly exploring random tree)算法的多自由度機械臂避障的運動規(guī)劃方法，對解決了搜索時花費時間長，迭代次數(shù)多，易于限于局部無窮小，收斂率低的問題，使機械臂能夠快速有效地避開障礙物。這種概率規(guī)劃尤其對于高維空間中的規(guī)劃問題更是有效。

技術(shù)實現(xiàn)要素：

為了實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為基于多自由度機械臂的改進式RRT*的避障運動規(guī)劃方法，該方法的實現(xiàn)過程如下：

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

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

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

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

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

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

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

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

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

步驟十：重新采樣下一個隨機點，根據(jù)正態(tài)分布的規(guī)律改變隨機采樣的特性μ代表平均值，σ是標準差。將均值設置為x_goal，通過改變標準差的值來改變搜索的隨機特性，設置標準差σ的參數(shù)值；

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

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

將生成的軌跡添加到軌跡規(guī)劃器中，本發(fā)明與現(xiàn)有技術(shù)相比具有以下優(yōu)點，通過添加正態(tài)分布的方式改變隨機搜索的特性，這種帶有啟發(fā)式的搜索可以提高算法的收斂率，RRT*算法具有漸進優(yōu)化的路徑，不用通過大量的計算；加入啟發(fā)點在目標點附近的高斯分布后，提高了收斂速率，節(jié)約了搜索時間。

附圖說明

圖1為搭建的六自由度機械臂

圖2為發(fā)明流程圖

圖3為RRT*搜索示意圖

圖4為RRT*在三維環(huán)境下的搜索路徑圖

圖5目標位姿點添加高斯分布密度函數(shù)圖

圖6為加入高斯分布后的改進式RRT*最優(yōu)搜索路徑圖

圖7為在ROS仿真環(huán)境下六自由度機械臂添加改進式RRT*運動規(guī)劃的仿真路徑圖

具體實施方式

下面結(jié)合說明附圖和具體實施方式對本發(fā)明進一步說明，本發(fā)明的實施例是按照本發(fā)明的技術(shù)方案前提下實施的。給出了詳細的實施方式和具體的操作流程，但本發(fā)明的保護范圍不限于以下實例范圍。

首先建立七連桿六旋轉(zhuǎn)關(guān)節(jié)的機械臂，選擇二指夾持器作為末端執(zhí)行器，如圖1一樣(x0，y0，z0)作為基坐標系，其余關(guān)節(jié)圍繞旋轉(zhuǎn)的軸作為z軸，根據(jù)DH參數(shù)值計算出末端的位姿矩陣其中A1到A6為各關(guān)節(jié)旋轉(zhuǎn)矩陣，通過逆運動學分析求解目標狀態(tài)下的各個關(guān)節(jié)角度

根據(jù)圖2的流程圖完成算法路徑的搜索，具體為

在100*100*100的三維立體空間內(nèi)，起始位型x_init設定為(5,5,5),在不考慮機械臂姿態(tài)，只考慮空間位置的情況下，目標位型x_goal(95,95,95),擴展步長λ設為1.5,當前迭代次數(shù)為0次，在搜索空間內(nèi)隨機采樣選取x_rand,根據(jù)公式得到x_near的集合X_near,其中_d設定為3，γ選擇0.618，在該集合里查看是否有搜索樹上的節(jié)點，如果沒有進一步擴展迭代次數(shù)增加搜索域直到集合X_near包含附近節(jié)點；選定x_min,σ_min作為暫定的父節(jié)點和父路徑，遍歷集合X_near，定義最優(yōu)路徑minCost，將x_min,σ_min賦Null值，從隨機點x_rand向集合內(nèi)子節(jié)點x_near生成各自路徑σ；

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

圖4是RRT*的搜索路徑圖，包含了8個球形的障礙物，起始點是(5,5,5)終點是(95,95,95)只考慮末端的位置，不考慮姿態(tài)的情況下按照如上所述搜索策略的搜索路徑圖，其中迭代次數(shù)為792次，路徑規(guī)劃的時間是17.89s，黑線是最終搜索到的路徑。

圖6是通過改變σ的值來調(diào)試適合實際情況的修正過后的高斯分布采樣，通過將μ值設置為目標點，改變σ值來改變隨機點的分布情況，圖6所示為σ值分別為20,50,80情況下的RRT*路徑圖，通過這種改變，搜索的時間遞減，搜索采樣的迭代次數(shù)遞減，可以看出改方法的改進可以提高算法的收斂性。最后通過目標點反解得到一系列關(guān)節(jié)角集合，得到最終機械臂各關(guān)節(jié)位姿。最后在ros仿真環(huán)境中使用moveit和rviz顯現(xiàn)六自由度機械臂應用改進的RRT*算法的抓取實驗圖。