A control method for a humanoid robot to get up from the ground based on hybrid force-position control

Abstract

The present invention discloses a control method for a humanoid robot to get up from the ground based on hybrid force-position control. Initially, the robot is in a prone position. After receiving the instruction to perform the getting-up action, it starts to move, and the angles of each joint reach the corresponding angles calculated by inverse kinematics. Subsequently, torques are applied to each joint of the upper body one by one, and then the upper body is lifted off the ground. When the sole force enters the friction cone, the state of the lower body is switched from position control to force control. After the state is switched to force control, PD control is performed on the ankles and linear quadratic regulation is performed on the difference between the sole forces of the two feet until the pitch angle, roll angle, and yaw angle of the upper body are all 0, achieving the squatting state of the humanoid robot. Subsequently, it is switched from the squatting state to the final standing state. During this process, virtual constraints are applied and the standing-up is completed through position control. Finally, the humanoid robot can stand stably. The present invention adopts a simplified model to greatly simplify the standing-up process, designs a transitional posture, and well realizes the standing-up action of the robot.

Description

Technical Field

[0001] The present invention relates to a robot control method, in particular to a humanoid robot falling and getting up control method based on force-position hybrid control. Background Art

[0002] With the development of technology, robots, as tools to improve production efficiency and improve life, have received more and more attention. In recent years, "Atlas" of Boston Dynamics and "Optimus Prime" of Tesla have become extremely popular on the Internet, and the eyes of scientific and technological workers all over the world have refocused on humanoid robots. Compared with traditional industrial robotic arms, wheeled robots, etc., humanoid robots can better adapt to complex road surfaces, especially through discrete terrains, due to their human-like structures, and have far better adaptability than wheeled and tracked robots. In addition, most of the scenarios we live in are designed for humans, and humanoid robots can better integrate into human production and life to provide services for people.

[0003] The purpose of humans designing humanoid robots is to hope that they can help or replace humans to complete a certain task. However, due to the complexity of the environment, when a humanoid robot performs a certain task, it is inevitable to fall. Therefore, in order to improve the autonomy and adaptability of humanoid robots, it is very necessary to study the method of getting up after falling.

[0004] Generally speaking, there is less research on the falling and getting up of humanoid robots at present, and most of the existing methods are for the motion planning of static falling and getting up of humanoids controlled by high reduction ratio motors. Currently, the more representative ones are: the HRP-2 humanoid robot developed by Kawada Company in Japan in cooperation with the National Institute of Advanced Industrial Science and Technology, the iCub robot developed by the European Consortium of Innovative Universities, the NAO humanoid robot of Aldebaran Robotics in France, the iStruct robot of the German Research Center for Artificial Intelligence, and the Asimo robot of Honda Company in Japan.

[0005] The currently disclosed methods for humanoid robots to fall and get up mainly include the following:

[0006] Kanehiro F, Yoshimi T, etc. proposed a gait generator based on 3D reconstruction and height and center-of-mass compensation by changing the waist to realize the strategy of crawling by online adjusting the gait operation parameters, and experimental verification was carried out on HRP-2.

[0007] Gay S, Dégallier S et al. proposed a CPG-based motion controller and an artificial potential field-based motion planning algorithm. At the same time, visual tracking and an inverse kinematics solver were used to achieve the target reaching task. The closed-loop based on visual feedback enables the humanoid robot to move autonomously in a complex environment; the crawling gait based on potential field planning reaches the target and avoids obstacles.

[0008] Li C, Lowe R et al. proposed a neural network control based on a four-unit central pattern generator (utilizing sensor feedback) to plan the crawling strategy. The specific approach was to first learn under a simulation platform and then input the learned results into a physical NAO entity for experiments. Finally, a conclusion was obtained using the motion learning method of dynamic system theory to imitate the crawling gait of infants.

[0009] Kamioka T, Watabe T et al. proposed a bipedal and quadrupedal conversion motion planning based on dynamic gait at the 2015 IROS. This motion first plans the key postures of each part of the body, then calculates the motion trajectories of each part of the body, and finally calculates the ZMP and COG trajectories based on the inverted pendulum model to complete all planning actions.

[0010] However, the robots introduced above are mainly large reduction ratio position control, and the above methods are all proposed for this kind of structure and cannot be migrated to force control robots with small reduction ratios. Although the functions of falling, getting up and standing of force control humanoid robots have been realized on the PETMAN and ATLAS robots of Boston Dynamics in the United States, they have not publicly disclosed the relevant technologies and theories in detail.

[0011] Currently, the relevant patents that have been publicly disclosed: a method for state detection and standing planning of a humanoid robot after falling (Patent No.: CN202211034551.0) and a method for crawling trajectory planning and motion control of a humanoid robot (Patent No.: CN202210948202.3) are also research on the falling and getting up solutions of large reduction ratio motor position control robots on relatively flat ground, and do not consider the slight unevenness of the ground and the force control scheme. The high-dynamic small reduction ratio force control robots are receiving more and more attention because they can achieve relatively fast walking or even jumping and other motions and have robustness to various terrains compared with the static or quasi-static robots in these two patents. This patent focuses on elaborating a falling and getting up strategy applicable to high-dynamic robots. Summary of the Invention

