Motion control method and device, robot and storage medium

Abstract

The invention relates to a motion control method and device, a robot and a storage medium, and the method comprises the steps: controlling the robot to move according to a reference state in a reference track and a feedback state of the robot when the robot is in a take-off stage; and when the robot is in a landing stage, tracking a state variable error and a control input error in the remaining time of the landing stage under constraint conditions to obtain a state variable optimization result and a control input optimization result, and controlling the robot to move according to the state variable optimization result and the control input optimization result, the state variable error comprises an error between a specified state and a state variable optimization result, the control input error comprises an error between specified control input and a control input optimization result, and the constraint condition comprises at least one of a centroid dynamics constraint, an all-kinematics constraint and a collision dynamics constraint.

Description

Technical Field

[0001] The present disclosure relates to the field of robot technology, and in particular to a motion control method, device, robot and storage medium. Background Art

[0002] In recent years, the technology of robotics has been developing continuously, becoming more and more intelligent and automated, and the richness, stability and flexibility of actions have been improved to varying degrees. Robots can replace users in production and life to perform specific tasks, thus bringing convenience to users. In related technologies, when robots perform some movements that include the air state, the robot's movement stability is often affected by the collision impact at the moment of landing. Summary of the invention

[0003] In order to overcome the problems existing in the related art, the embodiments of the present disclosure provide a motion control method, device, robot and storage medium to solve the defects in the related art.

[0004] According to a first aspect of an embodiment of the present disclosure, a motion control method is provided, the method comprising:

[0005] When the robot is in the take-off stage, the robot is controlled to move according to the reference state in the reference trajectory and the feedback state of the robot;

[0006] When the robot is in the landing phase, the state variable error and the control input error in the remaining time of the landing phase are tracked under constraints to obtain the state variable optimization results and the control input optimization results, and the robot is controlled to move according to the state variable optimization results and the control input optimization results, wherein the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint conditions include at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint.

[0007] In one embodiment of the present disclosure, the method further includes:

[0008] When the robot is in the take-off phase, obtain the vertical velocity of the robot's contact point;

[0009] In response to the speed of the contact point of the robot in the vertical direction being greater than a preset threshold, the control coefficient of the take-off phase is reduced.

[0010] In one embodiment of the present disclosure, the control coefficient of the shrinking take-off phase includes:

[0011] When the robot is at the end of the take-off phase, the control coefficient is reduced according to a first ratio, wherein the end of the take-off phase includes a second ratio of the last duration of the take-off phase, and the duration of the take-off phase is determined according to a reference trajectory.

[0012] In one embodiment of the present disclosure, the method further includes:

[0013] Determine a state variable at at least one moment in the landing phase according to a reference state at the landing moment in the reference trajectory and a state at the stable standing moment specified by a user; and / or,

[0014] The control input at at least one moment in the landing phase is determined according to the reference control input at the landing moment in the reference trajectory and the control input at the stable standing moment specified by the user.

[0015] In one embodiment of the present disclosure, the state variable includes a momentum vector and a position vector, wherein the momentum vector includes a linear momentum and a nonlinear momentum, and the position vector includes a floating base position and a joint position; and / or,

[0016] The control input includes a contact point vector and a joint velocity vector, wherein the contact point vector includes an external force applied to a contact point of the robot, and the joint velocity vector includes a velocity of a joint.

[0017] In one embodiment of the present disclosure, the constraint condition includes at least one of the following:

[0018] The momentum vector, position vector and velocity vector of the robot conform to the dynamic model obtained according to the center of mass dynamics and the whole kinematics, wherein the velocity vector includes the floating basis velocity vector and the joint velocity vector, and the floating basis velocity vector includes the velocity of the floating basis;

[0019] The tangential friction force on the contact point of the robot with the ground during the landing phase is less than the maximum static friction force;

[0020] The friction force on the contact point of the robot with the ground during the landing phase is less than the maximum friction force;

[0021] The robot's contact point with the ground is stationary in the landing phase position;

[0022] The contact force on the robot's contact points that are not in contact with the ground during the landing phase is 0;

[0023] The vertical velocity of the robot's contact point that is not in contact with the ground during the landing phase satisfies the specified velocity trajectory.

[0024] In one embodiment of the present disclosure, the method further includes:

[0025] In response to the robot ending the take-off phase, determining the first control parameter of the robot according to the reference state in the reference trajectory and the feedback state of the robot, and tracking the state variable error and the control input error within a preset time length under constraint conditions to obtain the state variable optimization result and the control input optimization result, and determining the second control parameter of the robot according to the state variable optimization result and the control input optimization result;

[0026] The robot is controlled to move according to the first control parameter and the second control parameter.

[0027] In one embodiment of the present disclosure, the method further includes:

[0028] In response to the time length of the robot ending the take-off phase reaching the preset buffer time length, it is determined that the robot enters the landing phase.

[0029] In one embodiment of the present disclosure, the method further includes:

[0030] When the robot is in a take-off phase, a contact state of a contact point of the robot is acquired, and in response to the contact point being in contact with the ground, it is determined that the robot ends the take-off phase.

[0031] According to a second aspect of an embodiment of the present disclosure, a motion control device is provided, the device comprising:

[0032] A take-off module is used to control the robot to move according to the reference state in the reference trajectory and the feedback state of the robot when the robot is in the take-off stage;

[0033] A landing module is used to track the state variable error and control input error in the remaining time of the landing phase under constraints when the robot is in the landing phase, obtain the state variable optimization result and the control input optimization result, and control the robot to move according to the state variable optimization result and the control input optimization result, wherein the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint condition includes at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint.

