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

有效

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

本申請屬于機器人技術領域，尤其涉及一種質心位姿估計方法、裝置、計算機可讀存儲介質及機器人。所述方法包括：獲取機器人的腰部測量姿態和支撐腿的各個關節角；根據所述腰部測量姿態和各個關節角計算所述支撐腿的腳踝姿態；根據所述腳踝姿態對預設的初始腳踝位置進行補償，得到所述支撐腿的腳踝位置；根據所述支撐腿的腳踝位姿和各個關節角計算所述機器人的腰部位姿；根據所述腰部位姿計算所述機器人的質心位姿。通過本申請，在質心位姿的計算過程中對腳踝位姿進行了充分考慮，能夠有效地處理腳掌翻轉的情況，極大提高了結果的精確度。

技術領域

本申請屬于機器人技術領域，尤其涉及一種質心位姿估計方法、裝置、計算機可讀存儲介質及機器人。

背景技術

仿人機器人的步態規劃與平衡控制是機器人技術領域的熱門課題。步態規劃算法和平衡控制算法都是基于反饋控制，反饋控制的前提是獲取機器人狀態的精確反饋值，而機器人狀態的反饋值獲取通常有兩類：一類可以直接從傳感器中獲取，另一類則需要通過其它傳感器獲取其它間接的機器人狀態，然后進行運動學和動力學計算得到機器人狀態，其過程較為復雜和困難。

例如，質心位姿估計通常是仿人機器人步態規劃與平衡控制的一個難點，現有技術中的一些質心位姿估計算法通常要求機器人支撐腿與地面完全貼合，即支撐腿的腳踝姿態均為零，然而在機器人實際運動中，如擬人行走、舞蹈等場景，通常會存在腳掌翻轉的情況，通過現有的這些算法所估計出的質心位姿精確度較低。

發明內容

有鑒于此，本申請實施例提供了一種質心位姿估計方法、裝置、計算機可讀存儲介質及機器人，以解決現有的質心位姿估計方法精確度較低的問題。

本申請實施例的第一方面提供了一種質心位姿估計方法，可以包括：

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

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

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

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

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

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

在第一方面的一種具體實現中，所述根據所述腳踝姿態對預設的初始腳踝位置進行補償，得到所述支撐腿的腳踝位置，可以包括：

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

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

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

在第一方面的一種具體實現中，所述根據所述腳踝姿態確定所述支撐腿的腳踝到足底支撐點的位置矢量，可以包括：

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

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