[0012] The purpose of the present invention is to provide a control method for a humanoid robot to fall and get up based on force-position hybrid control in view of the deficiencies of the prior art.

[0013] The object of the present invention is achieved by the following technical solutions: A humanoid robot falling and getting up control method based on force-position hybrid control, comprising the following steps:

[0014] (1) Place the humanoid robot in the initial prone posture, and then send a getting-up action command to the humanoid robot through a computer;

[0015] (2) After the humanoid robot receives the getting-up action command, it starts to move, and the angles of each joint reach the corresponding angles calculated by inverse kinematics. At this time, the humanoid robot is in the prone transition state;

[0016] (3) After the humanoid robot is in the prone transition state, apply torques to the joints of the upper body of the humanoid robot respectively, and then lift the upper body of the humanoid robot off the ground;

[0017] (4) When the sole force of the humanoid robot enters the friction cone, switch the state of the lower body of the humanoid robot from position control to force control;

[0018] (5) After the state is switched to force control, perform PD control on the ankles of the humanoid robot and perform linear quadratic regulation on the difference in the sole forces of the two feet of the humanoid robot until the pitch angle, roll angle, and yaw angle of the upper body of the humanoid robot are all 0, and the squatting state of the humanoid robot is completed;

[0019] (6) Then, it is converted from the squatting state to the final standing state. During the process, virtual constraints are applied and the standing up is completed through position control. Finally, the humanoid robot can stand stably.

[0020] Furthermore, the humanoid robot consists of a torso, lower limbs and upper limbs; the center of mass of the torso is the torso center of mass; the lower limbs include hip joints, a left leg and a right leg, the left leg is connected to the torso through the hip joint, and the right leg is connected to the torso through the hip joint; the left leg includes a left thigh link, a left calf link and a left foot sole, the left foot sole is connected to the left calf link through the left ankle joint, the left calf link is connected to the left thigh link through the left knee joint, and the left thigh link is connected to the hip joint through the left hip joint; the right leg includes a right thigh link, a right calf link and a right foot sole, the right foot sole is connected to the right calf link through the right ankle joint, the right calf link is connected to the right thigh link through the right knee joint, and the right thigh link is connected to the hip joint through the right hip joint; the upper limbs include a left arm and a right arm; the left arm includes a left upper arm link, a left lower arm link and a left hand, the left hand is connected to the left lower arm link, the left lower arm link is connected to the left upper arm link through the left elbow joint, and the left upper arm link is connected to the torso through the left shoulder joint; the right arm includes a right upper arm link, a right lower arm link and a right hand, the right hand is connected to the right lower arm link, the right lower arm link is connected to the right upper arm link through the right elbow joint, and the right upper arm link is connected to the torso through the right shoulder joint; an inertial measurement unit is installed in the torso; force sensors are respectively installed on the left foot sole and the right foot sole; grating encoders are respectively installed on the left shoulder joint, the right shoulder joint, the left elbow joint, the right elbow joint, the left hip joint, the right hip joint, the left knee joint, the right knee joint, the left ankle joint and the right ankle joint.

[0021] Furthermore, step (1) is specifically as follows:

[0022] Before the start of getting up from the ground, the humanoid robot is made to lie prone on the ground in an initial prone posture through position control, and then a getting-up action instruction is sent to the humanoid robot by a computer; the initial prone posture is: the torso is parallel to the ground, both the left upper arm link and the right upper arm link are parallel to the torso, the angle θ1 between the left upper arm link and the left lower arm link and the angle θ2 between the right upper arm link and the right lower arm link are the same and less than 90°, both the right hand and the left hand are in contact with the ground, the angle θ3 between the left thigh link and the left calf link and the angle θ4 between the right thigh link and the right calf link are the same and less than 180°, and both the left knee joint and the right knee joint are in contact with the ground.

[0023] Furthermore, step (2) is specifically as follows:

[0024] After receiving the getting-up action instruction, the humanoid robot starts to move, and the angles of each joint reach the corresponding angles θ∈R calculated by inverse kinematics m , where m is the number of joints in the humanoid robot;

[0025] The angle θ∈R m is calculated by the following formula:

[0026] θ = IK(r com );

[0027]

[0028] where n is the number of links in the humanoid robot; r i is the centroid position of any one link; M i is the mass of any one link; r com is the centroid position of the humanoid robot; IK(·) is the inverse kinematics function.

[0029] Furthermore, the specific content of step (3) is as follows:

[0030] After the humanoid robot is in the prone transition state, torques τ1, τ2, τ3, and τ5 are respectively applied to the left shoulder joint, right shoulder joint, left elbow joint, and right elbow joint, so that the angle θ1 between the left upper arm link and the left lower arm link and the angle θ2 between the right upper arm link and the right lower arm link increase simultaneously until θ1 = θ2 = 180°, and then the right hand and the left hand leave the ground at the same time; during the above process, the angle θ3 between the left thigh link and the left lower leg link and the angle θ4 between the right thigh link and the right lower leg link remain unchanged;

[0031] The torques τ1, τ2, τ3, and τ5 are obtained through the following formula:

[0032]

[0033]

[0034]