[0034] In one embodiment of the present disclosure, the device further includes an instruction softening module, which is used to:

[0035] When the robot is in the take-off phase, obtain the vertical velocity of the robot's contact point;

[0036] In response to the speed of the contact point of the robot in the vertical direction being greater than a preset threshold, the control coefficient of the take-off phase is reduced.

[0037] In one embodiment of the present disclosure, when the instruction softening module is used to reduce the control coefficient of the take-off phase, it is used to:

[0038] When the robot is at the end of the take-off phase, the control coefficient is reduced according to a first ratio, wherein the end of the take-off phase includes a second ratio of the last duration of the take-off phase, and the duration of the take-off phase is determined according to a reference trajectory.

[0039] In one embodiment of the present disclosure, the device further includes an interpolation module, configured to:

[0040] Determine a state variable at at least one moment in the landing phase according to a reference state at the landing moment in the reference trajectory and a state at the stable standing moment specified by a user; and / or,

[0041] The control input at at least one moment in the landing phase is determined according to the reference control input at the landing moment in the reference trajectory and the control input at the stable standing moment specified by the user.

[0042] In one embodiment of the present disclosure, the state variable includes a momentum vector and a position vector, wherein the momentum vector includes a linear momentum and a nonlinear momentum, and the position vector includes a floating base position and a joint position; and / or,

[0043] The control input includes a contact point vector and a joint velocity vector, wherein the contact point vector includes an external force applied to a contact point of the robot, and the joint velocity vector includes a velocity of a joint.

[0044] In one embodiment of the present disclosure, the constraint condition includes at least one of the following:

[0045] The momentum vector, position vector and velocity vector of the robot conform to the dynamic model obtained according to the center of mass dynamics and the whole kinematics, wherein the velocity vector includes the floating basis velocity vector and the joint velocity vector, and the floating basis velocity vector includes the velocity of the floating basis;

[0046] The tangential friction force on the contact point of the robot with the ground during the landing phase is less than the maximum static friction force;

[0047] The friction force on the contact point of the robot with the ground during the landing phase is less than the maximum friction force;

[0048] The robot's contact point with the ground is stationary in the landing phase position;

[0049] The contact force on the robot's contact points that are not in contact with the ground during the landing phase is 0;

[0050] The vertical velocity of the robot's contact point that is not in contact with the ground during the landing phase satisfies the specified velocity trajectory.

[0051] In one embodiment of the present disclosure, the device further includes a buffer module, which is used to:

[0052] In response to the robot ending the take-off phase, determining the first control parameter of the robot according to the reference state in the reference trajectory and the feedback state of the robot, and tracking the state variable error and the control input error within a preset time length under constraint conditions to obtain the state variable optimization result and the control input optimization result, and determining the second control parameter of the robot according to the state variable optimization result and the control input optimization result;

[0053] The robot is controlled to move according to the first control parameter and the second control parameter.

[0054] In one embodiment of the present disclosure, the device further includes a first switching module, configured to:

[0055] In response to the time length of the robot ending the take-off phase reaching the preset buffer time length, it is determined that the robot enters the landing phase.

[0056] In one embodiment of the present disclosure, the device further includes a second switching module, configured to:

[0057] When the robot is in a take-off phase, a contact state of a contact point of the robot is acquired, and in response to the contact point being in contact with the ground, it is determined that the robot ends the take-off phase.

[0058] According to a third aspect of an embodiment of the present disclosure, a robot is provided, comprising a memory and a processor, wherein the memory is used to store computer instructions executable on the processor, and the processor is used to implement the motion control method described in the first aspect when executing the computer instructions.

[0059] According to a fourth aspect of an embodiment of the present disclosure, a computer-readable storage medium is provided, on which a computer program is stored, and when the program is executed by a processor, the method described in the first aspect is implemented.

[0060] The technical solution provided by the embodiments of the present disclosure may have the following beneficial effects:

[0061] The motion control method provided by the embodiment of the present disclosure controls the robot to move according to the reference state in the reference trajectory and the feedback state of the robot when the robot is in the take-off stage; and when the robot is in the landing stage, the state variable error and the control input error in the remaining time of the landing stage are tracked under the constraint condition to obtain the state variable optimization result and the control input optimization result, and the robot is controlled to move according to the state variable optimization result and the control input optimization result. Since the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint condition includes at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint, the robot adopts different control methods for motion control at different stages, especially continuously optimizes the state variables and the control input in the landing stage, so as to reach the desired state after the collision and improve the system robustness during the landing process, which enables the robot to slow down the collision impact after landing and improve the motion stability of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0063] Figure 1 is a schematic structural diagram of a biped robot shown in an exemplary embodiment of the present disclosure;

[0064] Figure 2 is a flow chart of a motion control method shown in an exemplary embodiment of the present disclosure;

[0065] Figure 3 is a flow chart of a motion control method shown in an exemplary embodiment of the present disclosure;

[0066] Figure 4 is a structural schematic diagram of a motion control device shown in an exemplary embodiment of the present disclosure;

[0067] Figure 5 It is a structural block diagram of a robot shown in an exemplary embodiment of the present disclosure. DETAILED DESCRIPTION

[0068] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present disclosure. Instead, they are merely examples of devices and methods consistent with some aspects of the present disclosure as detailed in the appended claims.

