[發明專利]基于多自由度機械臂的改進式RRT*的避障運動規劃方法

在審

• 申請號: 201710163211.0
• 申請日: 2017-03-19
• 公開（公告）號: CN106695802A
• 公開（公告）日: 2017-05-24
• 發明（設計）人: 龔道雄;李赟程;邵杰
• 申請（專利權）人: 北京工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京思海天達知識產權代理有限公司11203
• 代理人: 沈波

技術領域

本發明涉及到機械臂避障環節中運動規劃方法，具體是一種改進式快速隨機搜索的方法應用到全局空間，屬于機械臂運動規劃領域

背景技術

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

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

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

發明內容

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

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

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

步驟三：在位型空間內隨機采樣選取x_rand,選取x_rand附近臨近節點區域，根據節點區域公式得到x_near的集合X_near，其中d是空間維度，γ是選擇的常數，V是已構成搜索樹節點的集合，x′是搜索域中某一節點。

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

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

步驟六：對新生成的σ做碰撞檢測，如果新生成的路徑沒有碰撞到障礙物區間，則將節點x_rand和路徑σ添加到樹中；

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

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