[0035] where θ up is the set of joint angles of the left shoulder joint, right shoulder joint, left elbow joint, and right elbow joint, and the angles are measured in real time by the grating encoders in each joint; m is the mass of the torso; J is the Jacobian matrix; FK(·) is the forward kinematics function; F is the force applied to the upper body of the humanoid robot; H is the height of the centroid of the torso in the squatting vertical state; h is the height of the centroid of the torso at the moment when the right hand and the left hand leave the ground during the climbing process.

[0036] Furthermore, the specific content of step (4) is as follows:

[0037] After the right hand and the left hand leave the ground at the same time, when the sole forces of the left foot sole and the right foot sole enter the friction cone at the same time, the left leg and the right leg simultaneously perform a state switch from position control to force control, and the friction cone needs to meet the following conditions:

[0038] f min≤f z ≤f max ;

[0039] -μf z ≤±f x ≤μf z ;

[0040] -μf z ≤±f y ≤μf z ;

[0041] where f x is the x - component of the plantar force of the left or right foot sole in the world coordinate system w; f y is the y - component of the plantar force of the left or right foot sole in the world coordinate system w; f z is the z - component of the plantar force of the left or right foot sole in the world coordinate system w; f min = 0; f max = 1.5mg; μ is the friction coefficient of the ground; the plantar force of the left foot sole (5) or the right foot sole is obtained through the force sensor installed therein.

[0042] Further, the step (5) is specifically:

[0043] After the state is switched to force control, torques τ are applied to the left and right ankle joints respectively ankle ;

[0044] The torque τ ankle is calculated by the PD control algorithm:

[0045]

[0046] where θ pitch is the actual pitch angle of the torso; is the desired pitch angle of the torso; is the change rate of the actual pitch angle of the torso; is the change rate of the desired pitch angle of the torso, θ pitch and are obtained through the inertial measurement unit in the torso;

[0047] Based on the dynamic model, linear - quadratic regulation is performed on the difference between the plantar force of the left foot sole and the plantar force of the right foot sole. The dynamic model is:

[0048]

[0049] where x′ is the state variable; is the first - order derivative of the state variable; y θ roll is the angle of the roll angle of the torso in the local coordinate system y, is the angular velocity of the roll angle of the torso in the local coordinate system y, w θ yaw is the angle of the yaw angle of the torso in the world coordinate system w, is the angular velocity of the yaw angle of the torso in the world coordinate system w; A is the system matrix, B is the control matrix, l is the lateral length of the torso (1), I x is the inertia of the torso in the x direction in the local coordinate system y, I z the inertia of the torso in the z direction in the local coordinate system y; ΔF z is the difference in the z direction between the plantar force of the left foot sole and the plantar force of the right foot sole in the world coordinate system w; ΔF x is the difference in the x direction between the plantar force of the left foot sole and the plantar force of the right foot sole in the world coordinate system w;

[0050] and obtain the state - space equation of the error:

[0051]

[0052] where, is the derivative of the reference state; is the state error, x r is the reference state; is the input error, u r is the reference input;

[0053] Design a linear - quadratic regulator to calculate the feedback gain K, define the cost function I cost as:

[0054]

[0055] where, Q and R are positive - definite matrices respectively;

[0056] Let the cost function I cost be minimized, at this time K satisfies: K = -R -1 B T P, where, P is a constant positive - definite matrix and satisfies the algebraic Riccati equation: A T P + PA - PBR -1 B T P + Q = 0;

[0057] Solve the algebraic Riccati equation to obtain the feedback gain K;

[0058] Obtain the final control input:

[0059] u = u r + K(x′ - x r );

[0060] Until θ pitch , y θ roll , w θ yaw are all 0, the humanoid robot reaches the squatting state.

[0061] Furthermore, the specific steps of step (6) are as follows:

[0062] Subsequently, it transitions from the squatting state to the final standing state; during the process, the angle θ5 between the left calf link and the left ankle joint and the angle θ6 between the right calf link and the right ankle joint gradually increase from the angles in the squatting state until the humanoid robot can stand stably;

[0063] The angle θ7 between the left thigh link and the horizontal line of the torso and the angle θ3 between the left thigh link and the left calf link satisfy the following virtual constraints:

[0064] 0 = L1cos(θ5) + L2cos(θ5 + θ3);

[0065] π = θ5 + θ3 + θ7;

[0066] where L1 is the length of the left calf link; L2 is the length of the left thigh link;

[0067] The angle θ8 between the right thigh link and the horizontal line of the torso and the angle θ4 between the right thigh link and the right calf link satisfy the following virtual constraints:

[0068] 0 = L3cos(θ6) + L4cos(θ6 + θ4);

[0069] π = θ6 + θ4 + θ8;

[0070] where L3 is the length of the right calf link; L4 is the length of the right thigh link;

[0071] Finally, the humanoid robot can stand stably.

[0072] The beneficial effects of the present invention are as follows: Firstly, in view of the current situation that most of the control methods for a humanoid robot to fall and get up are based on position-controlled robots, when a humanoid robot needs to be designed for force control in order to achieve high-speed movement, the existing falling and getting up strategies for position control of humanoid robots with large reduction ratio motors are no longer applicable. At the same time, the adaptability of the force control-based method on complex terrains is greater than that of position-controlled robots. Therefore, the present invention focuses on the falling and getting up of force-controlled humanoid robots. The present invention adopts a simplified model to greatly simplify the standing-up process, designs a transitional posture, and calculates the magnitude of the force that the arm should exert. And by switching to force control at an appropriate time, the calculation error caused by inaccurate models is achieved, enabling the robot to switch to a squatting posture well. At the same time, in order to prevent tipping over throughout the process, a linear quadratic regulator is also designed to keep the roll angle and yaw angle stably near 0. In the final process of squatting to standing, a position control strategy with virtual constraints is adopted to well achieve the standing-up action of the robot. Description of the Drawings