[0069] The terms used in this disclosure are for the purpose of describing specific embodiments only and are not intended to limit the disclosure. The singular forms of "a", "said" and "the" used in this disclosure and the appended claims are also intended to include plural forms unless the context clearly indicates otherwise. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0070] It should be understood that although the terms first, second, third, etc. may be used in the present disclosure to describe various information, such information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of the present disclosure, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0071] In recent years, the technology of robotics has been developing continuously, becoming more and more intelligent and automated, and the richness, stability and flexibility of actions have been improved to varying degrees. Robots can replace users in production and life to perform specific tasks, thus bringing convenience to users. In related technologies, when robots perform some movements that include the air state, the robot's movement stability is often affected by the collision impact at the moment of landing.

[0072] For example, when a robot performs high-dynamic motion, it can do so by tracking its reference trajectory. Currently, for continuous dynamic systems, the reference trajectory can well reflect the actual dynamics of the robot. However, for high-dynamic motion, when the robot is completely airborne, the robot is in an under-driven passive motion process. The impact of the accumulated errors before taking off will erupt during the airborne phase, causing the actual state variables of the robot before landing to seriously deviate from the state variables within the reference trajectory, which makes the robot's controller easily diverge after landing and collides, resulting in a large collision impact that affects the robot's motion stability.

[0073] Based on this, at least one embodiment of the present disclosure provides a motion control method, which can be applied to robots, for example, to bipedal robots (humanoid robots) and quadrupedal robots (robot dogs). Figure 1, which takes a biped robot as an example to illustrate the freedom structure of the robot. The biped robot includes two upper limbs and two lower limbs. The shoulder of the upper limb has three degrees of freedom in the directions of pitch, roll, and yaw, the elbow has one degree of freedom in the direction of pitch, and the wrist can also have two or three degrees of freedom. The hip of the lower limb has three degrees of freedom in the directions of pitch, roll, and yaw, the knee has one degree of freedom in the direction of pitch, and the ankle has two degrees of freedom in the directions of pitch and roll, wherein roll, pitch, and yaw are the directions of rotation around the X-axis, Y-axis, and Z-axis, respectively. There are two flat feet at the end of the lower limbs. By changing the contact state variables between the flat feet and the outside world, a variety of dynamic behaviors can be realized, such as walking, running, jumping, somersaults, stable operation, etc., wherein the flat feet (i.e., the robot) can have multiple contact points with the ground, such as at least two contact points of toe and heel.

[0074] The method can be used to control the robot to perform high-dynamic motion, and the high-dynamic motion can refer to a motion that includes a completely vacant state. For example, the method can use different control methods to control the robot motion at different stages of the high-dynamic motion, for example, the high-dynamic motion can include a take-off stage and a landing stage, wherein the take-off stage is the stage when the robot enters the vacant state from the non-vacant state until the vacant state ends (i.e., lands), and the landing stage is the stage when the robot lands until it reaches a stable standing posture specified by the user.

[0075] This application first establishes the continuous center of mass dynamics equation of the landing phase, and then constructs the complete control process of the landing phase in the method based on the continuous center of mass dynamics equation. Therefore, before introducing the specific process of the method, the continuous center of mass dynamics equation is first introduced in detail.

[0076] Assume the generalized coordinates of the robot system are: n a is the number of drivable joints of the robot; where, is the position of the floating base B in the coordinate system I (i.e., the world coordinate system), r IB is the position, expressed as x, y, z coordinates, is the attitude angle, expressed as zyx-Euler angle; where q j is the joint position vector, including the position of each joint of the robot (i.e., the joint angle).

[0077] First, the nominal joint configuration q is calculated norm The robot moment of inertia matrix I norm and the relative displacement from the center of mass to the floating base For example, the nominal joint configuration can be the joint configuration corresponding to the robot standing. Then, the nominal single rigid body is used to represent the entire robot, and the relationship between the momentum of the corresponding single rigid body and the floating basis state is as follows:

[0078]

[0079] In the above formula, B b represents the inertia matrix of a single rigid body, h = [h lin ,h ang ] T ∈R 6 It is the mass center momentum relative to the mass center coordinate system G, where the origin of G is located at the mass center and the coordinate axis direction is the same as that of the inertial coordinate system I. 3×3 is the 3rd-order identity matrix, R IB represents the rotation matrix corresponding to the zyx-Euler angle, E R,Euler Represents the matrix that maps the derivative of the zyx-Euler angle to the global angular velocity:

[0080]

[0081] Again, the derivative of the momentum h of a single rigid body is determined by the external force on the single rigid body:

[0082]

[0083] In the above formula, is the position of contact point i relative to the center of mass, and and They are the contact force and torque on contact point i respectively.

[0084] So, let the state variables and notification inputs of the robot system be as follows:

[0085]

[0086] v j Represents the joint velocity vector.

[0087] Based on the above analysis, as well as the state variables and control inputs of the system, the continuous dynamics model kino-dynamics of the robot during the landing phase can be obtained as follows:

[0088]

[0089] Please refer to the attached Figure 2 , which shows the process of the method, including steps S201 to S203.

[0090] In step S201, when the robot is in the take-off stage, the robot is controlled to move according to the reference state in the reference trajectory and the feedback state of the robot.

[0091] As mentioned above, the take-off phase may be the phase from when the robot enters the airborne state from the non-airborne state until the airborne state ends (ie, lands).

[0092] The reference trajectory can be a reference trajectory of high dynamic motion. The robot has obtained the reference trajectory before executing the method. The reference trajectory can be obtained by full model offline trajectory optimization, simplified model planning, human motion capture and collection processing, etc. The reference trajectory at least includes a reference curve X of the generalized state X changing with time. ref (t), where the generalized state X is as follows:

[0093]

