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

权利要求书

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及其后续步骤，直至所述误差小于所述误差阈值为止。