【11】證書號數：I714462
【45】公告日： 中華民國 109 (2020) 年 12 月 21 日
【51】Int. Cl.： B25J9/08 B25J19/00
(2006.01) (2006.01)
B25J13/08 G01L3/22
(2006.01) (2006.01)
發明 全 7 頁
【54】名 稱：並聯式機械手臂外力估測系統及其方法
【21】申請案號：109105755 【22】申請日： 中華民國 109 (2020) 年 02 月 21 日
【72】發 明 人： 藍振洋 (TW) LAN, CHEN YANG；劉孟昆 (TW) LIU, MENG KUN；郭翰 (TW) GUO, HAN
【71】申 請 人： 國立臺灣科技大學 NATIONAL TAIWAN UNIVERSITY OF SCIENCE AND TECHNOLOGY
臺北市大安區基隆路四段 43 號
【74】代 理 人： 林鼎鈞
【56】參考文獻：
CN 102049776B CN 109333530A
CN 103034123B US 7367771B2 審查人員：蔡文明
【57】申請專利範圍
1. 一種並聯式機械手臂外力估測系統，其包含：一並聯式機械手臂，所述並聯式機械手臂 包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與 各第二連桿相連接的操作平台；及一控制裝置，所述控制裝置與所述並聯式機械手臂的 所述馬達連接，所述控制裝置自所述馬達接收量測到的力矩值以及轉動位置，所述控制 裝置生成控制訊號以對所述並聯式機械手臂的所述馬達進行控制，所述控制裝置更包 含：一動態模型建立模組，透過對並聯式機械手臂的位置、速度以及加速度進行運動學 分析，再對並聯式機械手臂進行動力學分析，以得到所述並聯式機械手臂的動態模型，
所述並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出；一阻 抗控制建立模組，透過質量阻尼彈簧二階系統建立所述並聯式機械手臂與外力的一阻抗 控制器；及一外力估測模組，將所述並聯式機械手臂的實際軌跡以及所述並聯式機械手 臂的理論軌跡的誤差代入所述阻抗控制器以計算得到並聯式機械手臂所承受到的外力。
2. 如請求項 1 所述的並聯式機械手臂外力估測系統，其中所述動態模型為下列公式：
其中，τm為並聯式機械手 臂的馬達扭矩；q 為並聯式機械手臂各節點的角度向量； 為並聯式機械手臂各節點的角 速度向量； 為並聯式機械手臂各節點的角加速度向量；D 為並聯式機械手臂的慣性矩 陣；C 為並聯式機械手臂的離心力矩陣；G 為並聯式機械手臂的重力項矩陣；F 為並聯 式機械手臂的馬達摩擦力；JT為雅可比(Jacobian)矩陣的轉置矩陣；及 Fext為接觸外力。
3. 如請求項 1 所述的並聯式機械手臂外力估測系統，其中所述質量阻尼彈簧二階系統為下
列公式： 其中，P 為並聯式
機械手臂的真實位移； 為並聯式機械手臂的真實速度； 為並聯式機械手臂的真實加速 度；P 為並聯式機械手臂的期望位移； 為並聯式機械手臂的期望速度； 為並聯式
機械手臂的期望加速度；Mm為目標阻抗的質量；Bm為目標阻抗的阻尼；及 Km為目標 阻抗的彈簧。
4. 如請求項 1 所述的並聯式機械手臂外力估測系統，其中所述阻抗控制器為下列公式：
其中，τm為並聯式機械手臂的馬達扭矩；W-1為 JD-1的逆矩陣；J 為雅可比矩陣；j 為雅 可比矩陣的微分矩陣；D-1為並聯式機械手臂的慣性矩陣的逆矩陣； 為並聯式機械手臂 各節點的角速度向量；P 為並聯式機械手臂的真實位移； 為並聯式機械手臂的真實速 度； 為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移； 為並聯式 機械手臂的期望速度； 為並聯式機械手臂的期望加速度；Mm-1為目標阻抗的質量的逆 矩陣；Bm為目標阻抗的阻尼；Km為目標阻抗的彈簧；C 為並聯式機械手臂的離心力矩 陣；G 為並聯式機械手臂的重力項矩陣；F 為並聯式機械手臂的馬達摩擦力；JT為雅可 比(Jacobian)矩陣的轉置矩陣；及 Fext為接觸外力。
5. 如請求項 1 所述的並聯式機械手臂外力估測系統，其中所述外力估測模組透過下列公式 以計算得到並聯式機械手臂所承受到的外力：
其中，Fext為接觸外力；P 為並聯式機械手 臂的真實位移； 為並聯式機械手臂的真實速度；Pd為並聯式機械手臂的期望位移；
為並聯式機械手臂的期望速度；Bm為目標阻抗的阻尼；及 Km為目標阻抗的彈簧。
6. 一種並聯式機械手臂外力估測方法，其包含下列步驟：一並聯式機械手臂包含有三個馬 達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相 連接的操作平台；一控制裝置與所述並聯式機械手臂的所述馬達連接，所述控制裝置自 所述馬達接收量測到的力矩值以及轉動位置，所述控制裝置生成控制訊號以對所述並聯 式機械手臂的所述馬達進行控制；所述控制裝置透過對並聯式機械手臂的位置、速度以 及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手 臂的動態模型，所述並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別 與計算出；所述控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的一阻 抗控制器；及所述控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌 跡的誤差代入所述阻抗控制器以計算得到並聯式機械手臂所承受到的外力。
7. 如請求項 6 所述的並聯式機械手臂外力估測方法，其中所述動態模型為下列公式：
其中，τm為並聯式機械手臂
8. 如請求項 6 所述的並聯式機械手臂外力估測方法，其中所述質量阻尼彈簧二階系統為下 列公式：
其中，P 為並聯式機械手臂的真實位移； 為並聯式機械手臂的真實速度； 為並聯式機 械手臂的真實加速度；Pd為並聯式機械手臂的期望位移； 為並聯式機械手臂的期望速 度； 為並聯式機械手臂的期望加速度；Mm為目標阻抗的質量；Bm為目標阻抗的阻 尼；及 Km為目標阻抗的彈簧。
9. 如請求項 6 所述的並聯式機械手臂外力估測方法，其中所述阻抗控制器為下列公式：
其中，τm為並聯式機械手臂的馬達扭矩；W-1為 JD-1的逆矩陣；J 為雅可比矩陣；j 為雅 可比矩陣的微分矩陣；D-1為並聯式機械手臂的慣性矩陣的逆矩陣； 為並聯式機械手臂 各節點的角速度向量；P 為並聯式機械手臂的真實位移； 為並聯式機械手臂的真實速 度； 為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移； 為並聯式 機械手臂的期望速度； 為並聯式機械手臂的期望加速度；Mm-1為目標阻抗的質量的逆 矩陣；Bm為目標阻抗的阻尼；Km為目標阻抗的彈簧；C 為並聯式機械手臂的離心力矩 陣；G 為並聯式機械手臂的重力項矩陣；F 為並聯式機械手臂的馬達摩擦力；JT為雅可 比(Jacobian)矩陣的轉置矩陣；及 Fext為接觸外力。
10. 如請求項 6 所述的並聯式機械手臂外力估測方法，其中所述控制裝置透過下列公式以計 算得到並聯式機械手臂所承受到的外力：
其中，Fext為接觸外力；P 為並聯式機械手臂的真實位移； 為並聯式機械手臂的真實速 度；Pd為並聯式機械手臂的期望位移； 為並聯式機械手臂的期望速度；Bm為目標阻 抗的阻尼；及 Km為目標阻抗的彈簧。
圖式簡單說明
第 1 圖繪示為本發明並聯式機械手臂外力估測系統的並聯式機械手臂立體圖。
第 2 圖繪示為本發明並聯式機械手臂外力估測系統的系統方塊圖。
第 3 圖繪示為本發明並聯式機械手臂外力估測系統的阻抗控制架構圖。
第 4 圖繪示為本發明並聯式機械手臂外力估測方法的方法流程圖。