[0094] In the above formula, q represents the position vector, q b =[q x ,q y ,q z ,q yaw ,q pitch ,q roll ] T is the position of the floating basis in the world coordinate system, q leg Represents the joint positions of the left and right lower limbs, q arm Represents the joint positions of the left and right upper limbs; represents the velocity vector, is the velocity of the floating basis in the world coordinate system, Represents the joint speed of the left and right lower limbs, represents the joint velocities of the left and right upper limbs; represents the acceleration vector, is the acceleration of the floating basis in the world coordinate system, represents the joint acceleration of the left and right lower limbs, Represents the joint acceleration of the left and right upper limbs.

[0095] Understandable, q j =[q leg ,q ram ] T Represents the controllable joints of the legs and upper limbs, which have four legs in the case of a quadruped robot.

[0096] It is understandable that the robot can be controlled at a certain frequency, that is, the method can be executed at a certain frequency. The moment when the method executes control each time can be called a control frame. The frequency of the control frame and the frequency of the reference moment in the reference trajectory are not necessarily the same and synchronized. Therefore, the generalized state of at least one moment in the reference trajectory other than the multiple reference moments (these moments may include all control frames in the remaining time of the take-off phase) can also be determined based on the generalized state X of the multiple reference moments in the reference trajectory.

[0097] For example, the generalized state of at least one moment other than the multiple reference moments in the reference trajectory can be obtained by interpolation. Specifically, for a certain moment t, the reference moments t1 and t2 on both sides of moment t can be found in the reference trajectory, and the generalized state at moment t can be obtained by the following difference method:

[0098]

[0099] Exemplarily, a take-off controller may be constructed to perform motion control in the take-off phase, and the take-off controller may include a real-time trajectory generator, a state estimation module, and a PVT-PID joint controller.

[0100] In order to achieve closed-loop control, the state estimation module can use the inertial measurement unit (IMU) installed on the floating base and the encoders at each joint in each control cycle (i.e., the duration between adjacent control frames) to obtain the actual generalized state X of the robot at the current moment through an appropriate state estimation algorithm. act (t), namely the feedback status; and the feedback status is reported to the real-time trajectory generator.

[0101] The real-time trajectory generator can determine the reference state X of each control frame according to the above interpolation method. des (t), and the reference state X des (t) and feedback state X act (t) and the corresponding joint PID coefficients are sent to the joint controller, which performs the following torque control:

[0102]

[0103] In the above formula, q j =[q leg ,q arm ] T , The reference state X des The joint position vectors in (t) contain the reference position of each joint, The reference state Xdes The joint velocity vector in (t) contains the reference velocity of each joint, q j is the feedback status X act The joint position vector in (t) contains the actual position of each joint, is the feedback status X act The joint velocity vector in (t) contains the actual velocity of each joint, k p , k d , k i They are proportional coefficient, differential coefficient and integral coefficient respectively.

[0104] Joint torque command τ j The commands sent to the joint actuators are executed to control the robot motion.

[0105] Optionally, the contact state of the robot's contact point can be obtained in each control frame when the robot is in the take-off phase; and in response to the contact point contacting the ground, it is determined that the robot ends the take-off phase (and enters the phase after the take-off phase, such as the landing phase).

[0106] The contact state may include the external force on the contact point, and the external force on the foot end can be calculated based on the external force received by each contact point on the foot end. If the absolute value of the linear component of the resultant force on the foot end in the previous frame is less than the threshold: ‖f(t - )‖ <f thre , and the absolute value of the linear component of the resultant external force on the foot end in the current frame is not less than or equal to the threshold ‖f(t)‖≥f thre , it can be determined that the robot is in contact with the ground; if the absolute value of the linear component of the resultant force of the external force on the foot end in the previous frame is less than the threshold: ‖f(t - )‖ <f thre , and the absolute value of the linear component of the resultant external force on the foot end in the current frame is less than or equal to the threshold ‖f(t)‖<f thre , it can be determined that the robot is in contact with the ground.

[0107] In step S202, when the robot is in the landing phase, the state variable error and the control input error in the remaining time of the landing phase are tracked under constraints to obtain the state variable optimization result and the control input optimization result, and the robot is controlled to move according to the state variable optimization result and the control input optimization result.

[0108] The state variable error includes the error between the specified state and the state variable optimization result, and the control input error includes the error between the specified control input and the control input optimization result. Since the landing phase is the time from the robot landing to reaching the stable standing posture specified by the user, the specified state and the specified control input in the landing phase can be determined as follows:

[0109] According to the reference state at the landing moment in the reference trajectory and the state at the stable standing moment specified by the user, the state variable of at least one moment in the landing phase is determined, for example, by interpolation.

[0110] The control input at at least one moment in the landing phase is determined according to the reference control input at the landing moment in the reference trajectory and the control input at the stable standing moment specified by the user, for example, by interpolation.

[0111] The reference trajectory may include state variables and control inputs at the time of landing. As described above, the state variable x includes a momentum vector h and a position vector q. The momentum vector h includes a linear momentum h. lin and nonlinear momentum h ang , the position vector q includes the floating base position q b and the position q of (each) joint j ; The control input u includes a contact point vector and a joint velocity vector. The contact point vector includes the external force applied to (each) contact point of the robot, and the joint velocity vector includes the velocity of (each) joint of the robot.

[0112] The reference trajectory can be a reference trajectory of high dynamic motion. The robot has obtained the reference trajectory before executing the method. The reference trajectory can be obtained by full model offline trajectory optimization, simplified model planning, human motion capture and collection processing, etc. The reference trajectory at least includes a reference curve X of the generalized state X changing with time. ref (t), where the generalized state X is as follows:

[0113]