[0073] Figure 1 It is a schematic diagram of a humanoid robot in Embodiment 1, where Figure 1 (a) is a front schematic diagram of a humanoid robot in Embodiment 1, Figure 1 (b) is a side schematic diagram of a humanoid robot in Embodiment 1;

[0074] Figure 2 It is a schematic flow diagram of a control method for a humanoid robot to fall and get up based on force-position hybrid control;

[0075] Figure 3 It is a schematic diagram of a simplified model of a linear quadratic regulator;

[0076] Figure 4 It is a schematic diagram of the lower body structure during the standing-up process, where Figure 4 (a) is a schematic diagram of the left leg structure during the standing-up process, Figure 4 (b) is a schematic diagram of the right leg structure during the standing-up process;

[0077] Figure 5 It is the posture change process of the humanoid robot throughout the process;

[0078] In the figure, 1 - torso; 2 - hip joint; 3 - left thigh link; 4 - left calf link; 5 - left foot sole; 6 - right thigh link; 7 - right calf link; 8 - right foot sole; 9 - left upper arm link; 10 - left lower arm link; 11 - right upper arm link; 12 - right lower arm link; 13 - right shoulder joint; 14 - right elbow joint; 15 - left shoulder joint; 16 - left elbow joint; 17 - right hip joint; 18 - right knee joint; 19 - right ankle joint; 20 - left hip joint; 21 - left knee joint; 22 - left ankle joint; 23 - right hand; 24 - left hand; 25 - torso center of mass. Detailed implementation manner

[0079] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0080] The objective of the present invention is to propose a control method for a humanoid robot to fall and get up with force-position hybrid control in view of the deficiencies of the prior art. To simplify the problem, the present invention only considers the fall and getting up of the humanoid robot after falling face down to the ground. For the fallen robot, first, a fixed motion plan needs to be completed to make the robot in a specific preparatory posture, and then based on a simplified inverted pendulum model, the conversion to the squatting motion is realized with the arms as the input. During the process, a linear quadratic regulator is used to prevent the robot from tipping over and make the supporting forces of the two legs as equal as possible. In the final standing-up stage, the stable standing-up motion of the robot is realized by applying virtual constraints, and the conversion from the squatting state to the standing-up state is completed.

[0081] Embodiment 1

[0082] In the present invention, the structure of the humanoid robot used is as Figure 1 (a) and Figure 1As shown in (b), the humanoid robot consists of a torso 1, lower limbs and upper limbs; the center of mass of the torso 1 is the torso center of mass 25; the lower limbs include a hip joint 2, a left leg and a right leg, the left leg is connected to the torso 1 through the hip joint 2, and the right leg is connected to the torso 1 through the hip joint 2; the left leg includes a left thigh link 3, a left calf link 4 and a left foot sole 5, the left foot sole 5 is connected to the left calf link 4 through a left ankle joint 22, the left calf link 4 is connected to the left thigh link 3 through a left knee joint 21, and the left thigh link 3 is connected to the hip joint 2 through a left hip joint 20; the right leg includes a right thigh link 6, a right calf link 7 and a right foot sole 8, the right foot sole 8 is connected to the right calf link 7 through a right ankle joint 19, the right calf link 7 is connected to the right thigh link 6 through a right knee joint 18, and the right thigh link 6 is connected to the hip joint 2 through a right hip joint 17. The upper limbs include a left arm and a right arm; the left arm includes a left upper arm link 9, a left lower arm link 10 and a left hand 24, the left hand 24 is connected to the left lower arm link 10, the left lower arm link 10 is connected to the left upper arm link 9 through a left elbow joint 16, and the left upper arm link 9 is connected to the torso 1 through a left shoulder joint 15; the right arm includes a right upper arm link 11, a right lower arm link 12 and a right hand 23, the right hand 23 is connected to the right lower arm link 12, the right lower arm link 12 is connected to the right upper arm link 11 through a right elbow joint 14, and the right upper arm link 11 is connected to the torso 1 through a right shoulder joint 13. Among them, an inertial measurement unit is installed in the torso 1 for measuring the attitude information of the torso 1; a first force sensor is installed in the left foot sole 5 for measuring the contact force between the left foot sole 5 and the ground surface. A second force sensor is installed in the right foot sole 8 for measuring the contact force between the right foot sole 8 and the ground surface. The left shoulder joint 15, the right shoulder joint 13, the left elbow joint 16, the right elbow joint 14, the left hip joint 20, the right hip joint 17, the left knee joint 21, the right knee joint 18, the left ankle joint 22 and the right ankle joint 19 are respectively equipped with grating encoders for measuring the angles and speeds of the corresponding joints.

[0083] As Figure 2 shown, the present invention proposes a control method for a humanoid robot to get up from the ground based on force-position hybrid control, including the following steps:

[0084] (1) Make the humanoid robot in the initial prone posture, and then send a getting-up action instruction to the humanoid robot through a computer;

