[发明专利]质心位姿估计方法、装置、计算机可读存储介质及机器人

说明书

本申请属于机器人技术领域，尤其涉及一种质心位姿估计方法、装置、计算机可读存储介质及机器人。所述方法包括：获取机器人的腰部测量姿态和支撑腿的各个关节角；根据所述腰部测量姿态和各个关节角计算所述支撑腿的脚踝姿态；根据所述脚踝姿态对预设的初始脚踝位置进行补偿，得到所述支撑腿的脚踝位置；根据所述支撑腿的脚踝位姿和各个关节角计算所述机器人的腰部位姿；根据所述腰部位姿计算所述机器人的质心位姿。通过本申请，在质心位姿的计算过程中对脚踝位姿进行了充分考虑，能够有效地处理脚掌翻转的情况，极大提高了结果的精确度。

技术领域

本申请属于机器人技术领域，尤其涉及一种质心位姿估计方法、装置、计算机可读存储介质及机器人。

背景技术

仿人机器人的步态规划与平衡控制是机器人技术领域的热门课题。步态规划算法和平衡控制算法都是基于反馈控制，反馈控制的前提是获取机器人状态的精确反馈值，而机器人状态的反馈值获取通常有两类：一类可以直接从传感器中获取，另一类则需要通过其它传感器获取其它间接的机器人状态，然后进行运动学和动力学计算得到机器人状态，其过程较为复杂和困难。

例如，质心位姿估计通常是仿人机器人步态规划与平衡控制的一个难点，现有技术中的一些质心位姿估计算法通常要求机器人支撑腿与地面完全贴合，即支撑腿的脚踝姿态均为零，然而在机器人实际运动中，如拟人行走、舞蹈等场景，通常会存在脚掌翻转的情况，通过现有的这些算法所估计出的质心位姿精确度较低。

发明内容

有鉴于此，本申请实施例提供了一种质心位姿估计方法、装置、计算机可读存储介质及机器人，以解决现有的质心位姿估计方法精确度较低的问题。

本申请实施例的第一方面提供了一种质心位姿估计方法，可以包括：

步骤1、获取机器人的腰部测量姿态和支撑腿的各个关节角；

步骤2、根据所述腰部测量姿态和各个关节角计算所述支撑腿的脚踝姿态；

步骤3、根据所述脚踝姿态对预设的初始脚踝位置进行补偿，得到所述支撑腿的脚踝位置；

步骤4、根据所述支撑腿的脚踝位姿和各个关节角计算所述机器人的腰部位姿：

其中，p_F为所述脚踝位置，R_F为所述脚踝姿态的旋转矩阵，i为各个关节角的序号，1≤i≤Num，Num为所述机器人的支撑腿的关节角数目，p_i为第i个关节角的位置，R_i为第i个关节角的旋转矩阵，p_T为所述机器人的腰部位置，为所述机器人的腰部计算姿态的旋转矩阵；

步骤5、确定所述机器人的各个连杆在本体坐标系下的质心位置；根据各个连杆在本体坐标系下的质心位置、预设的各个连杆的质量、预设的所述机器人的质量计算所述机器人在本体坐标系下的质心位置；根据所述腰部位姿和所述机器人在本体坐标系下的质心位置计算所述机器人在世界坐标系下的质心位姿。

在第一方面的一种具体实现中，所述根据所述脚踝姿态对预设的初始脚踝位置进行补偿，得到所述支撑腿的脚踝位置，可以包括：

根据所述脚踝姿态确定所述支撑腿的脚踝到足底支撑点的位置矢量；

根据所述脚踝姿态和所述位置矢量计算所述支撑腿的脚踝位置补偿量；

将所述脚踝位置补偿量与所述初始脚踝位置进行叠加，得到所述支撑腿的脚踝位置。

在第一方面的一种具体实现中，所述根据所述脚踝姿态确定所述支撑腿的脚踝到足底支撑点的位置矢量，可以包括：

根据所述脚踝姿态中的俯仰角确定所述位置矢量的第一分量；

根据所述脚踝姿态中的翻滚角确定所述位置矢量的第二分量；