[0114] In the above formula, q represents the position vector, q b =[q x ,q y ,q z ,q yaw ,q pitch ,q roll ] T is the position of the floating basis in the world coordinate system, q leg Represents the joint positions of the left and right lower limbs, qarm Represents the joint positions of the left and right upper limbs; represents the velocity vector, is the velocity of the floating basis in the world coordinate system, Represents the joint speed of the left and right lower limbs, represents the joint velocities of the left and right upper limbs; represents the acceleration vector, is the acceleration of the floating basis in the world coordinate system, represents the joint acceleration of the left and right lower limbs, represents the joint acceleration of the left and right upper limbs. It can be understood that q j =[q leg ,q arm ] T Represents the controllable joints of the legs and upper limbs. For a quadruped robot, there are four legs. It can be seen that the generalized state contains state variables and control inputs.

[0115] The user can specify the generalized state X at the moment of stable standing. In this step, the state variables and control inputs at that moment can be obtained from the generalized state X.

[0116] It is understandable that the robot can be controlled at a certain frequency, that is, the method can be executed at a certain frequency. The moment when the method executes control each time can be called a control frame. The state variable optimization result generated in each control frame in this step includes the state variable optimization result in the remaining time of the landing phase, that is, the state variable optimization result of multiple control frames in the remaining time, and the control input optimization result generated includes the control input optimization result in the remaining time of the landing phase, that is, the control input optimization result of multiple control frames in the remaining time.

[0117] The above interpolation method to obtain the state variables and control inputs at at least one time in the remaining time of the landing phase can be performed as follows: First, the generalized state X at time t is obtained by difference method. des (t); then, based on the generalized state at time t, the state variable x at time t can be obtained * (t) and control input u * (t):

[0118]

[0119] In the above formula, t TD is the landing time, t end For stable standing moment.

[0120] The constraint condition includes at least one of the following: a center of mass dynamics constraint, a full kinematics constraint, and a collision dynamics constraint. For example, the constraint condition includes at least one of the following:

[0121] The first item: The momentum vector, position vector and velocity vector of the robot conform to the dynamic model obtained based on the center of mass dynamics and full kinematics (i.e. the continuous dynamic model mentioned above), wherein the velocity vector includes the floating basis velocity vector and the joint velocity vector, and the floating basis velocity vector includes the velocity of the floating basis. That is, the state variables and control inputs conform to the following formula:

[0122]

[0123] The second item: The tangential friction force on the robot's contact point with the ground during the landing phase is less than the maximum static friction force. That is:

[0124]

[0125] In the above formula, and are the tangent and normal unit vectors of the contact surface, μ is the friction coefficient; c stores the contact status of each contact point, 1 indicates contact with the outside world, and 0 indicates no contact with the outside world.

[0126] The third item: The friction force on the robot's contact point with the ground during the landing phase is less than the maximum friction force. That is:

[0127]

[0128] This constraint can prevent the optimized joint torque from peak saturation and truncation.

[0129] Item 4: The robot's contact point with the ground is stationary during the landing phase. That is:

[0130]

[0131] In the above formula, is the velocity Jacobian matrix of the contact point.

[0132] Item 5: The contact force on the robot's contact points that are not in contact with the ground during the landing phase is 0. That is:

[0133]

[0134] Item 6: The vertical velocity of the robot's contact point that is not in contact with the ground during the landing phase satisfies the specified velocity trajectory. That is:

[0135]

[0136] The velocity trajectory can be obtained by tracking the desired normal position through proportional control.

[0137]

[0138] Among them, k p is the gain coefficient, is the specified position of the non-contact point in the vertical direction relative to the inertial coordinate system, It is the actual position of the non-contact point in the vertical direction relative to the inertial coordinate system.

[0139] Exemplarily, this step can track the state variable errors and control input errors of multiple control frames in the remaining time by using an MPC (model predictive control) optimization method to obtain state variable optimization results and control input optimization results of multiple control frames in the remaining time. For example, the following formula is used to establish an MPC optimization model:

[0140]

[0141] In the above formula, x is the state variable optimization result on multiple control frames, x * The specified state on multiple control frames, u is the control input optimization result on multiple control frames, u * Specify control inputs on multiple control frames, where W1 and W2 are the weight matrices of state variables and control inputs, respectively.

[0142] The state variable optimization results and control input optimization results obtained in this step on multiple control frames during the remaining time of the landing phase are equivalent to online replanning of the trajectory during this period so that the trajectory meets various physical constraints and expected behaviors. The state variable optimization results and control input optimization results can be calculated by inverse dynamics to obtain the joint torque corresponding to each joint.

[0143] Specifically, the full dynamics equation of the robot is:

[0144]

[0145] In the above formula, M and b represent the inertia matrix and nonlinear term (Coriolis force, centrifugal force and gravity in generalized coordinate space), respectively; τ is the torque vector, which contains the torque of each joint of the robot, S τ is the selection matrix of the drivable joints; J c is the velocity Jacobian matrix of the contact point, f c is the contact force on the contact point. In the optimization results of this step, only the joint torque τ and acceleration Unknown, acceleration It can be obtained by differential velocity, and then the inverse dynamics solution can be obtained by simple calculation of the above full dynamics equation:

[0146]

[0147] The obtained joint torque τ is used to control the movement of the robot's joints.

[0148] As another example, a landing controller can be constructed to perform motion control in the landing phase, and the landing controller can include an MPC module, a state estimation module, and a PVT-PID joint controller. The MPC module can optimize the state variables and control inputs in the manner described in the above embodiment, and perform inverse dynamics calculations based on the optimization results to obtain the joint torques corresponding to each joint, and send the joint torques of each joint to the joint control module to perform torque tracking control.