[0085] The specific content of the step (1) is:

[0086] Before starting to get up from the ground, the humanoid robot is made to lie prone on the ground in an initial posture through position control, and then a getting-up motion instruction is sent to the humanoid robot by a computer; the initial posture is: the torso 1 is parallel to the ground, the left upper arm link 9 and the right upper arm link 11 are both parallel to the torso 1, the included angle θ1 between the left upper arm link 9 and the left lower arm link 10 and the included angle θ2 between the right upper arm link 11 and the right lower arm link 12 are the same and less than 90°, the right hand 23 and the left hand 24 both touch the ground, the included angle θ3 between the left thigh link 3 and the left lower leg link 4 and the included angle θ4 between the right thigh link 6 and the right lower leg link 7 are the same and less than 180°, and the left knee joint 21 and the right knee joint 18 both touch the ground.

[0087] Such an initial prone posture is mainly for subsequent exertion of force.

[0088] (2) After receiving the getting-up motion instruction, the humanoid robot starts to move, and the angles of each joint reach the corresponding angles calculated by inverse kinematics. At this time, the humanoid robot is in a prone transition state;

[0089] The specific steps of step (2) are as follows:

[0090] After receiving the getting-up motion instruction, the humanoid robot starts to move, and the angles of each joint reach the corresponding angles θ∈R calculated by inverse kinematics m , where m is the number of joints in the humanoid robot;

[0091] The angle θ∈R m is calculated by the following formula:

[0092] θ=IK(r com );

[0093]

[0094] where n is the number of links in the humanoid robot; r i is the centroid position of any link; M i is the mass of any link; r com is the centroid position of the humanoid robot; IK(·) is the inverse kinematics function.

[0095] (3) After the humanoid robot is in the prone transition state, torques are applied to each joint of the upper body of the humanoid robot, and then the upper body of the humanoid robot is lifted off the ground;

[0096] The specific steps of step (3) are as follows: After the humanoid robot is in the prone transition state, torques τ1, τ2, τ3, and τ5 are respectively applied to the left shoulder joint 15, the right shoulder joint 13, the left elbow joint 16, and the right elbow joint 14, so that the angle θ1 between the left upper arm link 9 and the left lower arm link 10 and the angle θ2 between the right upper arm link 11 and the right lower arm link 12 increase simultaneously until θ1 = θ2 = 180°. Subsequently, the right hand 23 and the left hand 24 leave the ground at the same time. During the above process, the angle θ3 between the left thigh link 3 and the left lower leg link 4 and the angle θ4 between the right thigh link 6 and the right lower leg link 7 remain unchanged. At the moment of leaving contact with the ground, the angle between the left upper arm link 9 and the left lower arm link 10 is 180°, and the angle between the right upper arm link 11 and the right lower arm link 12 is 180°, that is, the left arm composed of the left upper arm link 9, the left lower arm link 10, and the left hand 24 and the right arm composed of the right upper arm link 11, the right lower arm link 12, and the right hand 23 are both in a straight state.

[0097] And calculate the speed v1 of the trunk centroid 25 at the moment of leaving contact with the ground:

[0098]

[0099] Where, H is the height of the trunk centroid 25 in the squatting vertical state; h is the height of the trunk centroid 25 at the moment when the right hand 23 and the left hand 24 leave contact with the ground during the climbing process;

[0100] And calculate the time t for the humanoid robot to move from the initial posture to the moment when the right hand 23 and the left hand 24 leave contact with the ground at the same time:

[0101]

[0102] The torques τ1, τ2, τ3, and τ5 are obtained through the following formula:

[0103]

[0104]

[0105]

[0106] Where, θ up is the set of joint angles of the left shoulder joint 15, the right shoulder joint 13, the left elbow joint 16, and the right elbow joint 14. Among them, the angles are measured in real time by the grating encoders in each joint; m is the mass of the trunk 1; J is the Jacobian matrix; FK(·) is the forward kinematics function; F is the force applied to the upper body of the humanoid robot; H is the height of the trunk centroid 25 in the squatting vertical state; h is the height of the trunk centroid 25 at the moment when the right hand 23 and the left hand 24 leave contact with the ground during the climbing process.

[0107] (4) When the sole force of the humanoid robot enters the friction cone, the state of the lower body of the humanoid robot is switched from position control to force control;

[0108] The specific content of step (4) is as follows:

[0109] After the right hand 23 and the left hand 24 are simultaneously disengaged from the ground, when the sole forces of the left foot sole 5 and the right foot sole 8 simultaneously meet the following conditions, the left leg and the right leg are simultaneously switched from position control to force control, and the friction cone needs to meet the following conditions:

[0110] f min ≤f z ≤f max ;

[0111] -μf z ≤±f x ≤μf z ;

[0112] -μf z ≤±f y ≤μf z ;

[0113] Among them, f x is the component of the sole force of the left foot sole 5 or the right foot sole 8 in the x direction in the world coordinate system w; f y is the component of the sole force of the left foot sole 5 or the right foot sole 8 in the y direction in the world coordinate system w; f z is the component of the sole force of the left foot sole 5 or the right foot sole 8 in the z direction in the world coordinate system w; f min = 0; f max = 1.5mg; μ is the friction coefficient of the ground; the sole force of the left foot sole 5 or the right foot sole 8 is obtained through the force sensor installed therein.

