[發明專利]一種基于虛擬力的主從跟隨與避碰方法

在審

• 申請號: 202110596275.6
• 申請日: 2021-05-30
• 公開（公告）號: CN113334379A
• 公開（公告）日: 2021-09-03
• 發明（設計）人: 王越;鄭先杰;羅威;陳歡;樊衛華;郭健
• 申請（專利權）人: 南京理工大學
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J18/00
• 代理公司: 南京理工大學專利中心 32203
• 代理人: 薛云燕

1.一種基于虛擬力的主從跟隨與避碰方法，其特征在于，包括以下步驟：

步驟1、構建雙機械臂系統運動模型，得到機械臂末端相對于基座的位置和姿態；

步驟2、根據機械臂末端狀態，構建虛擬力場函數，計算雙機械臂末端的虛擬引力與虛擬斥力；

步驟3、通過計算出虛擬引力和斥力，采用主從控制模式確定雙機械臂跟隨的約束關系；

步驟4、對機械臂進行虛擬力優化，求虛擬的最終目標點，使雙機械臂系統跳出局值狀態；

步驟5、利用求解的虛擬目標點、障礙物、雙機械臂系統以及目標點之間的虛擬力場來對雙機械臂系統進行避碰規劃，判斷是否發生碰撞，實現避碰；

步驟6、從機械臂根據主從機械臂約束關系確定末端姿態，并將主機械臂加入到從機械臂的障礙物中，再對從機械臂進行軌跡規劃，實現從機械臂跟隨主機械臂。

2.根據權利要求1所述的基于虛擬力的主從跟隨與避碰方法，其特征在于，步驟1中構建雙機械臂系統運動模型，得到機械臂末端相對于基座的位置和姿態，具體包括以下步驟：

步驟1-1、采用DH參數法，建立連桿i的固連坐標系{O_i}，坐標系{O_i-1}設立在i關節處，與i-1連桿固連；坐標系{O_i}的三個坐標軸為x_i軸、y_i軸、z_i軸，采集關節扭角α_i、連桿長度a_i、關節偏移d_i以及旋轉角θ_i；

其中θ_i表示關于z_i-1軸，x_i軸在x_i-1軸下逆時針轉過的角度；d_i表示坐標系{O_i-1}沿z_i-1軸平移到z_i-1軸與x_i軸的交點所經過的距離；a_i表示沿x_i-1軸方向z_i-1軸到z_i軸之間的距離；α_i表示z_i-1軸繞x_i軸到z_i軸所轉過的角度；

步驟1-2、根據連桿間的一組DH參數，求出連桿間齊次變換矩陣

其中Trans(0,0,d_i)為沿著z_i-1軸移動d_i的齊次變換矩陣的簡寫，Trans(a_i,0,0)為沿著x_i-1軸移動a_i的齊次變換矩陣的簡寫，Rot(z_i-1,θ_i)為沿著z_i-1旋轉θ_i的齊次變換矩陣的簡寫，Rot(x_i-1,α_i)為沿著x_i-1旋轉α_i的齊次變換矩陣的簡寫；

步驟1-3、由齊次變換矩陣求解到六自由度機械臂末端相對于基座的位置和姿態，得到機械臂末端相對于基坐標系的齊次變換矩陣T,得到得到該坐標系相對于基坐標系的姿態向量

其中，n_x、o_x、a_x向量為x方向上的矩陣相乘所得積的縮寫，n_y、o_y、a_y向量為y方向上的矩陣相乘所得積的縮寫，n_z、o_z、a_z向量為z方向上的矩陣相乘所得積的縮寫，p_x、p_y、p_z向量代表了機械臂末端的位置參數。