[0149] Optionally, in each control frame when the robot is in the landing phase, it is possible to determine whether the landing phase has reached a preset duration, and in response to reaching the preset duration, determine that the robot has ended the landing phase (for example, entering the phase after the landing phase, such as the take-off phase or the stationary phase, etc.).

[0150] The motion control method provided by the embodiment of the present disclosure controls the robot to move according to the reference state in the reference trajectory and the feedback state of the robot when the robot is in the take-off stage; and when the robot is in the landing stage, the state variable error and the control input error in the remaining time of the landing stage are tracked under the constraint condition to obtain the state variable optimization result and the control input optimization result, and the robot is controlled to move according to the state variable optimization result and the control input optimization result. Since the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint condition includes at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint, the robot adopts different control methods for motion control at different stages, especially continuously optimizes the state variables and the control input in the landing stage, so as to reach the desired state after the collision and improve the system robustness during the landing process, which enables the robot to slow down the collision impact after landing and improve the motion stability of the robot.

[0151] In some embodiments of the present disclosure, it is also possible to obtain the vertical speed of the robot's contact point when the robot is in the take-off stage; and in response to the vertical speed of the robot's contact point being greater than a preset threshold, reduce the control coefficient of the take-off stage (i.e., the control coefficient of the take-off controller).

[0152] Among them, the collision impact received by the robot when landing is proportional to the vertical speed of its contact point relative to the outside world, that is, the greater the vertical speed of the contact point relative to the outside world, the greater the collision impact received by the robot when landing, and the smaller the vertical speed of the contact point relative to the outside world, the smaller the collision impact received by the robot when landing. Therefore, this embodiment can estimate the collision impact of the robot when landing by judging the speed of the robot's contact point in the vertical direction. If the speed of the robot's contact point in the vertical direction is greater than the preset threshold, it can be indicated that the collision impact of the robot when landing exceeds the acceptable range, and joint softening needs to be performed in the manner provided by this embodiment.

[0153] The joint softening in this embodiment can be: when the robot is at the end of the take-off phase, the control coefficient is reduced according to the first ratio, wherein the end of the take-off phase includes the duration of the last second ratio of the take-off phase, and the duration of the take-off phase is determined according to the reference trajectory. For example, the first ratio is ρ (0≤ρ≤1), and the second ratio is 20%. In this embodiment, the control coefficient can be smoothly transitioned from the nominal value (i.e., the proportional coefficient, differential coefficient, and integral coefficient in step S201) to ρ times the nominal value in the last 20% of the take-off phase:

[0154] {k p ,k d ,k i}→{ρk p ,ρk d ,ρk i}

[0155] This embodiment can make the joint control stiffness relatively soft when the robot falls to the ground, so that it can buffer and absorb part of the impact energy, so as to effectively prevent the robot from bouncing relative to the ground and destroying the movement stability of the robot.

[0156] In some embodiments of the present disclosure, the landing phase is immediately entered after the take-off phase is completed, thereby directly switching the control mode.

[0157] Alternatively, in some embodiments of the present disclosure, a buffer phase is added during the take-off phase and the landing phase to achieve a smooth transition when switching between the two control modes, so as to avoid a jump in the control of the robot. The buffer phase can be set with a preset buffer duration.

[0158] Based on this, the robot can be controlled in the buffer phase as follows:

[0159] First, in response to the robot ending the take-off phase, a first control parameter of the robot is determined according to a reference state in a reference trajectory and a feedback state of the robot, and the state variable error and the control input error within a preset time length are tracked under constraint conditions to obtain the state variable optimization result and the control input optimization result, and a second control parameter of the robot is determined according to the state variable optimization result and the control input optimization result;

[0160] In which, in response to the robot ending the take-off phase, it enters the buffer phase.

[0161] As can be seen from the above description, determining the first control parameter of the robot according to the reference state in the reference trajectory and the feedback state of the robot refers to the control method of the take-off stage. The control method has been described in detail in the above embodiments and will not be repeated here. The first control parameter obtained is the joint torque, which is referred to as the joint torque here to distinguish it from the second control parameter.

[0162] As can be seen from the above introduction, the state variable error and the control input error within the preset time length are tracked under the constraint condition to obtain the state variable optimization result and the control input optimization result. The second control parameter of the robot is determined according to the state variable optimization result and the control input optimization result, which refers to the control method of the landing phase. The control method has been described in detail in the above embodiments and will not be repeated here. It should be pointed out that the preset time length here can be the remaining time length of the buffer phase. The second control parameter obtained is the joint torque, which is referred to here as the joint torque to distinguish it from the first control parameter.

[0163] Next, the robot is controlled to move according to the first control parameter and the second control parameter.

[0164] It is understandable that the robot can be controlled at a certain frequency, that is, the method can be executed at a certain frequency. The moment when the method executes control each time can be called a control frame. Therefore, illustratively, the weights of the first control parameter and the second control parameter can be determined according to the position of the moment of the current control frame in the buffer stage (or the ratio of the time spent in the buffer stage to the preset buffer time of the buffer stage), and then the weights of the first control parameter and the second control parameter are used to perform a weighted sum of the two to obtain the total control parameter, and the total control parameter is used to control the robot to move. That is:

[0165]

[0166] In the above formula, t is the time of the current control frame, t bis the time when the buffering phase starts, T buffer is the preset buffering time, τ j is the total control parameter, i.e. the total joint torque.

[0167] It is understandable that the control coefficient can be further softened in the buffer stage, that is, the control coefficient (i.e., the control coefficient of the take-off controller) is smoothly transitioned from the nominal value (i.e., the proportional coefficient, differential coefficient, and integral coefficient in step S201) to 0 within the preset buffer time:

