[發明專利]多關節機器人及其零位標定方法、裝置和可讀介質

有效

• 申請號: 202210674624.6
• 申請日: 2022-06-14
• 公開（公告）號: CN114872025B
• 公開（公告）日: 2022-11-15
• 發明（設計）人: 賀巖;黃舒平
• 申請（專利權）人: 上海捷勃特機器人有限公司
• 主分類號: B25J9/04
• 分類號: B25J9/04;B25J9/16
• 代理公司: 上海專利商標事務所有限公司 31100
• 代理人: 杜娟;駱希聰

1.一種多關節機器人的零位標定方法，所述多關節機器人包括底座、大臂和小臂，所述大臂位于第二關節和第三關節之間，所述小臂位于第四關節和機器人末端之間，其特征在于，包括：

控制所述小臂的小臂軸線與所述底座的底座安裝平面平行；

控制第一關節勻速旋轉，所述第一關節位于第一初始位置，通過小臂慣性傳感器獲取所述小臂的第一動態加速度信息，根據所述第一動態加速度信息、大臂長度和所述小臂慣性傳感器的位置計算第一偏轉角，控制所述第二關節和所述第三關節旋轉所述第一偏轉角，使所述第二關節和所述第三關節位于各自的零位；

控制所述第四關節從第四初始位置旋轉至目標位置，通過所述小臂慣性傳感器獲取所述小臂的第一加速度信息，根據所述第一加速度信息計算所述第四關節的第四偏轉角，控制所述第四關節從所述第四初始位置旋轉所述第四偏轉角，使所述第四關節位于其零位；以及

控制所述第二關節和所述第三關節移動以使所述小臂中的電磁線圈位于靠近所述底座的目標位置，控制所述電磁線圈兩端的電壓使所述電磁線圈分別產生方向相反的第一磁場和第二磁場，讀取所述底座中的磁力計的磁力計數據，所述磁力計數據包括所述第一磁場對應的第一矢量和所述第二磁場對應的第二矢量，使所述第一矢量和所述第二矢量相減獲得第三矢量，獲得所述第三矢量投影在所述底座安裝平面上的第三投影矢量，根據所述磁力計的安裝方向和所述第三投影矢量的當前方向計算第二偏轉角，控制所述第一關節旋轉所述第二偏轉角，使所述第一關節位于其零位，其中，所述磁力計的安裝方向和所述第一關節的零位方向重合。

2.如權利要求1所述的零位標定方法，其特征在于，控制所述小臂的小臂軸線與所述底座的底座安裝平面平行的步驟包括：

當所述第一關節靜止在所述第一初始位置時，通過所述小臂慣性傳感器獲取所述小臂的初始加速度信息；

控制所述第一關節勻速旋轉，通過所述小臂慣性傳感器獲取所述小臂的初始動態加速度信息；

根據所述初始加速度信息和所述初始動態加速度信息計算所述小臂與所述底座安裝平面之間的第一夾角；以及

控制所述第二關節或所述第三關節旋轉所述第一夾角，使所述小臂軸線與所述底座的底座安裝平面平行。

3.如權利要求2所述的零位標定方法，其特征在于，采用下面的公式計算所述第一夾角：

其中，θ表示所述第一夾角，(α0_x,α0_y,α0_z)表示所述初始加速度信息，(β0_x,β0_y,β0_z)表示所述初始動態加速度信息，(x,y,z)表示所述小臂慣性傳感器的小臂坐標系的每個坐標軸。

4.如權利要求2所述的零位標定方法，其特征在于，還包括：調整所述第一關節的位置到除所述第一初始位置之外的至少一個其他位置，當所述第一關節位于所述其他位置時，重新計算所述第一夾角，將所述第一初始位置對應的第一夾角和所述至少一個其他位置對應的至少一個第一夾角的平均值作為所述第一夾角。

5.如權利要求4所述的零位標定方法，其特征在于，所述至少一個其他位置包括所述第一初始位置沿第一方向轉動90度或所述第一初始位置沿第二方向轉動90度，所述第一方向和所述第二方向相反。

6.如權利要求1所述的零位標定方法，其特征在于，所述大臂長度是第二關節旋轉軸和第三關節旋轉軸之間的垂直距離。

7.如權利要求6所述的零位標定方法，其特征在于，根據所述第一動態加速度信息、大臂長度和所述小臂慣性傳感器的位置計算第一偏轉角的步驟包括，采用下面的公式計算：

其中，γ表示所述第一偏轉角，L表示所述小臂慣性傳感器的位置到所述第三關節的距離，y表示所述小臂慣性傳感器的小臂坐標系中的y軸，β1_y表示所述第一動態加速度信息中的y軸加速度，表示所述第一關節的角速度，D表示所述大臂長度。