[0114] (5) After the state is switched to force control, by performing PD control on the ankles of the humanoid robot and performing linear quadratic regulation on the difference in the sole forces of the two feet of the humanoid robot, until the pitch angle, roll angle, and yaw angle of the upper body of the humanoid robot are all 0, the realization of the squatting state of the humanoid robot is completed;

[0115] The specific content of step (5) is as follows:

[0116] After the state is switched to force control, torques τ ankle ;

[0117] The torque τ ankle is calculated through the PD control algorithm:

[0118]

[0119] Among them, θ pitch is the pitch angle of the actual torso 1; is the desired pitch angle of the torso 1; is the change rate of the pitch angle of the actual torso 1; is the change rate of the desired pitch angle of the torso 1, θ pitch and are obtained through the inertial measurement unit in the torso 1;

[0120] As Figure 3 shown, based on the dynamics model, linear quadratic regulation is performed on the difference between the sole force of the left foot sole 5 and the sole force of the right foot sole 8, and the dynamics model is:

[0121]

[0122] Among them, x′ is the state variable; is the first derivative of the state variable; y θ roll is the angle of the roll angle of the torso 1 in the local coordinate system y, is the angular velocity of the roll angle of the torso 1 in the local coordinate system y, w θ yaw is the angle of the yaw angle of the torso 1 in the world coordinate system w, is the angular velocity of the yaw angle of the torso 1 in the world coordinate system w; A is the system matrix, B is the control matrix, l is the lateral length of the torso 1, I x is the inertia of the torso 1 in the x direction in the local coordinate system y, I z the inertia of the torso 1 in the z direction in the local coordinate system y; ΔF z is the difference in the z direction between the sole force of the left foot sole 5 and the sole force of the right foot sole 8 in the world coordinate system w; ΔF x is the difference in the x direction between the sole force of the left foot sole 5 and the sole force of the right foot sole 8 in the world coordinate system w;

[0123] And the state space equation of the error is obtained:

[0124]

[0125] Among them, is the derivative of the reference state; is the state error, x r is the reference state; is the input error, u r is the reference input;

[0126] Design a linear quadratic regulator to calculate the feedback rate K and define the cost function I cost as:

[0127]

[0128] where Q and R are positive definite matrices respectively;

[0129] Let the cost function I cost be minimized. At this time, K satisfies: K = -R -1 B T P, where P is a constant positive definite matrix and satisfies the algebraic Riccati equation: A T P + PA - PBR -1 B T P + Q = 0;

[0130] Solve the algebraic Riccati equation to obtain the feedback rate K;

[0131] Obtain the final control quantity:

[0132] u = u r + K(x′ - x r );

[0133] Until θ pitch , y θ roll , w θ yaw are all 0, the humanoid robot reaches the squatting state.

[0134] (6) Subsequently, it is converted from the squatting state to the final standing state. During the process, virtual constraints are imposed and the standing up is completed through position control. Finally, the humanoid robot can stand stably.

[0135] The specific steps of step (6) are as follows:

[0136] Subsequently, it is converted from the squatting state to the final standing state; during the process, the angle θ5 between the left calf link 4 and the left ankle joint 22 and the angle θ6 between the right calf link 7 and the right ankle joint 19 gradually increase from the angles in the squatting state until the humanoid robot can stand stably;

[0137] As Figure 4 (a) shows, the angle θ7 between the left thigh link 3 and the horizontal line of the torso 1 and the angle θ3 between the left thigh link 3 and the left calf link 4 satisfy the following virtual constraints:

[0138] 0 = L1cos(θ5) + L2cos(θ5 + π - θ3);

[0139] 0 = θ5 - θ3 + θ7;

[0140] Wherein, L1 is the length of the left lower leg link 4; L2 is the length of the left upper leg link 3;

[0141] According to the above formula, the expressions of θ3 and θ7 in terms of θ5 can be written:

[0142]

[0143]

[0144] As Figure 4 (b) shows that the angle θ8 between the right upper leg link 6 and the horizontal line of the torso 1 and the angle θ4 between the right upper leg link 6 and the right lower leg link 7 satisfy the following virtual constraint:

[0145] 0 = L3cos(θ6) + L4cos(θ6 + π - θ4);

[0146] 0 = θ6 - θ4 + θ8;

[0147] Wherein, L3 is the length of the right lower leg link 7; L4 is the length of the right upper leg link 6;

[0148] According to the above formula, the expressions of θ4 and θ8 in terms of θ6 can be written:

[0149]

[0150]

[0151] Finally, the humanoid robot can stand stably.

[0152] As Figure 5 shown, the first one from left to right is the initial prone posture of the humanoid robot, the second one is the prone transition state of the humanoid robot, the third one is the moment when the upper body of the humanoid robot leaves the ground, the fourth one is the squatting state of the humanoid robot, and the fifth one is the final standing state of the humanoid robot.