[0168] {ρk p ,ρk d ,ρk i}→{0,0,0}

[0169] This can further soften the joint control stiffness after the robot falls to the ground, allowing it to buffer and absorb part of the impact energy, thereby effectively preventing the robot from bouncing relative to the ground and destroying the robot's movement stability.

[0170] It is understandable that the robot may also be determined to enter the landing phase in response to the duration of the robot ending the take-off phase reaching a preset buffer duration.

[0171] This embodiment adds a buffer stage between the take-off stage and the landing stage of the high-dynamic motion, and uses two control methods in the take-off stage and the landing stage for control in the buffer stage. For example, a buffer controller can be constructed to perform motion control in the buffer stage. The function of the buffer controller is to smoothly transfer the main components of the controller instructions from the above-mentioned take-off controller to the subsequent landing controller within a preset buffering time. The purpose is: on the one hand, to continue to buffer the collision, and on the other hand, to provide sufficient initialization time for the solution of the landing controller.

[0172] Please refer to the attached Figure 3, which shows a flow chart of the motion control method obtained by combining the above multiple embodiments. It can be seen from the figure that after the high dynamic motion starts, the default setting stage is the take-off stage; then in the motion control process (each control frame), the stage is judged, if it is in the take-off stage, the take-off controller is activated, and the offline trajectory is tracked during the action process to control the robot movement, and in the control process, it is judged whether the robot lands from the air, if it lands, the stage is set to the buffer stage, if it does not land, the joint end tracking (output instruction of the take-off controller) is controlled; if it is in the buffer stage, the buffer controller is activated (transition from the take-off controller to the landing controller), and in the control process, it is judged whether the specified buffer time (i.e., the preset buffer duration) is exceeded, if it exceeds, the stage is set to the landing stage, if it does not exceed, the joint end tracking (output instruction of the buffer controller) is controlled; if it is in the landing stage, the landing controller is activated, and in the control process, it is judged whether the specified landing time is exceeded, if it exceeds, the stage is set to the take-off stage or the static stage, if it does not exceed, the joint end tracking (output instruction of the landing controller) is controlled.

[0173] At least one embodiment of the present disclosure further provides a motion control device, Figure 4 , the device comprises:

[0174] A take-off module 401 is used to control the robot to move according to a reference state in a reference trajectory and a feedback state of the robot when the robot is in a take-off stage;

[0175] The landing module 402 is used to track the state variable error and the control input error in the remaining time of the landing phase under the constraint conditions when the robot is in the landing phase, obtain the state variable optimization results and the control input optimization results, and control the robot to move according to the state variable optimization results and the control input optimization results, wherein the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint conditions include at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint.

[0176] In one embodiment of the present disclosure, the device further includes an instruction softening module, which is used to:

[0177] When the robot is in the take-off phase, obtain the vertical velocity of the robot's contact point;

[0178] In response to the speed of the contact point of the robot in the vertical direction being greater than a preset threshold, the control coefficient of the take-off phase is reduced.

[0179] In one embodiment of the present disclosure, when the instruction softening module is used to reduce the control coefficient of the take-off phase, it is used to:

[0180] When the robot is at the end of the take-off phase, the control coefficient is reduced according to a first ratio, wherein the end of the take-off phase includes a second ratio of the last duration of the take-off phase, and the duration of the take-off phase is determined according to a reference trajectory.

[0181] In one embodiment of the present disclosure, the device further includes an interpolation module, configured to:

[0182] Determine a state variable at at least one moment in the landing phase according to a reference state at the landing moment in the reference trajectory and a state at the stable standing moment specified by a user; and / or,

[0183] The control input at at least one moment in the landing phase is determined according to the reference control input at the landing moment in the reference trajectory and the control input at the stable standing moment specified by the user.

[0184] In one embodiment of the present disclosure, the state variable includes a momentum vector and a position vector, wherein the momentum vector includes a linear momentum and a nonlinear momentum, and the position vector includes a floating base position and a joint position; and / or,

[0185] The control input includes a contact point vector and a joint velocity vector, wherein the contact point vector includes an external force applied to a contact point of the robot, and the joint velocity vector includes a velocity of a joint.

[0186] In one embodiment of the present disclosure, the constraint condition includes at least one of the following:

[0187] The momentum vector, position vector and velocity vector of the robot conform to the dynamic model obtained according to the center of mass dynamics and the whole kinematics, wherein the velocity vector includes the floating basis velocity vector and the joint velocity vector, and the floating basis velocity vector includes the velocity of the floating basis;

[0188] The tangential friction force on the contact point of the robot with the ground during the landing phase is less than the maximum static friction force;

[0189] The friction force on the contact point of the robot with the ground during the landing phase is less than the maximum friction force;

[0190] The robot's contact point with the ground is stationary in the landing phase position;

[0191] The contact force on the robot's contact points that are not in contact with the ground during the landing phase is 0;

[0192] The vertical velocity of the robot's contact point that is not in contact with the ground during the landing phase satisfies the specified velocity trajectory.

[0193] In one embodiment of the present disclosure, the device further includes a buffer module, which is used to:

[0194] In response to the robot ending the take-off phase, determining the first control parameter of the robot according to the reference state in the reference trajectory and the feedback state of the robot, and tracking the state variable error and the control input error within a preset time length under constraint conditions to obtain the state variable optimization result and the control input optimization result, and determining the second control parameter of the robot according to the state variable optimization result and the control input optimization result;

[0195] The robot is controlled to move according to the first control parameter and the second control parameter.

[0196] In one embodiment of the present disclosure, the device further includes a first switching module, configured to:

