Method for estimating pose of humanoid robot, humanoid robot and computer-readable storage medium

1. A computer-implemented method for estimating a pose of a humanoid robot to control the humanoid robot, wherein the humanoid robot comprises: a processor, an inertial measurement unit arranged at a center point of a waist of the humanoid robot, and detection sensors arranged at each joint of the humanoid robot, and wherein the method comprises: obtaining, through the inertial measurement unit, pose parameters of the waist of the humanoid robot, and obtaining, through the detection sensors, plantar motion parameters of the humanoid robot; processing, through the processor, the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain measured pose parameters of the center point of the waist of the humanoid robot, wherein the pose parameters of the waist comprise waist attitude angles and waist attitude angular velocities of the center point of the waist in a world coordinate system, and wherein the measured pose parameters of the center point of the waist comprise: measured position coordinates of the center point of the waist obtained by using the waist attitude angles and the plantar motion parameters, and a measured linear velocity of the center point of the waist obtained by using the waist attitude angular velocities and the plantar motion parameters; calculating, through the processor, predicted pose parameters of the center point of the waist according to the pose parameters of the waist; and fusing, through the processor, the measured pose parameters and the predicted pose parameters to obtain estimated pose parameters of the center point of the waist, and controlling, through the processor, the humanoid robot to move according to the estimated pose parameters of the center point of the waist.

2. The method of claim 1, wherein processing, through the processor, the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain the measured pose parameters of the center point of the waist of the humanoid robot, comprises: solving, through the processor, kinematics by using the pose parameters of the waist and the plantar motion parameters to obtain contact point coordinates, wherein the contact point coordinates are coordinates of a contact point between a sole of the humanoid robot and a support surface in the world coordinate system; and solving, through the processor, kinematics by using the contact point coordinates, the pose parameters of the waist and the plantar motion parameters to obtain the measured pose parameters of the center point of the waist.

3. The method of claim 2, wherein solving, through the processor, kinematics by using the contact point coordinates, the pose parameters of the waist and the plantar motion parameters to obtain the measured pose parameters of the center point of the waist, comprises: solving, through the processor, kinematics by using the waist attitude angles, the contact point coordinates, and the plantar motion parameters to obtain the measured position coordinates of the center point of the waist; and solving, through the processor, kinematics by using the waist attitude angular velocities, the contact point coordinates, and the plantar motion parameters to obtain the measured linear velocity of the center point of the waist.

4. The method of claim 2, wherein the plantar motion parameters comprise plantar displacement parameters of the humanoid robot; and solving, through the processor, kinematics by using the pose parameters of the waist and the plantar motion parameters to obtain the contact point coordinates, comprises: solving, through the processor, kinematics by using the pose parameters of the waist and the plantar displacement parameters to obtain plantar attitude angles of the humanoid robot; finding, through the processor, initial contact point coordinates corresponding to the plantar attitude angles in a preset pose configuration strategy, wherein the initial contact point coordinates are the coordinates of the contact point between the sole of the humanoid robot and the support surface in a sole coordinate system, the sole coordinate system is a coordinate system established according to the sole of the humanoid robot, and the preset pose configuration strategy comprise pre-configured coordinates of the contact point in the sole coordinate system when the sole is in different poses.

5. The method of claim 1, wherein the pose parameters of the waist further comprise a first waist linear acceleration in a waist coordinate system established according to the center point; and calculating, through the processor, the predicted pose parameters of the center point of the waist according to the pose parameters of the waist, comprises: converting, through the processor, the first waist linear acceleration into a second waist linear acceleration in the world coordinate system according to the waist attitude angles; and calculating, through the processor, a predicted linear velocity and predicted position coordinates of the center point according to the second waist linear acceleration, a historical linear velocity of the center point, and historical position coordinates of the center point, wherein the historical linear velocity is an estimated linear velocity corresponding to the center point of the waist at a historical moment, the historical position coordinates are corresponding estimated position coordinates of the center point at the historical moment, and the predicted attitude parameters comprise the predicted linear velocity and the predicted position coordinates.

6. The method of claim 1, wherein fusing, through the processor, the measured pose parameters and the predicted pose parameters to obtain the estimated pose parameters of the center point of the waist, comprises: calculating, through the processor, a Kalman gain at a current moment according to historical Kalman parameters corresponding to the historical moment; and processing, through the processor, the measured pose parameters and the predicted pose parameters by using the Kalman gain to obtain the estimated pose parameters of the center point of the waist.

