Humanoid robot and its balance control method and computer readable storage medium

A humanoid robot, comprising:

• a processor;
• a memory coupled to the processor;
• and
• one or more computer programs stored in the memory and executable on the processor;
• wherein, the one or more computer programs comprise:
• instructions for obtaining an angular velocity and angular displacement of each joint of the humanoid robot and motion data of a sole of the humanoid robot, motion data of a centroid of the humanoid robot, and motion data of a waist of the humanoid robot;
• instructions for obtaining an expected acceleration of the sole corresponding to a current expected balance trajectory, an expected acceleration of the centroid corresponding to the current expected balance trajectory, and an expected angular acceleration of the waist corresponding to the current expected balance trajectory, based on the motion data of the sole, the motion data of the centroid, and the motion data of the waist, respectively;
• instructions for calculating an expected angular acceleration of each joint meeting a control requirement of the sole, a control requirement of the centroid, and a control requirement of the waist while the humanoid robot corresponds to the current expected balance trajectory, based on the angular velocity of the joint, the expected angular acceleration of the waist, the expected acceleration of the sole, and the expected acceleration of the centroid;
• and
• instructions for controlling each joint of the humanoid robot to move at the obtained expected angular acceleration of the joint based on the angular displacement of the joint, so that the humanoid robot maintains balance according to the current expected balance trajectory;
• wherein the expected balance trajectory comprises a preset balance trajectory corresponding to two balance control parts comprising the sole and the centroid;
• wherein the preset balance trajectory comprises a reference acceleration, a reference pose, and a reference velocity of the corresponding balance control part;
• wherein the motion data of the sole comprises a motion speed and a motion pose of the sole, and the motion data of the centroid comprises a motion pose and a motion speed of the centroid, and the instructions for obtaining the expected acceleration of the sole and the expected acceleration of the centroid corresponding to the current expected balance trajectory, based on the motion data of the sole and the motion data of the centroid, respectively comprise:
• instructions for each balance control part corresponding to the sole and the centroid, subtracting the reference pose and the motion pose of the balance control part and subtracting the reference speed and the motion speed of the balance control part to obtain a corresponding pose difference and speed difference;
• instructions for selecting a difference parameter matching a preset control function corresponding to the balance control part from the obtained pose difference and speed difference;
• and
• instructions for substituting the reference acceleration of the balance control part and the selected difference parameter into the corresponding preset control function to obtain the expected acceleration of the balance control part;
• wherein the expected balance trajectory further comprises a preset balance trajectory of the waist wherein the preset balance trajectory of the waist comprises a reference angular acceleration, a reference posture angle, and a reference angular velocity of the waist, and the motion data of the waist comprises a motion posture angle and a motion angular velocity of the waist, and the instructions for obtaining the expected angular acceleration of the waist corresponding to the current expected balance trajectory, based on the motion data of the waist comprise:
• instructions for subtracting the reference posture angle and the motion posture angle of the waist and subtracting the reference angular velocity and the motion angular velocity of the waist to obtain a corresponding posture angle difference and angular velocity difference;
• instructions for selecting a difference angle parameter matching a preset control function corresponding to the waist from the posture angle difference and the angular velocity difference;
• and
• instructions for substituting the reference angular acceleration of the waist and the selected difference angle parameter into the corresponding preset control function to obtain the expected angular acceleration of the waist;
• and
• wherein the instructions for calculating the expected angular acceleration of each joint meeting the control requirement of the sole, the control requirement of the centroid, and the control requirement of the waist while the humanoid robot corresponds to the current expected balance trajectory, based on the angular velocity of the joint, the expected angular acceleration of the waist, the expected acceleration of the sole, and the expected acceleration of the centroid comprise:
• instructions for calculating a first null space matrix for the sole, based on the angular velocity of each joint and the motion speed of the sole;
• instructions for calculating a second null space matrix for the sole and the centroid, based on the angular velocity of each joint, the first null space matrix and the motion speed of the centroid;
• and
• instructions for calculating the angular acceleration of each joint meeting the null space corresponding to the first null space matrix and the null space corresponding to the second null space matrix while the humanoid robot corresponds to the current expected balance trajectory to obtain the corresponding expected angular acceleration of the joint based on the angular velocity of the joint, the first null space matrix, the second null space matrix, the expected angular acceleration of the waist, the expected acceleration of the sole, and the expected acceleration of the centroid.

