[發明專利]一種種植牙手術機器人末端柔順控制方法

在審

• 申請號: 202210167765.9
• 申請日: 2022-02-23
• 公開（公告）號: CN114505859A
• 公開（公告）日: 2022-05-17
• 發明（設計）人: 祝勝山;汪陽;房鶴;崔小飛;田忠正
• 申請（專利權）人: 四川鋒準機器人科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京市盛峰律師事務所 11337
• 代理人: 席小東

1.一種種植牙手術機器人末端柔順控制方法，其特征在于，包括以下步驟：

步驟S1，構建種植牙手術機器人的末端位姿控制器；

步驟S2，構建二階質量-彈簧-阻尼系統F＝{F₁,F₂,…,F₆}，其中，F₁,F₂,…,F₆分別代表種植牙手術機器人末端六個自由度的二階質量-彈簧-阻尼系統；六個自由度是指：種植牙手術機器人末端具有xyz坐標系，第1個自由度為沿x軸平移自由度；第2個自由度為沿y軸平移自由度；第3個自由度為沿z軸平移自由度；第4個自由度為繞x軸轉動自由度；第5個自由度為繞y軸轉動自由度；第6個自由度為繞z軸轉動自由度；

對于任意第i個自由度的二階質量-彈簧-阻尼系統，i＝1,2,…,6，包括：第i質量塊，第i彈簧和第i阻尼器；第i質量塊的底部一側通過第i彈簧與支撐臺連接，第i質量塊的底部另一側通過第i阻尼器與支撐臺連接；

步驟S3，在種植牙手術機器人的末端安裝末端力-力矩傳感器以及末端實際位姿檢測器，在種植牙手術機器人運動過程中，通過末端力-力矩傳感器實時檢測機器人末端受到的環境作用力F_e在每個自由度的力分量，表示為：F_e＝{F_e1,F_e2,…,F_e6}，其中，F_ei代表機器人末端受到的環境作用力在第i個自由度的力分量；i＝1,2,…,6；

根據末端力-力矩傳感器實時檢測到的第i個自由度的力分量和力矩分量，計算得到以下兩個參數:

第i個自由度下，由于力分量F_ei的作用所引起的運動速度

第i個自由度下，由于力分量F_ei的作用所引起的運動位移x_ei；

通過末端實際位姿檢測器，實時檢測種植牙手術機器人的當前末端實際位姿，包括末端實際位置和末端實際姿態；

步驟S4，將得到的F_ei、和x_ei實時輸入到第i個自由度的二階質量-彈簧-阻尼系統，采用下式，得到第i個自由度的二階質量-彈簧-阻尼系統中質量塊的加速度

其中：

Mi：代表第i個自由度的二階質量-彈簧-阻尼系統的質量塊的質量；

Bi：代表第i個自由度的二階質量-彈簧-阻尼系統的阻尼器的阻尼系數；

Ki：代表第i個自由度的二階質量-彈簧-阻尼系統的彈簧的剛度系數；

步驟S5，將第i個自由度的二階質量-彈簧-阻尼系統中質量塊的加速度進行積分，獲得第i個自由度的二階質量-彈簧-阻尼系統中質量塊的位移

步驟S6，將第i個自由度的二階質量-彈簧-阻尼系統中質量塊的位移輸入到步驟S1構建的末端位姿控制器，對種植牙手術機器人的末端進行控制，具體方法為：

步驟S6.1，期望末端位姿包括期望末端位置和期望末端姿態；其中，期望末端位置包括第1個自由度的期望末端位置、第2個自由度的期望末端位置和第3個自由度的期望末端位置；

期望末端姿態包括第4個自由度的期望末端姿態、第5個自由度的期望末端姿態和第6個自由度的期望末端姿態；

步驟S6.2，確定新期望末端位置和新期望末端姿態：

將第1個自由度的期望末端位置和第1個自由度的二階質量-彈簧-阻尼系統中質量塊的位移進行累加運算，得到第1個自由度的新期望末端位置targetPosition(1)；

