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

有效

• 申請?zhí)枺?/td>: 202011242965.3
• 申請日: 2020-11-09
• 公開（公告）號: CN112356033B
• 公開（公告）日: 2021-09-10
• 發(fā)明（設(shè)計(jì)）人: 代偉;李創(chuàng)業(yè);繆燕子;馬小平
• 申請（專利權(quán)）人: 中國礦業(yè)大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 江蘇圣典律師事務(wù)所 32237
• 代理人: 郝偉揚(yáng)

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

步驟一：初始化工作環(huán)境

建立機(jī)械臂的運(yùn)動學(xué)模型并確定機(jī)械臂的運(yùn)動空間和障礙物Obstacle；獲取機(jī)械臂狀態(tài)空間中的起始點(diǎn)x_start和目標(biāo)點(diǎn)x_goal；初始化隨機(jī)樹T_node；并確定RRT算法參數(shù)：目標(biāo)偏置概率p、采樣池容量n、步長step及允許誤差error；

步驟二：關(guān)節(jié)角空間采樣

2.1設(shè)定目標(biāo)偏置概率p，并在區(qū)間范圍(0，1)之間生成隨機(jī)數(shù)rand；

2.2若rand≤p，不再進(jìn)行采樣，令采樣點(diǎn)x_sample等于目標(biāo)點(diǎn)x_goal；

2.3若randp使用Sobol序列生成n個(gè)采樣點(diǎn)x_sample按與目標(biāo)點(diǎn)x_goal歐式距離的遠(yuǎn)近，從小到大排列進(jìn)入采樣池；

步驟三：生成新節(jié)點(diǎn)

3.1尋找隨機(jī)樹T_node中距離采樣點(diǎn)x_sample最近的一個(gè)點(diǎn)作為最近節(jié)點(diǎn)x_nearest；

3.2在最近節(jié)點(diǎn)x_nearest到采樣點(diǎn)x_sample的連線方向上以步長step前進(jìn)生成新節(jié)點(diǎn)x_new；

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

3.4根據(jù)更新后的隨機(jī)樹T_node，重復(fù)步驟二～步驟三，直到得到的最新的節(jié)點(diǎn)x_new滿足|x_new-x_goal|error，視為找到目標(biāo)點(diǎn)；

3.5在隨機(jī)樹T_node中根據(jù)各節(jié)點(diǎn)的父子關(guān)系，反向搜索找到規(guī)劃路徑；

步驟四：路徑優(yōu)化處理

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

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

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

當(dāng)ip時(shí)需要指定m_i的值，而當(dāng)ip時(shí)，m_i遞推公式為：

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

其中表示向下取整。

得到Sobol序列的第i個(gè)數(shù)：

其中…b₃b₂b₁是i的二進(jìn)制表示；

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