7. The method of claim 1, wherein processing, through the processor, the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain the measured pose parameters of the center point of the waist of the humanoid robot, comprises: processing, through the processor, the pose parameters of the waist and plantar motion parameters corresponding to a support leg of at least two feet of the humanoid robot to obtain the measured pose parameters of the center point of the waist, wherein the support leg is one of the at least two legs that supports the humanoid robot on a support surface.

8. The method of claim 1, wherein the detection sensors comprise displacement sensors installed on legs of the humanoid robot, and the plantar motion parameters of the humanoid robot are obtained by the displacement sensors.

9. A humanoid robot comprising: one or more processors; an inertial measurement unit arranged at a center point of a waist of the humanoid robot and coupled to the one or more processors; detection sensors arranged at each joint of the humanoid robot and coupled to the one or more processors; and a memory coupled to the one or more processors, the memory storing programs that, when executed by the one or more processors, cause performance of operations comprising: obtaining, through the inertial measurement unit, pose parameters of the waist of the humanoid robot, and obtaining, through the detection sensors, plantar motion parameters of the humanoid robot; processing the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain measured pose parameters of the center point of the waist of the humanoid robot, wherein the pose parameters of the waist comprise waist attitude angles and waist attitude angular velocities of the center point of the waist in a world coordinate system, and wherein the measured pose parameters of the center point of the waist comprise: measured position coordinates of the center point of the waist obtained by using the waist attitude angles and the plantar motion parameters, and a measured linear velocity of the center point of the waist obtained by using the waist attitude angular velocities and the plantar motion parameters; calculating predicted pose parameters of the center point of the waist according to the pose parameters of the waist; and fusing the measured pose parameters and the predicted pose parameters to obtain estimated pose parameters of the center point of the waist, and controlling the humanoid robot to move according to the estimated pose parameters of the center point of the waist.

10. The humanoid robot of claim 9, wherein processing the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain the measured pose parameters of the center point of the waist of the humanoid robot, comprises: solving kinematics by using the pose parameters of the waist and the plantar motion parameters to obtain contact point coordinates, wherein the contact point coordinates are coordinates of a contact point between a sole of the humanoid robot and a support surface in the world coordinate system; and solving kinematics by using the contact point coordinates, the pose parameters of the waist and the plantar motion parameters to obtain the measured pose parameters of the center point of the waist.

11. The humanoid robot of claim 10, wherein solving kinematics by using the contact point coordinates, the pose parameters of the waist and the plantar motion parameters to obtain the measured pose parameters of the center point of the waist, comprises: solving kinematics by using the waist attitude angles, the contact point coordinates, and the plantar motion parameters to obtain the measured position coordinates of the center point of the waist; and solving kinematics by using the waist attitude angular velocities, the contact point coordinates, and the plantar motion parameters to obtain the measured linear velocity of the center point of the waist.

12. The humanoid robot of claim 10, wherein the plantar motion parameters comprise plantar displacement parameters of the humanoid robot; and solving kinematics by using the pose parameters of the waist and the plantar motion parameters to obtain contact point coordinates, comprises: solving kinematics by using the pose parameters of the waist and the plantar displacement parameters to obtain plantar attitude angles of the humanoid robot; finding initial contact point coordinates corresponding to the plantar attitude angles in a preset pose configuration strategy, wherein the initial contact point coordinates are the coordinates of the contact point between the sole of the humanoid robot and the support surface in a sole coordinate system, the sole coordinate system is a coordinate system established according to the sole of the humanoid robot, and the preset pose configuration strategy comprise pre-configured coordinates of the contact point in the sole coordinate system when the sole is in different poses.

13. The humanoid robot of claim 9, wherein the pose parameters of the waist further comprise a first waist linear acceleration in a waist coordinate system established according to the center point; and calculating the predicted pose parameters of the center point of the waist according to the pose parameters of the waist, comprises: converting the first waist linear acceleration into a second waist linear acceleration in the world coordinate system according to the waist attitude angles; and calculating a predicted linear velocity and predicted position coordinates of the center point according to the second waist linear acceleration, a historical linear velocity of the center point, and historical position coordinates of the center point, wherein the historical linear velocity is an estimated linear velocity corresponding to the center point of the waist at a historical moment, the historical position coordinates are corresponding estimated position coordinates of the center point at the historical moment, and the predicted attitude parameters comprise the predicted linear velocity and the predicted position coordinates.

14. The humanoid robot of claim 9, wherein fusing the measured pose parameters and the predicted pose parameters to obtain the estimated pose parameters of the center point of the waist, comprises: calculating a Kalman gain at a current moment according to historical Kalman parameters corresponding to the historical moment; and processing the measured pose parameters and the predicted pose parameters by using the Kalman gain to obtain the estimated pose parameters of the center point of the waist.

