[發明專利]一種機械臂避障路徑規劃方法

有效

• 申請號: 202310385761.2
• 申請日: 2023-04-11
• 公開（公告）號: CN116352714B
• 公開（公告）日: 2023-09-26
• 發明（設計）人: 黃曉濤;馮桑;陳樹濤;胡焱;黃藝明
• 申請（專利權）人: 廣東工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 上海思真遠達專利代理事務所(特殊普通合伙) 31481
• 代理人: 戚淼

1.一種機械臂避障路徑規劃方法，其特征在于，包括如下步驟：

步驟一：對機械臂模型以及障礙物進行簡化，確定機械臂關節點，采用虛擬關節點插補對相鄰關節間的連桿進行插補；在每個關節點處采用球形包圍盒進行包絡以代替機械臂模型，使得機械臂處于任何姿態使均能完整替代用以距離計算；

步驟二：采用眼在手外的布置方式放置深度相機，實時獲取工作空間的機械臂以及障礙物點云信息，并采用機器人實時過濾包將機械臂本體的點云信息進行濾除，僅保留障礙物點云信息；

步驟三：在機械臂運動前使用RRT-Connect算法對機械臂所處的靜態環境進行路徑規劃，依托其完備性獲得一條可行的全局規劃路徑；

步驟四：控制機械臂沿全局規劃路徑運動，并實時獲取機械臂所在空間的障礙物點云信息，在機械臂坐標下對障礙物點云與機械臂關節點進行距離計算，以獲得最短距離；

步驟五：通過改進人工勢場法進行局部路徑規劃實現避障；實時確定機械臂末端與目標點的距離與位置向量，根據機械臂末端與目標點的距離與位置向量確定目標點對機械臂末端的吸引力大小；

所述目標點為控制機械臂到達的目標位置；所述實時確定機械臂末端與目標點的距離和位置向量，根據機械臂末端與目標點的距離和位置向量確定吸引力大小的函數公式為：

其中，ρ(q,q_goal)為機械臂末端與目標點之間的歐氏距離；U_att為改進后的引力場函數，ξ'為引力的動態增益系數，其表達方式為其中動態增益系數與機械臂到目標點之間距離成反比，起始點的增益系數最小，從而使引力大小得以調節，防止引力過大使機械臂和障礙物之間發生碰撞，保證機械臂獲得一條安全且平滑的路徑；

實時確定機械臂關節點與障礙物點云之間的最短距離，當所述最短距離大于設定安全距離時，障礙物對機械臂的排斥力為0，而當最短距離小于設定安全距離時，根據機械臂上的關節點和最近障礙物點確定排斥力的大小，所述排斥力大小的函數公式為：

式中U_rep為障礙物產生的斥力勢場；ρ₀為安全距離半徑；ρ_i(q,q_obst)為機械臂末端與第i個障礙物之間的歐氏距離；

F_rep1i為斥力分量，方向由障礙物指向機械臂末端，大小為：

F_rep2i為引力分量，方向是機械臂末端指向目標點，大小為：

當機械臂使用局部規劃器規劃路徑時，由所述吸引力和排斥力產生的合力引導機械臂運動；

步驟六：對機械臂末端引入一個指引力，該指引力在機械臂陷入局部最小值時使其運動進入下一位置進行重新規劃；

所述指引力的函數公式為：

進入局部極小值時的合力的函數公式：

其中s為障礙物個數；a為指引力系數；x的取值范圍為[1,10]；a的取值隨障礙物個數以及機械臂與障礙物之間的距離變化而變化，當障礙物個數為1，若此時機械臂與障礙物的距離大于安全距離的1/2，則a的取值為1，反之則a取值為0.5；當障礙物個數大于1時，則采用單調遞增函數作為指引函數對a進行賦值，直到機械臂脫離局部最小值點，若下一位置為依舊陷入局部最小值，則重復以上方法；

步驟七：在局部路徑規劃中引入預規劃機制；

步驟八：在局部路徑規劃中加入跟隨機制。

2.根據權利要求1所述的一種機械臂避障路徑規劃方法，其特征在于：所述RRT-Connect算法是基于隨機采樣方法對隨機采樣點進行連接以形成一條可行路徑，其連接過程類似樹的生長，因此稱為隨機樹，該算法通過從起始點與目標點同時生成兩棵隨機樹進行交匯獲得可行路徑以提升其路徑規劃速度；

為避免采樣時過于隨機，提升路徑的方向性，RRT-Connect算法還采用交替方式進行隨機樹的生長。