[發明專利]重力補償方法及裝置

有效

• 申請號: 202011586561.6
• 申請日: 2020-12-28
• 公開（公告）號: CN112757296B
• 公開（公告）日: 2022-06-03
• 發明（設計）人: 辜紀文;林嘉;辜正濤
• 申請（專利權）人: 江西明天高科技股份有限公司;南昌嘉研科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J18/00;G06F30/23;G06F17/13;G06F17/15
• 代理公司: 北京清亦華知識產權代理事務所(普通合伙) 11201
• 代理人: 何世磊

1.一種重力補償方法，其特征在于，包括：

實時獲取各個旋轉電機中電機編碼器的輸出值，并根據所述旋轉電機中編碼器的當前輸出值確定對應的當前轉角，其中，所述電機連接在相鄰兩個連桿的鉸接處，所述旋轉電機的定子與其中一所述連桿的稍部通過轉軸固定連接，所述旋轉電機的輸出軸與另一所述連桿的基部固定連接；

根據每個所述旋轉電機的當前轉角計算與其輸出軸連接的所述連桿與水平方向的當前夾角；

獲取每個所述連桿的長度和重量，以及每個所述連桿的重心到其基部轉軸的距離，并根據每個所述連桿的當前夾角、每個所述連桿的長度和每個所述連桿的重量，以及每個所述連桿的重心到其基部轉軸的距離計算各個所述旋轉電機的重力補償力矩；

將各個所述補償力矩分別發送至對應的所述旋轉電機，以使各個所述旋轉電機根據接收到的補償力矩進行輸出力矩補償；

所述并根據每個所述連桿的當前夾角、每個所述連桿的重量和每個所述連桿的重心到其基部轉軸的距離計算各個所述旋轉電機的重力補償力矩的步驟包括：

根據當前連桿的當前夾角和所述當前連桿的重心到其基部轉軸的距離計算所述當前連桿相對于所述當前連桿之前的每一所述旋轉電機的重力力臂長度；

根據所述重力力臂長度以及所述當前連桿的當前夾角計算所述旋轉電機的重力補償力矩；

所述旋轉電機的重力補償力矩的計算公式為：

其中，M_i為重力補償力矩，L_k為第k個連桿的長度，l_j和l_i分別為第j個和第i個連桿的重心到其基部轉軸的距離，G_i為第i個連桿的重量，L_ij為連桿j對電機i的重力力臂長，cos(A_k)為第k個連桿的當前夾角，cos(A_j)為第j個連桿的當前夾角；

所述根據所述旋轉電機中編碼器的當前輸出值確定對應的當前轉角的步驟包括：

獲取所述旋轉電機在預設轉角時，在力補償狀態下所述電機編碼器的標準輸出值；

根據所述旋轉電機中編碼器的當前輸出值、所述預設轉角，以及所述標準輸出值計算所述旋轉電機的當前轉角；

所述旋轉電機的當前轉角的計算公式為：

其中，a_i為旋轉電機的當前轉角，e_i為電機編碼器的當前輸出值，e_i0為電機編碼器對應的標準輸出值，a_i0為所述預設轉角，E_i為電機編碼器的最大輸出值；

所述連桿的當前夾角的計算公式為：

A_i＝A_i-1+a_i，(A_i-1+a_i)＜2π；

其中，A_i為連桿i與水平面的當前夾角，A_i-1為連桿i-1與水平面的當前夾角，a_i為旋轉電機i的當前轉角，m為取整數部分的值。

2.一種力反饋機械臂的重力補償裝置，其特征在于，包括：

轉角模塊，用于實時獲取各個旋轉電機中電機編碼器的輸出值，并根據所述旋轉電機中編碼器的當前輸出值確定對應的當前轉角，其中，所述電機連接在相鄰兩個連桿的鉸接處，所述旋轉電機的定子與其中一所述連桿的稍部通過轉軸固定連接，所述旋轉電機的輸出軸與另一所述連桿的基部固定連接；

第一計算模塊，用于根據每個所述旋轉電機的當前轉角計算與其輸出軸連接的所述連桿與水平方向的當前夾角；

配置模塊，用于獲取每個所述連桿的重量和每個所述連桿的重心到其基部轉軸的距離；

第二計算模塊，用于根據每個所述連桿的當前夾角、每個所述連桿的重量和每個所述連桿的重心到其基部轉軸的距離計算各個所述旋轉電機的重力補償力矩；

執行模塊，用于將各個所述補償力矩分別發送至對應的所述旋轉電機，以使各個所述旋轉電機根據接收到的補償力矩進行輸出力矩補償；

所述第二計算模塊具體用于：

根據當前連桿的當前夾角和所述當前連桿的重心到其基部轉軸的距離計算所述當前連桿相對于所述當前連桿之前的每一所述旋轉電機的重力力臂長度；

根據所述重力力臂長度以及所述當前連桿的當前夾角計算所述旋轉電機的重力補償力矩；

所述旋轉電機的重力補償力矩的計算公式為：

其中，M_i為重力補償力矩，L_k為第k個連桿的長度，l_j和l_i分別為第j個和第i個連桿的重心到其基部轉軸的距離，G_i為第i個連桿的重量，L_ij為連桿j對電機i的重力力臂長，cos(A_k)為第k個連桿的當前夾角，cos(A_j)為第j個連桿的當前夾角；

所述轉角模塊具體用于：

獲取所述旋轉電機在預設轉角時，在力補償狀態下所述電機編碼器的標準輸出值；

根據所述旋轉電機中編碼器的當前輸出值、所述預設轉角，以及所述標準輸出值計算所述旋轉電機的當前轉角；

所述旋轉電機的當前轉角的計算公式為：

其中，a_i為旋轉電機的當前轉角，e_i為電機編碼器的當前輸出值，e_i0為電機編碼器對應的標準輸出值，a_i0為所述預設轉角，E_i為電機編碼器的最大輸出值；

所述連桿的當前夾角的計算公式為：

A_i＝A_i-1+a_i，(A_i-1+a_i)＜2π；

其中，A_i為連桿i與水平面的當前夾角，A_i-1為連桿i-1與水平面的當前夾角，a_i為旋轉電機i的當前轉角，m為取整數部分的值。