15. The humanoid robot of claim 9, wherein processing the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain the measured pose parameters of the center point of the waist of the humanoid robot, comprises: processing the pose parameters of the waist and plantar motion parameters corresponding to a support leg of at least two feet of the humanoid robot to obtain the measured pose parameters of the center point of the waist, wherein the support leg is one of the at least two legs that supports the humanoid robot on a support surface.

16. A non-transitory computer-readable storage medium storing instructions that, when executed by at least one processor of a humanoid robot, cause the at least one processor to perform a method, wherein the humanoid robot comprises: a processor, an inertial measurement unit arranged at a center point of a waist of the humanoid robot, and detection sensors arranged at each joint of the humanoid robot, and wherein the method comprises: obtaining, through the inertial measurement unit, pose parameters of the waist of the humanoid robot, and obtaining, through the detection sensors, plantar motion parameters of the humanoid robot; processing the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain measured pose parameters of the center point of the waist of the humanoid robot; calculating predicted pose parameters of the center point of the waist according to the pose parameters of the waist; and fusing the measured pose parameters and the predicted pose parameters to obtain estimated pose parameters of the center point of the waist, and controlling the humanoid robot to move according to the estimated pose parameters of the center point of the waist; wherein the pose parameters of the waist comprise waist attitude angles of the center point in a world coordinate system and a first waist linear acceleration in a waist coordinate system established according to the center point; and calculating the predicted pose parameters of the center point of the waist according to the pose parameters of the waist, comprises: converting the first waist linear acceleration into a second waist linear acceleration in the world coordinate system according to the waist attitude angles; and calculating a predicted linear velocity and predicted position coordinates of the center point according to the second waist linear acceleration, a historical linear velocity of the center point, and historical position coordinates of the center point, wherein the historical linear velocity is an estimated linear velocity corresponding to the center point of the waist at a historical moment, the historical position coordinates are corresponding estimated position coordinates of the center point at the historical moment, and the predicted attitude parameters comprise the predicted linear velocity and the predicted position coordinates.

17. The non-transitory computer-readable storage medium of claim 16, wherein processing the pose parameters of the waist of the humanoid robot and the plantar motion parameters of the humanoid robot to obtain the measured pose parameters of the center point of the waist of the humanoid robot, comprises: solving kinematics by using the pose parameters of the waist and the plantar motion parameters to obtain contact point coordinates, wherein the contact point coordinates are coordinates of a contact point between a sole of the humanoid robot and a support surface in the world coordinate system; and solving kinematics by using the contact point coordinates, the pose parameters of the waist and the plantar motion parameters to obtain the measured pose parameters of the center point of the waist.

18. The non-transitory computer-readable storage medium of claim 17, wherein the pose parameters of the waist further comprise waist attitude angular velocities of the center point of the waist in the world coordinate system; and solving kinematics by using the contact point coordinates, the pose parameters of the waist and the plantar motion parameters to obtain the measured pose parameters of the center point of the waist, comprises: solving kinematics by using the waist attitude angles, the contact point coordinates, and the plantar motion parameters to obtain measured position coordinates of the center point of the waist; and solving kinematics by using the waist attitude angular velocities, the contact point coordinates, and the plantar motion parameters to obtain a measured linear velocity of the center point of the waist, wherein the measured pose parameters of the center point of the waist comprise the measured position coordinates and the measured linear velocity.

19. The non-transitory computer-readable storage medium of claim 17, wherein the plantar motion parameters comprise plantar displacement parameters of the humanoid robot; and solving kinematics by using the pose parameters of the waist and the plantar motion parameters to obtain the contact point coordinates, comprises: solving kinematics by using the pose parameters of the waist and the plantar displacement parameters to obtain plantar attitude angles of the humanoid robot; finding initial contact point coordinates corresponding to the plantar attitude angles in a preset pose configuration strategy, wherein the initial contact point coordinates are the coordinates of the contact point between the sole of the humanoid robot and the support surface in a sole coordinate system, the sole coordinate system is a coordinate system established according to the sole of the humanoid robot, and the preset pose configuration strategy comprise pre-configured coordinates of the contact point in the sole coordinate system when the sole is in different poses.

20. The non-transitory computer-readable storage medium of claim 16, wherein fusing the measured pose parameters and the predicted pose parameters to obtain the estimated pose parameters of the center point of the waist, comprises: calculating a Kalman gain at a current moment according to historical Kalman parameters corresponding to the historical moment; and processing the measured pose parameters and the predicted pose parameters by using the Kalman gain to obtain the estimated pose parameters of the center point of the waist.