[0197] In response to the time length of the robot ending the take-off phase reaching the preset buffer time length, it is determined that the robot enters the landing phase.

[0198] In one embodiment of the present disclosure, the device further includes a second switching module, configured to:

[0199] When the robot is in a take-off phase, a contact state of a contact point of the robot is acquired, and in response to the contact point being in contact with the ground, it is determined that the robot ends the take-off phase.

[0200] At least one embodiment of the present disclosure further provides a robot, Figure 5 , which shows the structure of the robot, the robot includes a memory and a processor, the memory is used to store computer instructions that can be executed on the processor, and the processor is used to control the robot based on the motion control method in any of the aforementioned embodiments when executing the computer instructions.

[0201] At least one embodiment of the present disclosure further provides a computer-readable storage medium having a computer program stored thereon, and when the program is executed by a processor, the motion control method in any of the aforementioned embodiments is implemented.

[0202] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the disclosure disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present disclosure that follow the general principles of the present disclosure and include common knowledge or customary techniques in the art that are not disclosed in the present disclosure. The description and examples are intended to be exemplary only, and the true scope and spirit of the present disclosure are indicated by the following claims.

[0203] It should be understood that the present disclosure is not limited to the exact structures that have been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present disclosure is limited only by the appended claims.

Claims

1. A motion control method, characterized in that: The method comprises: When the robot is in the take-off stage, the robot is controlled to move according to the reference state in the reference trajectory and the feedback state of the robot; When the robot is in the landing phase, the state variable error and the control input error in the remaining time of the landing phase are tracked under constraints to obtain the state variable optimization results and the control input optimization results, and the robot is controlled to move according to the state variable optimization results and the control input optimization results, wherein the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint conditions include at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint.

2. The motion control method according to claim 1, characterized in that: The method further comprises: When the robot is in the take-off phase, obtain the vertical velocity of the robot's contact point; In response to the speed of the contact point of the robot in the vertical direction being greater than a preset threshold, the control coefficient of the take-off phase is reduced.

3. The motion control method according to claim 2, characterized in that: The control coefficients of the shrinking take-off phase include: When the robot is at the end of the take-off phase, the control coefficient is reduced according to a first ratio, wherein the end of the take-off phase includes a second ratio of the last duration of the take-off phase, and the duration of the take-off phase is determined according to a reference trajectory.

4. The motion control method according to claim 1, characterized in that: The method further comprises: Determine a state variable at at least one moment in the landing phase according to a reference state at the landing moment in the reference trajectory and a state at the stable standing moment specified by a user; and / or, The control input at at least one moment in the landing phase is determined according to the reference control input at the landing moment in the reference trajectory and the control input at the stable standing moment specified by the user.

5. The motion control method according to claim 1, characterized in that: The state variables include a momentum vector and a position vector, wherein the momentum vector includes a linear momentum and a nonlinear momentum, and the position vector includes a floating base position and a joint position; and / or, The control input includes a contact point vector and a joint velocity vector, wherein the contact point vector includes an external force applied to a contact point of the robot, and the joint velocity vector includes a velocity of a joint.

6. The motion control method according to claim 5, characterized in that: The constraint condition includes at least one of the following: The momentum vector, position vector and velocity vector of the robot conform to the dynamic model obtained according to the center of mass dynamics and the whole kinematics, wherein the velocity vector includes the floating basis velocity vector and the joint velocity vector, and the floating basis velocity vector includes the velocity of the floating basis; The tangential friction force on the contact point of the robot with the ground during the landing phase is less than the maximum static friction force; The friction force on the contact point of the robot with the ground during the landing phase is less than the maximum friction force; The robot's contact point with the ground is stationary in the landing phase position; The contact force on the robot's contact points that are not in contact with the ground during the landing phase is 0; The vertical velocity of the robot's contact point that is not in contact with the ground during the landing phase satisfies the specified velocity trajectory.

7. The motion control method according to claim 1, characterized in that: The method further comprises: In response to the robot ending the take-off phase, determining the first control parameter of the robot according to the reference state in the reference trajectory and the feedback state of the robot, and tracking the state variable error and the control input error within a preset time length under constraint conditions to obtain the state variable optimization result and the control input optimization result, and determining the second control parameter of the robot according to the state variable optimization result and the control input optimization result; The robot is controlled to move according to the first control parameter and the second control parameter.

8. The motion control method according to claim 7, characterized in that: The method further comprises: In response to the time length of the robot ending the take-off phase reaching the preset buffer time length, it is determined that the robot enters the landing phase.

9. The motion control method according to claim 1 or 7, characterized in that: The method further comprises: When the robot is in a take-off phase, a contact state of a contact point of the robot is acquired, and in response to the contact point being in contact with the ground, it is determined that the robot ends the take-off phase.

10. A motion control device, characterized in that: The device comprises: A take-off module is used to control the robot to move according to the reference state in the reference trajectory and the feedback state of the robot when the robot is in the take-off stage; A landing module is used to track the state variable error and control input error in the remaining time of the landing phase under constraints when the robot is in the landing phase, obtain the state variable optimization result and the control input optimization result, and control the robot to move according to the state variable optimization result and the control input optimization result, wherein the state variable error includes the error between the specified state and the state variable optimization result, the control input error includes the error between the specified control input and the control input optimization result, and the constraint condition includes at least one of the center of mass dynamics constraint, the full kinematics constraint and the collision dynamics constraint.

11. A robot, characterized in that: The robot comprises a memory and a processor, wherein the memory is used to store computer instructions executable on the processor, and the processor is used to implement the method according to any one of claims 1 to 9 when executing the computer instructions.

12. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.

Images