[發明專利]一種改進的RRT算法及工業機器人路徑避障規劃方法

有效

• 申請號: 202011484365.8
• 申請日: 2020-12-16
• 公開（公告）號: CN112677153B
• 公開（公告）日: 2022-09-06
• 發明（設計）人: 劉亞秋;劉勛;呂云蕾;劉麗娜;趙漢琛;孫海超;王兆樂;趙月
• 申請（專利權）人: 東北林業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 哈爾濱市偉晨專利代理事務所(普通合伙) 23209
• 代理人: 陳潤明

1.一種改進的RRT算法，其特征在于：結合Dijkstra算法使用貪心策略計算從起始點到目標點的最短路徑，得到改進的RRT算法方程，其算法步驟為：

步驟A，初始化變量；

步驟B，隨機采樣生成節點qnew，并進行碰撞檢測，若發生碰撞則進入下一次迭代，重新采樣，否則進行下一步；采用包絡法來簡化機械臂與障礙物之間的碰撞檢測，包絡法將機械臂的各個連桿用圓柱體包絡起來，將關節處用球體包絡起來，將障礙物同樣用球體和柱體包絡起來，由此將碰撞檢測問題轉換成球體與球體，柱體與球體，柱體與柱體之間的位置關系問題；

球體與球體指關節處與障礙物之間的位置關系，計算得出：

(3)

ri代表的是關節的包絡半徑，rj代表障礙物的包絡半徑；

1) 若，不發生碰撞；

2) 若，發生碰撞；

柱體與球體指連桿與障礙物之間的位置關系：設點B到直線MN的最短距離為dmin，根據柱體與球的位置關系如下幾種情況：

1) ，不發生碰撞

2) ，分以下幾種情況討論

a) 若，則發生碰撞；

b) 若，則分以下兩種情況；

i. 當點M、N分布在B的兩側時，，發生碰撞；

ii. 當點M、N分布在B的同一側時，，不發生碰撞；

柱體與柱體之間的位置關系：設點Q為機械臂連桿C1上的一個插值點，點M，N為柱體障礙物C2上的相鄰插值點，P點為線段MN中點，得到，當Q在MN上的投影為P點時，Q點到M和N點的距離達到最大值，考慮兩個柱體碰撞的極限情況，當Q點所在切面與C2相切時，距離最近的大小為，規定只要C1上所有的插值點與C2上所有的插值點之間的距離大于(R+r)時，即認為兩者不發生碰撞；

步驟C，判斷生成的qnew節點與qgoal是否滿足提前給定的約束，若滿足，則轉步驟G，否則轉步驟D；

步驟D，根據生成的qnew情況，調用自適應步長策略，調整步長因子；

步驟E，根據擴展點選擇策略，擴展新節點，然后進行碰撞檢測，如果無碰撞，則生成新節點qnew然后轉步驟C判斷，否則轉到步驟F；

步驟F，此時判斷生成樹是否陷入了局部極小值，如果陷入局部極小值，則調用局部逃脫算法生成qnew然后轉步驟C判斷，若沒陷入極小值轉步驟D；

步驟G，此時一個滿足系統約束的隨機擴展樹已經生成，調用Dijkstra算法優化路徑，若新路徑代價較小，則更新路徑；

步驟H，返回生成的RRT樹，構建過程結束。

2.工業機器人路徑避障規劃方法，其特征在于：其方法步驟如下：

步驟一，對機械臂進行建模，利于建模過程計算處正逆運動學算法；

步驟二，利用機械臂所處環境，給定目標位姿和障礙物環境；

步驟三，通過步驟二參數給定三維任務空間約束，并基于約束，利用權利要求1所述一種改進的RRT算法在機械臂的笛卡爾空間搜索路徑，使其即不與障礙物碰撞，也不發生自碰撞；

步驟四，利用步驟三獲得的路徑，進行貪心優化，并利用約束對優化后的路徑上的插值點，確定出該點處的機械臂最優位姿；

步驟五，通過控制機械臂的行徑，整合出最優位姿集合，使機械臂無碰撞的達到目標位姿；

步驟六，根據上述步驟得到的結果進行驗證。

3.根據權利要求2所述的工業機器人路徑避障規劃方法，其特征在于：步驟一中所述的機械臂是指六自由度的串聯機械臂。