A non-transitory computer readable storage medium for storing one or more computer programs, wherein the one or more computer programs comprise:

• instructions for obtaining an angular velocity and angular displacement of each joint of the humanoid robot and motion data of a sole of the humanoid robot, motion data of a centroid of the humanoid robot, and motion data of a waist of the humanoid robot;
• instructions for obtaining an expected acceleration of the sole corresponding to a current expected balance trajectory, an expected acceleration of the centroid corresponding to the current expected balance trajectory, and an expected angular acceleration of the waist corresponding to the current expected balance trajectory, based on the motion data of the sole, the motion data of the centroid, and the motion data of the waist, respectively;
• instructions for calculating an expected angular acceleration of each joint meeting a control requirement of the sole, a control requirement of the centroid, and a control requirement of the waist while the humanoid robot corresponds to the current expected balance trajectory, based on the angular velocity of the joint, the expected angular acceleration of the waist, the expected acceleration of the sole, and the expected acceleration of the centroid;
• and
• instructions for controlling each joint of the humanoid robot to move at the obtained expected angular acceleration of the joint based on the angular displacement of the joint, so that the humanoid robot maintains balance according to the current expected balance trajectory;
• wherein the expected balance trajectory comprises a preset balance trajectory corresponding to two balance control parts comprising the sole and the centroid;
• wherein the preset balance trajectory comprises a reference acceleration, a reference pose, and a reference velocity of the corresponding balance control part;
• wherein the motion data of the sole comprises a motion speed and a motion pose of the sole, and the motion data of the centroid comprises a motion pose and a motion speed of the centroid, and the instructions for obtaining the expected acceleration of the sole and the expected acceleration of the centroid corresponding to the current expected balance trajectory, based on the motion data of the sole and the motion data of the centroid, respectively comprise:
• instructions for each balance control part corresponding to the sole and the centroid, subtracting the reference pose and the motion pose of the balance control part and subtracting the reference speed and the motion speed of the balance control part to obtain a corresponding pose difference and speed difference;
• instructions for selecting a difference parameter matching a preset control function corresponding to the balance control part from the obtained pose difference and speed difference;
• and
• instructions for substituting the reference acceleration of the balance control part and the selected difference parameter into the corresponding preset control function to obtain the expected acceleration of the balance control part;
• wherein the expected balance trajectory further comprises a preset balance trajectory of the waist wherein the preset balance trajectory of the waist comprises a reference angular acceleration, a reference posture angle, and a reference angular velocity of the waist, and the motion data of the waist comprises a motion posture angle and a motion angular velocity of the waist, and the instructions for obtaining the expected angular acceleration of the waist corresponding to the current expected balance trajectory, based on the motion data of the waist comprise:
• instructions for subtracting the reference posture angle and the motion posture angle of the waist and subtracting the reference angular velocity and the motion angular velocity of the waist to obtain a corresponding posture angle difference and angular velocity difference;
• instructions for selecting a difference angle parameter matching a preset control function corresponding to the waist from the posture angle difference and the angular velocity difference;
• and
• instructions for substituting the reference angular acceleration of the waist and the selected difference angle parameter into the corresponding preset control function to obtain the expected angular acceleration of the waist;
• and
• wherein the instructions for calculating the expected angular acceleration of each joint meeting the control requirement of the sole, the control requirement of the centroid, and the control requirement of the waist while the humanoid robot corresponds to the current expected balance trajectory, based on the angular velocity of the joint, the expected angular acceleration of the waist, the expected acceleration of the sole, and the expected acceleration of the centroid comprise:
• instructions for calculating a first null space matrix for the sole, based on the angular velocity of each joint and the motion speed of the sole;
• instructions for calculating a second null space matrix for the sole and the centroid, based on the angular velocity of each joint, the first null space matrix and the motion speed of the centroid;
• and
• instructions for calculating the angular acceleration of each joint meeting the null space corresponding to the first null space matrix and the null space corresponding to the second null space matrix while the humanoid robot corresponds to the current expected balance trajectory to obtain the corresponding expected angular acceleration of the joint based on the angular velocity of the joint, the first null space matrix, the second null space matrix, the expected angular acceleration of the waist, the expected acceleration of the sole, and the expected acceleration of the centroid.