[0153] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A control method for a humanoid robot to get up from the ground based on hybrid force-position control, characterized in that, Including the following steps: (1) Place the humanoid robot in the initial prone posture, and then send a getting-up motion instruction to the humanoid robot through a computer; (2) After the humanoid robot receives the getting-up motion instruction, it starts to move, and the angles of each joint reach the corresponding angles calculated by inverse kinematics. At this time, the humanoid robot is in the prone transition state; (3) After the humanoid robot is in the prone transition state, apply torques to each joint of the upper body of the humanoid robot, and then lift the upper body of the humanoid robot off the ground; (4) When the sole force of the humanoid robot enters the friction cone, switch the state of the lower body of the humanoid robot from position control to force control; (5) After the state is switched to force control, perform PD control on the ankles of the humanoid robot and perform linear quadratic regulation on the difference in the sole forces of the two feet of the humanoid robot until the pitch angle, roll angle, and yaw angle of the upper body of the humanoid robot are all 0, and the realization of the squatting state of the humanoid robot is completed; (6) Then, transition from the squatting state to the final standing state. During the process, apply virtual constraints and complete the standing up through position control. Finally, the humanoid robot can stand stably.

2. The humanoid robot falling and getting up control method based on force-position hybrid control according to claim 1, wherein, The humanoid robot consists of a torso (1), lower limbs and upper limbs; the center of mass of the torso (1) is the torso center of mass (25); the lower limbs include hip joints (2), a left leg and a right leg, the left leg is connected to the torso (1) through the hip joint (2), and the right leg is connected to the torso (1) through the hip joint (2); the left leg includes a left thigh link (3), a left calf link (4) and a left foot sole (5), the left foot sole (5) is connected to the left calf link (4) through a left ankle joint (22), the left calf link (4) is connected to the left thigh link (3) through a left knee joint (21), and the left thigh link (3) is connected to the hip joint (2) through a left hip joint (20); the right leg includes a right thigh link (6), a right calf link (7) and a right foot sole (8), the right foot sole (8) is connected to the right calf link (7) through a right ankle joint (19), the right calf link (7) is connected to the right thigh link (6) through a right knee joint (18), and the right thigh link (6) is connected to the hip joint (2) through a right hip joint (17); the upper limbs include a left arm and a right arm; the left arm includes a left upper arm link (9), a left lower arm link (10) and a left hand (24), the left hand (24) is connected to the left lower arm link (10), the left lower arm link (10) is connected to the left upper arm link (9) through a left elbow joint (16), and the left upper arm link (9) is connected to the torso (1) through a left shoulder joint (15); the right arm includes a right upper arm link (11), a right lower arm link (12) and a right hand (23), the right hand (23) is connected to the right lower arm link (12), the right lower arm link (12) is connected to the right upper arm link (11) through a right elbow joint (14), and the right upper arm link (11) is connected to the torso (1) through a right shoulder joint (13); an inertial measurement unit is installed in the torso (1); force sensors are respectively installed on the left foot sole (5) and the right foot sole (8); grating encoders are respectively installed on the left shoulder joint (15), the right shoulder joint (13), the left elbow joint (16), the right elbow joint (14), the left hip joint (20), the right hip joint (17), the left knee joint (21), the right knee joint (18), the left ankle joint (22) and the right ankle joint (19).

3. A method for controlling a humanoid robot to get up from the ground based on force-position hybrid control according to claim 2, characterized in that, The specific content of step (1) is as follows: Before starting to get up from the ground, the humanoid robot is made to lie prone on the ground in an initial prone posture through position control, and then a command for the getting-up action is sent to the humanoid robot by a computer; the initial prone posture is as follows: the torso (1) is parallel to the ground, the left upper arm link (9) and the right upper arm link (11) are both parallel to the torso (1), the angle θ1 between the left upper arm link (9) and the left lower arm link (10) and the angle θ2 between the right upper arm link (11) and the right lower arm link (12) are the same and less than 90°, the right hand (23) and the left hand (24) both touch the ground, the angle θ3 between the left thigh link (3) and the left calf link (4) and the angle θ4 between the right thigh link (6) and the right calf link (7) are the same and less than 180°, and the left knee joint (21) and the right knee joint (18) both touch the ground.

4. A control method for a humanoid robot to get up from the ground based on force-position hybrid control according to claim 1, characterized in that, The specific content of step (2) is as follows: After receiving the command to get up, the humanoid robot starts to move, and the angles of each joint reach the corresponding angles θ ∈ R calculated using inverse kinematics m , where m is the number of joints in the humanoid robot; The angle θ ∈ R m is calculated by the following formula: where n is the number of links in the humanoid robot; r i is the centroid position of any link; M i is the mass of any link; r com is the centroid position of the humanoid robot; IK(·) is the inverse kinematics function.

5. A control method for a humanoid robot to get up from the ground based on force-position hybrid control according to claim 2, characterized in that, The specific content of step (3) is as follows: After the humanoid robot is in the prone transition state, torques τ1, τ2, τ3, and τ5 are respectively applied to the left shoulder joint (15), the right shoulder joint (13), the left elbow joint (16), and the right elbow joint (14) to simultaneously increase the angle θ1 between the left upper arm link (9) and the left lower arm link (10) and the angle θ2 between the right upper arm link (11) and the right lower arm link (12) until θ1 = θ2 = 180°, and then the right hand (23) and the left hand (24) are simultaneously lifted off the ground; during the above process, the angle θ3 between the left thigh link (3) and the left calf link (4) and the angle θ4 between the right thigh link (6) and the right calf link (7) remain unchanged; The torques τ1, τ2, τ3, and τ5 are obtained through the following formula: where, θ up is the set of joint angles of the left shoulder joint (15), the right shoulder joint (13), the left elbow joint (16) and the right elbow joint (14), where the angles are measured in real time by the grating encoders in each joint; m is the mass of the torso (1); J is the Jacobian matrix; FK(·) is the forward kinematics function; F is the force applied to the upper body of the humanoid robot; H is the height of the torso center of mass (25) in the squatting vertical state; h is the height of the torso center of mass (25) at the moment when the right hand (23) and the left hand (24) are disengaged from the ground during the climbing process.