將第2個自由度的期望末端位置和第2個自由度的二階質量-彈簧-阻尼系統中質量塊的位移進行累加運算，得到第2個自由度的新期望末端位置targetPosition(2)；

將第3個自由度的期望末端位置和第3個自由度的二階質量-彈簧-阻尼系統中質量塊的位移進行累加運算，得到第3個自由度的新期望末端位置targetPosition(3)；

將第4個自由度的期望末端姿態和第4個自由度的二階質量-彈簧-阻尼系統中質量塊的位移進行累加運算，得到第4個自由度的新期望末端姿態targetPose(4)；

將第5個自由度的期望末端姿態和第5個自由度的二階質量-彈簧-阻尼系統中質量塊的位移進行累加運算，得到第5個自由度的新期望末端姿態targetPose(5)；

將第6個自由度的期望末端姿態和第6個自由度的二階質量-彈簧-阻尼系統中質量塊的位移進行累加運算，得到第6個自由度的新期望末端姿態targetPose(6)；

步驟S6.3，對于末端位置控制方法為：

步驟S6.3.1，采用下式，得到第u個自由度對應的末端位置偏差E_position(u),u＝1,2,3：

E_position(u)＝targetPosition(u)-currentPosition(u)

其中：currentPosition(u)代表第u個自由度對應的種植牙手術機器人末端實際位置，通過步驟S3的末端實際位姿檢測器檢測到；

步驟S6.3.2，采用下式，得到第u個自由度對應的種植牙手術機器人末端沿著機器人基座坐標系平移線速度V_position(u)：

V_position(u)＝k_p1(u)*E_position(u)

其中：k_p1(u)代表第u個自由度對應的種植牙手術機器人末端位置偏差的比例系數；

步驟S6.4，對于末端姿態控制方法為：

步驟S6.4.1，采用下式，得到第v個自由度對應的末端姿態偏差E_pose(v),v＝4,5,6：

E_pose(v)＝targetPose(v)-currentPose(v)

其中：

currentPose(v)代表第v個自由度對應的種植牙手術機器人末端實際姿態，通過步驟S3的末端實際位姿檢測器檢測到；

targetPose(v)代表第v個自由度對應的種植牙手術機器人新期望末端姿態；

步驟S6.4.2，采用下式，得到第v個自由度對應的種植牙手術機器人末端姿態偏差的變化速度

其中：

currentErrorPose(v)代表第v個自由度對應的種植牙手術機器人當前末端姿態偏差；

preErrorPose(v)代表第v個自由度對應的種植牙手術機器人上一次末端姿態偏差；

步驟S6.4.3，采用下式，得到第v個自由度對應的種植牙手術機器人末端繞著機器人基座坐標系旋轉的角速度V_pose(v)：

其中：

k_p2(v)代表第v個自由度對應的種植牙手術機器人末端姿態偏差的比例系數；

k_d2(v)代表第v個自由度對應的種植牙手術機器人末端姿態偏差變化率的比例系數；

步驟S6.5，由此得到六個自由度的速度控制量，分別為：第1個自由度對應的平移線速度V_position(1)、第2個自由度對應的平移線速度V_position(2)、第3個自由度對應的平移線速度V_position(3)、第4個自由度對應的旋轉的角速度V_pose(4)、第5個自由度對應的旋轉的角速度V_pose(5)、第6個自由度對應的旋轉的角速度V_pose(6)；

將六個自由度的速度控制量生成速度控制指令，發送給種植牙手術機器人末端機械臂，對種植牙手術機器人末端機械臂進行六個自由度的控制，由此實現對種植牙手術機器人末端柔性控制。

2.根據權利要求1所述的一種種植牙手術機器人末端柔順控制方法，其特征在于，還包括：通過調節每個自由度對應的二階質量-彈簧-阻尼系統中質量塊的質量值和/或彈簧的剛度系數和/或阻尼器的阻尼系數，實時調整種植牙手術機器人末端在六個自由度上的柔順性以及響應的快慢。