[發明專利]一種提高機器人碰撞力檢測精度及保護靈敏度的方法

有效

• 申請號: 202011146191.4
• 申請日: 2020-10-23
• 公開（公告）號: CN112318501B
• 公開（公告）日: 2022-02-22
• 發明（設計）人: 徐純科;楊金橋;夏久零
• 申請（專利權）人: 成都卡諾普機器人技術股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 四川省成都市天策商標專利事務所(有限合伙) 51213
• 代理人: 郭會

1.一種提高機器人碰撞力檢測精度及保護靈敏度的方法，其特征在于包括如下步驟：

步驟1：實時采集工業機器人各關節的位置q₁，…，q_i；

步驟2：根據各關節位置計算各關節速度

步驟3：根據各關節速度計算各關節加速度q_dd1，…，q_ddi；

步驟4：確定各關節的濾波器截止頻率f_cutoff；

確定各關節的濾波器截止頻率的方法為：

將各關節速度帶入下式得到各關節低通濾波器截止頻率f_cutoff，

f_cutoff＝kq_d

上式中f_cutoff為濾波器截止頻率，q_d為關節速度，k為比例系數；

步驟5：確定最終生效的濾波器截止頻率為各關節中最高截止頻率，將各關節的濾波器截止頻率f_cutoff帶入下式得到生效的濾波器截止頻率，

f_effct＝max(f_cutoff1，…，f_cutoffi)

上式中f_effct為生效的濾波器截止頻率，f_cutoff1，…，f_cutoffi為各軸各自的濾波器截止頻率；

步驟6：將各關節的位置、速度、加速度輸入步驟5中截止頻率為f_effct的低通濾波器中得到濾波后的位置q’₁，…，q’_i、速度q’_d1，…，q’_di、加速度q’_dd1，…，q’_ddi；

步驟7：從CAD模型中得到機器人本體各個關節、連桿以及末端負載的動力學參數，所述動力學參數包括質量、重心、轉動慣量；

步驟8：根據末端負載工件的參數預置幾組不同的工具質量、重心、轉動慣量，并根據實際的末端負載工況在編程指令中切換不同的工具號；

步驟9：根據不同的防碰撞應用要求預置幾組不同的碰撞靈敏度預設值，根據不同的需求在編程指令中切換不同的碰撞靈敏度預設值組號；

步驟10：系統根據編程指令中設置的不同的工具號得到不同的最小慣性參數集，包括慣性參數集M(q)，離心力和科氏力參數集重力參數集G(q)；

步驟11：計算各關節理論力矩；

步驟12：實時采集各關節電機的反饋電流I₁，…，I_i，帶入下式計算得到各關節的反饋力矩

τ＝GK_tI

上式中τ為關節力矩，G為減速比，K_t為電機力矩常數，I為電機電流；

步驟13：將各關節反饋力矩輸入步驟5中截止頻率為f_effct的低通濾波器中得到濾波后的各關節反饋力矩

步驟14：將理論力矩和反饋力矩帶入下式得到各關節的碰撞力矩絕對值τ_coll1，…，τ_colli，

τ_coll＝|τ_ref-τ_fbk|

上式中τ_coll為碰撞力矩，τ_ref為理論力矩，τ_fbk為反饋力矩；

步驟15：根據步驟9中預設的各關節的碰撞力靈敏度r_s1，…，r_si得到各關節的碰撞力檢測門檻τ_comp1，…，τ_compi，

τ_comp＝r_sτ_abs

上式中τ_comp碰撞力門檻，r_s為碰撞靈敏度，τ_abs為碰撞力最大門檻絕對值；

步驟16：分別比較各軸的碰撞力絕對值τ_coll1，…，τ_colli和各自的碰撞力門檻τ_comp1，…，τ_compi的大小檢測是否發生了碰撞；

步驟17：步驟16中只要有一個軸檢測到發生了碰撞就進入碰撞后處理流程將各軸都切換到力矩模式，并給定減速機和電機能承受的最大力矩，給定的減速機和電機能承受的最大力矩小于實際的所能承受的最大力矩；

步驟18：實時檢測各軸的速度，當速度接近0時候，采用不同的處理方式：不受重力影響的軸直接打開電機剎車并接通動態剎車短接電機三相繞組，受重力影響的軸保持力矩模式以及力矩給定不變。

2.根據權利要求1所述的一種提高機器人碰撞力檢測精度及保護靈敏度的方法，其特征在于，步驟11中，計算各關節理論力矩的方法為：

將濾波后的位置q’₁，…，q’_i，速度q’_d1，…，q’_di，加速度q’_dd1，…，q’_ddii帶入下式中得到各關節的理論力矩

上式中M(q)為慣性參數集，為離心力和科氏力參數集，G(q)為重力，為關節角加速度，為關節角速度，τ為關節力矩。