[发明专利]双足机器人的姿态测量方法、装置、设备及存储介质

权利要求书

1.一种双足机器人的姿态测量方法，其特征在于，包括：

根据双足机器人在腾空脚着地时刻的躯干倾角和所述着地时刻的预设关节的位置，分别计算着地时刻所述腾空脚足端到躯干的第一位姿变换矩阵，和支撑脚足端到躯干的第二位姿变换矩阵；

根据所述第一位姿变换矩阵和所述第二位姿变换矩阵，计算所述腾空脚足端相对所述支撑脚足端的位置变换矩阵；

根据所述位置变换矩阵和所述着地时刻所述支撑脚的位置，计算所述腾空脚在所述着地时刻的第一位置；

根据所述第一位置，计算所述着地时刻所述双足机器人的位姿参数。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

根据所述双足机器人在双足支撑状态的躯干倾角和所述双足支撑状态的预设关节的位置，计算所述双足支撑状态下双脚足端到躯干的位姿变换矩阵；

根据所述双足支撑状态下双脚足端到躯干的位姿变换矩阵和双脚的位置，对所述双足机器人的躯干位置进行校正；

根据校正后的所述双足机器人的躯干位置，计算所述双足支撑状态下所述双足机器人的位姿参数。

3.根据权利要求2所述的方法，其特征在于，所述根据所述双足支撑状态下双脚足端到躯干的位姿变换矩阵和双脚的位置，对所述双足机器人的躯干位置进行校正，包括：

计算获取双脚支撑比例；

根据所述双足支撑状态下双脚足端到躯干的位姿变换矩阵、双脚的位置以及所述双脚支撑比例，对所述双足机器人的躯干位置进行校正。

4.根据权利要求3所述的方法，其特征在于，所述计算获取双脚支撑比例，包括：

根据所述双足支撑状态下所述双脚的支撑重量和所述双足机器人的重量，分别计算所述双脚支撑比例。

5.根据权利要求3所述的方法，其特征在于，所述计算获取双脚支撑比例，包括：

根据所述双足支撑状态下所述双脚的位置和所述双足机器人的质心位置，分别计算所述双脚支撑比例。

6.根据权利要求1所述的方法，其特征在于，所述根据双足机器人在腾空脚着地时刻的躯干倾角和所述着地时刻的预设关节的位置，分别计算着地时刻所述腾空脚足端到躯干的第一位姿变换矩阵，和支撑脚足端到躯干的第二位姿变换矩阵之前，还包括：

根据所述双足机器人在单足支撑状态的躯干倾角和所述单足支撑状态的预设关节的位置，计算单足支撑状态下单足支撑脚足端到躯干的位姿变换矩阵；

根据所述单足支撑状态下单足支撑脚足端到躯干的位姿变换矩阵和单足支撑脚的位置，计算所述单足支撑状态下所述双足机器人的位姿参数。

7.根据权利要求1-6任一项所述的方法，其特征在于，所述根据所述第一位置，计算所述着地时刻所述双足机器人的位姿参数，包括：

根据所述第一位置，计算所述着地时刻所述躯干在世界坐标系的位姿矩阵；

根据所述躯干在世界坐标系中的位姿矩阵和所述双足机器人中其他连杆到躯干的位姿矩阵，计算获取所述着地时刻所述其他连杆在世界坐标系中的位姿矩阵。

8.一种双足机器人的姿态测量装置，其特征在于，包括：

第一计算模块，用于根据双足机器人在腾空脚着地时刻的躯干倾角和所述着地时刻的预设关节的位置，分别计算着地时刻所述腾空脚足端到躯干的第一位姿变换矩阵，和支撑脚足端到躯干的第二位姿变换矩阵；

第二计算模块，根据所述第一位姿变换矩阵和所述第二位姿变换矩阵，计算所述腾空脚足端相对所述支撑脚足端的位置变换矩阵；

位置计算模块，根据所述位置变换矩阵和所述着地时刻所述支撑脚的位置，计算所述腾空脚在所述着地时刻的第一位置；

位姿参数计算模块，根据所述第一位置，计算所述着地时刻所述双足机器人的位姿参数。