[發明專利]一種融合低差異序列與RRT算法的機械臂路徑規劃方法

有效

• 申請號: 202011242965.3
• 申請日: 2020-11-09
• 公開（公告）號: CN112356033B
• 公開（公告）日: 2021-09-10
• 發明（設計）人: 代偉;李創業;繆燕子;馬小平
• 申請（專利權）人: 中國礦業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 郝偉揚

1.一種融合低差異序列與RRT算法的機械臂路徑規劃方法，其特征在于，包括以下步驟：

步驟一：初始化工作環境

建立機械臂的運動學模型并確定機械臂的運動空間和障礙物Obstacle；獲取機械臂狀態空間中的起始點x_start和目標點x_goal；初始化隨機樹T_node；并確定RRT算法參數：目標偏置概率p、采樣池容量n、步長step及允許誤差error；

步驟二：關節角空間采樣

2.1設定目標偏置概率p，并在區間范圍(0，1)之間生成隨機數rand；

2.2若rand≤p，不再進行采樣，令采樣點x_sample等于目標點x_goal；

2.3若randp使用Sobol序列生成n個采樣點x_sample按與目標點x_goal歐式距離的遠近，從小到大排列進入采樣池；

步驟三：生成新節點

3.1尋找隨機樹T_node中距離采樣點x_sample最近的一個點作為最近節點x_nearest；

3.2在最近節點x_nearest到采樣點x_sample的連線方向上以步長step前進生成新節點x_new；

3.3進行碰撞檢測，判斷生成的新節點x_new對應的機械臂位姿是否與障礙物發生碰撞；在與障礙物發生碰撞的情況下，若當前采樣點x_sample是由步驟2.2得到的，轉到步驟二重新采樣；若當前采樣點x_sample是由步驟2.3得到的，舍棄該采樣點按順序選擇采樣池中下一個采樣點x_sample并轉到步驟三生成新節點x_new，除非采樣池中采樣點x_sample生成的新節點x_nearest都不滿足要求再重新進行采樣；若沒有檢測到碰撞則將新節點x_new存儲到隨機樹T_node，且最近節點x_nearest視為新節點x_new的父節點；

3.4根據更新后的隨機樹T_node，重復步驟二～步驟三，直到得到的最新的節點x_new滿足|x_new-x_goal|error，視為找到目標點；

3.5在隨機樹T_node中根據各節點的父子關系，反向搜索找到規劃路徑；

步驟四：路徑優化處理

將步驟三得到的路徑使用基于最小二乘法的五次多項式擬合方法擬合出光滑曲線，采樣光滑曲線上的點并按機械臂控制器的通訊協議通過串口發送控制指令。

2.根據權利要求1所述的融合低差異序列與RRT算法的機械臂路徑規劃方法，其特征在于：在所述步驟二中，所述Sobol序列是一種比偽隨機序列分布更加均勻的序列，是一組基于{v_i}所形成的序列，m_i2ⁱ且為正奇數；v_i，m_i的生成依賴于系數為a、階數為p的簡單多項式f(z)，多項式系數只為0或1，形式為：

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

當ip時需要指定m_i的值，而當ip時，m_i遞推公式為：

其中表示二進制按位異或，對于v_i的遞歸公式為：

其中表示向下取整。

得到Sobol序列的第i個數：

其中…b₃b₂b₁是i的二進制表示；

所述采樣點x_sample由機械臂各個關節角組成，每一個關節角由一個Sobol序列生成；通過改變多項式f(z)的系數，得到不同的Sobol序列；得到的Sobol序列均勻分布于(0,1)空間；采樣點x_sample中的一個關節角表示為min+(max-min)*x_i，其中min和max表示關節角上下限。