[發明專利]質心位姿估計方法、裝置、計算機可讀存儲介質及機器人

有效

• 申請號: 202110479942.2
• 申請日: 2021-04-30
• 公開（公告）號: CN113146638B
• 公開（公告）日: 2022-09-16
• 發明（設計）人: 白杰;陳春玉;葛利剛;劉益彰;周江琛;熊友軍
• 申請（專利權）人: 深圳市優必選科技股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 深圳中一聯合知識產權代理有限公司 44414
• 代理人: 劉永康

1.一種質心位姿估計方法，其特征在于，包括：

步驟1、獲取機器人的腰部測量姿態和支撐腿的各個關節角；

步驟2、根據所述腰部測量姿態和各個關節角計算所述支撐腿的腳踝姿態；

步驟3、根據所述腳踝姿態對預設的初始腳踝位置進行補償，得到所述支撐腿的腳踝位置；

步驟4、根據所述支撐腿的腳踝位姿和各個關節角計算所述機器人的腰部位姿：

其中，p_F為所述腳踝位置，R_F為所述腳踝姿態的旋轉矩陣，i為各個關節角的序號，1≤i≤Num，Num為所述機器人的支撐腿的關節角數目，p_i為第i個關節角的位置，R_i為第i個關節角的旋轉矩陣，p_T為所述機器人的腰部位置，為所述機器人的腰部計算姿態的旋轉矩陣；

步驟5、確定所述機器人的各個連桿在本體坐標系下的質心位置；根據各個連桿在本體坐標系下的質心位置、預設的各個連桿的質量、預設的所述機器人的質量計算所述機器人在本體坐標系下的質心位置；根據所述腰部位姿和所述機器人在本體坐標系下的質心位置計算所述機器人在世界坐標系下的質心位姿。

2.根據權利要求1所述的質心位姿估計方法，其特征在于，所述根據所述腳踝姿態對預設的初始腳踝位置進行補償，得到所述支撐腿的腳踝位置，包括：

根據所述腳踝姿態確定所述支撐腿的腳踝到足底支撐點的位置矢量；

根據所述腳踝姿態和所述位置矢量計算所述支撐腿的腳踝位置補償量；

將所述腳踝位置補償量與所述初始腳踝位置進行疊加，得到所述支撐腿的腳踝位置。

3.根據權利要求2所述的質心位姿估計方法，其特征在于，所述根據所述腳踝姿態確定所述支撐腿的腳踝到足底支撐點的位置矢量，包括：

根據所述腳踝姿態中的俯仰角確定所述位置矢量的第一分量；

根據所述腳踝姿態中的翻滾角確定所述位置矢量的第二分量；

根據所述支撐腿的腳踝高度確定所述位置矢量的第三分量。

4.根據權利要求2所述的質心位姿估計方法，其特征在于，所述根據所述腳踝姿態和所述位置矢量計算所述支撐腿的腳踝位置補償量，包括：

根據下式計算所述支撐腿的腳踝位置補償量：

Δp_F＝R_z(r_z)*(ll，dd，hh)^T+R_z(r_z)R_y(r_y)R_x(r_x)*(-ll，-dd，-hh)^T

其中，(ll，dd，hh)^T為所述位置矢量，(r_x，r_y，r_z)^T為所述腳踝姿態，R_x、R_y、R_z分別為繞x、y、z軸旋轉的旋轉矩陣，Δp_F為所述腳踝位置補償量。

5.根據權利要求1所述的質心位姿估計方法，其特征在于，在根據所述支撐腿的腳踝位姿和各個關節角計算所述機器人的腰部位姿之后，還包括：

計算所述腰部測量姿態與所述腰部計算姿態之間的誤差；

若所述誤差大于或等于預設的誤差閾值，則將所述腰部計算姿態作為所述腰部測量姿態，并返回執行步驟2及其后續步驟，直至所述誤差小于所述誤差閾值為止。