[发明专利]合质心状态获取方法、装置、仿人机器人和可读存储介质

说明书

本发明实施例提供一种合质心状态获取方法、装置、仿人机器人和可读存储介质，该合质心状态包括合质心的速度，该方法包括：获取仿人机器人各个真实关节的运动状态和浮动基座的运动状态，该浮动基座等效为依次连接的若干个虚拟关节，根据相应关节的位置基于链乘法则依次计算单个连杆在世界坐标系中的转轴位置、质心位置和旋转矩阵，进而求解单个连杆质心的雅克比矩阵；根据每个连杆质心的雅克比矩阵和合质量计算合质心的雅克比矩阵；根据合质心的雅克比矩阵等计算合质心的速度。该方法基于真实关节的运动状态和浮动基座的运动状态，利用递归方式求解得到雅可比矩阵及其导数，可降低计算出错率，进而为机器人的相关运动控制带来了极大便利等。

技术领域

本发明涉及仿人机器人技术领域，尤其涉及一种合质心状态获取方法、装置、仿人机器人和可读存储介质。

背景技术

在仿人机器人的运动控制中，对机器人的合质心的合质心控制是至关重要的，例如，在机器人的平衡控制中，需要保证合质心的投影落(zero moment point，ZMP)在机器人足底支撑多边形内。再例如，机器人的步态控制需要驱动机器人的合质心移动等。由于仿人机器人的自由度较多，存在多个连杆，其合质心无法直接测量获得，因此需要通过其他手段获得。现有的一种常用做法则是将仿人机器人的合质心等效为腰部的位置，但是这只是一种近似方法，无法进行更为精确的质心控制操作。

发明内容

有鉴于此，本发明的目的是为了克服现有技术中的不足，提供一种合质心状态获取方法、装置、仿人机器人和可读存储介质。

本发明的实施例提供一种合质心状态获取方法，应用于仿人机器人，所述仿人机器人的合质心状态包括合质心的速度，所述方法包括：

获取仿人机器人各个真实关节的运动状态和浮动基座的运动状态，将所述浮动基座等效为依次连接的若干个虚拟关节，通过所述浮动基座的运动状态计算得到各个虚拟关节的运动状态，其中，每一所述真实关节或所述虚拟关节的运动状态包括对应关节的位置和速度；

根据相应关节的位置基于链乘法则依次计算第k个连杆在世界坐标系中的转轴位置、质心位置和旋转矩阵，并根据自世界坐标系原点起的k个连杆各自在世界坐标系中的所述转轴位置、质心位置和旋转矩阵求解所述第k个连杆质心的雅克比矩阵；其中，k依次取值为1,…,Dof，Dof为该仿人机器人的虚拟关节和真实关节的自由度总和；

根据每个连杆质心的所述雅克比矩阵和所述仿人机器人的合质量计算所述仿人机器人的合质心的雅克比矩阵；

根据所述合质心的雅克比矩阵和各个关节的速度计算所述仿人机器人的合质心的速度。

在一些实施例中，每个关节的运动状态还包括对应关节的加速度，所述仿人机器人的合质心状态还包括合质心的加速度，所述方法还包括：

根据各个关节的速度和所述第k个连杆质心的所述雅克比矩阵计算所述k个连杆各自的转轴位置的线速度和质心位置的角速度；

根据所述k个连杆中对应连杆的所述转轴位置的角速度和所述旋转矩阵求解所述对应连杆的旋转矩阵的导数；

根据所述k个连杆各自的所述质心位置、所述转轴位置、所述旋转矩阵的导数、所述转轴位置的线速度和所述质心位置的线速度求解所述第k个连杆质心的雅克比矩阵的导数；

根据每个连杆质心的所述雅克比矩阵的导数和所述仿人机器人的合质量计算所述仿人机器人的合质心的雅克比矩阵的导数；

根据所述合质心的雅克比矩阵、所述合质心的雅克比矩阵的导数和各个关节的速度及加速度计算所述合质心的加速度。

在一些实施例中，所述根据自世界坐标系原点起的k个连杆各自在世界坐标系中的所述转轴位置、质心位置和旋转矩阵求解所述第k个连杆质心的雅克比矩阵，包括：

构建第k个连杆质心的雅克比矩阵其中，i依次取值为1,…,k；