6. A control method for a humanoid robot to get up from the ground based on force-position hybrid control according to claim 2, characterized in that The specific content of step (4) is as follows: After the right hand (23) and the left hand (24) are simultaneously lifted off the ground, when the sole forces of the left foot sole (5) and the right foot sole (8) simultaneously enter the friction cone, the left leg and the right leg simultaneously switch from the position control state to the force control state, and the friction cone needs to meet the following conditions: f min ≤ f z ≤ f max ; - μF z ≤ ±F x ≤ μF z ; -μf z ≤ ±f y ≤ μf z ; Among them, f x is the x - direction component of the sole force of the left foot sole (5) or the right foot sole (8) in the world coordinate system w; f y is the y - direction component of the sole force of the left foot sole (5) or the right foot sole (8) in the world coordinate system w; f z is the z - direction component of the sole force of the left foot sole (5) or the right foot sole (8) in the world coordinate system w; f min = 0; f max = 1.5mg; μ is the friction coefficient of the ground; the sole force of the left foot sole (5) or the right foot sole (8) is obtained through the force sensor installed therein.

7. A method for controlling a humanoid robot to get up from the ground based on force-position hybrid control according to claim 2, characterized in that, The specific content of step (5) is as follows: After the state is switched to force control, torques τ are respectively applied to the left ankle joint (22) and the right ankle joint (19). ankle ; The torque τ ankle is calculated by a PD control algorithm: where θ pitch is the pitch angle of the actual torso (1); is the desired pitch angle of the torso (1); is the change rate of the pitch angle of the actual torso (1); is the change rate of the desired pitch angle of the torso (1), θ pitch and are obtained by the inertial measurement unit in the torso (1). Based on the dynamic model, linear quadratic regulation is performed on the difference between the sole force of the left foot sole (5) and the sole force of the right foot sole (8), and the dynamic model is as follows: where x′ is a state variable; is the first derivative of the state variable; y θ roll is the angle of the roll angle of the torso (1) in the local coordinate system y, is the angular velocity of the roll angle of the torso (1) in the local coordinate system y, w θ yaw is the angle of the yaw angle of the torso (1) in the world coordinate system w, is the angular velocity of the yaw angle of the torso (1) in the world coordinate system w; A is the system matrix, B is the control matrix, l is the lateral length of the torso (1), I x is the inertia of the torso (1) in the x direction in the local coordinate system y, I z is the inertia of the torso (1) in the z direction in the local coordinate system y; ΔF z is the difference in the z direction between the sole force of the left foot sole (5) and the sole force of the right foot sole (8) in the world coordinate system w; ΔF x is the difference in the x direction between the sole force of the left foot sole (5) and the sole force of the right foot sole (8) in the world coordinate system w; And the state space equation of the error is obtained: wherein, is the derivative of the reference state; is the state error, x r is the reference state; is the input error, u r is the reference input; Design a linear quadratic regulator to calculate the feedback rate K and define the cost function I cost as follows: Where Q and R are respectively positive definite matrices; Let the cost function I cost be minimized, at this time K satisfies: K = -R -1 B T P, where P is a constant positive definite matrix and satisfies the algebraic Riccati equation: A T P + PA - PBR -1 B T P + Q = 0; Solve the algebraic Riccati equation to obtain the feedback rate K; Obtain the final control quantity: u = u r + K(x′ - x r ); Until θ pitch 、 y θ roll 、 w θ yaw When all of them are 0, the humanoid robot reaches the squatting state.

8. A humanoid robot's falling and getting up control method based on force-position hybrid control according to claim 2, characterized in that, The specific content of step (6) is as follows: Subsequently, it is converted from the squatting state to the final standing state; during the process, the angle θ5 between the left calf link (4) and the left ankle joint (22) and the angle θ6 between the right calf link (7) and the right ankle joint (19) gradually increase from the angles in the squatting state until the humanoid robot can stand stably; The angle θ7 between the left thigh link (3) and the horizontal line of the torso (1) and the angle θ3 between the left thigh link (3) and the left calf link (4) satisfy the following virtual constraints: 0 = L1cos(θ5)+L2cos(θ5 + θ3); π = θ5 + θ3 + θ7; Where L1 is the length of the left calf link (4); L2 is the length of the left thigh link (3); The angle θ8 between the right thigh link (6) and the horizontal line of the torso (1) and the angle θ4 between the right thigh link (6) and the right calf link (7) satisfy the following virtual constraints: 0 = L3cos(θ6) + L4cos(θ6 + θ4); π = θ6 + θ4 + θ8; where L3 is the length of the right calf link (7); L4 is the length of the right thigh link (6); Finally, the humanoid robot can stand stably.

Images