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 that according to a motion sequence of target motion, a track of the target motion is dispersed into a plurality of track points, the motion sequence comprises a plurality of motion stages, and at least one contact point with different contact states exists between different motion stages; the track point has a state variable; the state variables of the trajectory points in the multiple trajectory points are optimized, the state variable optimization result of the trajectory points is obtained, and the optimization direction of the state variables comprises the steps that the trajectory of the target motion conforms to the action sequence; and controlling the robot to execute the target motion according to the state variable optimization result of the track point.

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] According to an action sequence of the target movement, the trajectory of the target movement is discretized into a plurality of trajectory points, wherein the action sequence includes a plurality of action stages, different action stages have at least one contact point with a different contact state, and the trajectory point has a state variable;

[0006] Optimizing the state variables of the trajectory points among the plurality of trajectory points to obtain the state variable optimization results of the trajectory points, wherein the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence;

[0007] According to the state variable optimization result of the trajectory point, the robot is controlled to execute the target movement.

[0008] In one embodiment of the present disclosure, optimizing the state variables of the trajectory points among the plurality of trajectory points to obtain the state variable optimization results of the trajectory points includes:

[0009] The state variables of the trajectory points among the plurality of trajectory points are optimized by using a pre-constructed cost function to obtain an optimization result of the state variables of the trajectory points, wherein the cost function is related to at least one of the following:

[0010] an error between a position vector of at least one key trajectory point among the plurality of trajectory points and a preset reference position;

[0011] a contact force of at least one trajectory point of the plurality of trajectory points;

[0012] A joint angular velocity of at least one trajectory point among the plurality of trajectory points.

[0013] In one embodiment of the present disclosure, optimizing the state variables of the trajectory points among the plurality of trajectory points to obtain the state variable optimization results of the trajectory points includes:

[0014] The state variables of the trajectory points among the plurality of trajectory points are optimized under the constraint of a first constraint condition to obtain an optimization result of the state variables of the trajectory points, wherein the first constraint condition is related to the action sequence.

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

[0016] The state variables of each trajectory point conform to the dynamic model obtained from the center-of-mass dynamics and full kinematics;

[0017] The state variables of adjacent trajectory points satisfy the integral continuity;

[0018] The state variables of each trajectory point meet the relevant limit conditions;

[0019] The contact point with the ground has no relative motion with the ground, the supporting force between the contact point and the ground is not less than 0, and the friction between the contact point and the ground is not greater than the maximum static sliding friction;

[0020] The supporting force between the contact point without contact with the ground and the ground is 0, and the speed meets the speed requirement of the preset reference trajectory.

[0021] In one embodiment of the present disclosure, under the constraint of the first constraint condition, optimizing the state variables of the trajectory points among the multiple trajectory points using the cost function to obtain the state variable optimization result of the trajectory point includes:

[0022] Under the constraint of the linearization processing result of the first constraint condition, the state variables of the trajectory points among the multiple trajectory points are optimized using the quadratic approximation result of the cost function to obtain the state variable optimization result of the trajectory point.

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

[0024] A slack variable is added to the first constraint condition to perform linearization processing on the first constraint condition.

[0025] In one embodiment of the present disclosure, the state variables include at least one of the following: a momentum vector, a position vector, a contact point vector, and a joint velocity vector, wherein the momentum vector includes linear momentum and nonlinear momentum, the position vector includes a floating base position and a joint position, the floating base position includes a coordinate position and an attitude angle in the form of a unit quaternion, the contact point vector includes an external force acting on the contact point, and the joint velocity vector includes the velocity of the joint.

[0026] In one embodiment of the present disclosure, the step of controlling the robot to execute the target motion according to the state variable optimization result of the trajectory point includes:

[0027] Tracking at least one slack variable under a second constraint condition to obtain an optimization result of the optimization variable, wherein at least one condition in the second constraint condition is related to the at least one slack variable, at least one condition in the second constraint condition is related to the optimization variable, and at least one condition in the second constraint condition is related to the optimization result of the state variable;

[0028] According to the optimization result of the optimization variable, the robot is controlled to execute the target motion.

[0029] In one embodiment of the present disclosure, the optimization variables include an acceleration vector, a contact point vector and a relaxation vector, wherein the acceleration vector includes the acceleration of the joint, the contact point vector includes the external force exerted on the contact point, and the relaxation vector includes the at least one relaxation variable.

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

[0031] Floating base dynamics consistency;

[0032] The contact point with the ground has no relative motion with the ground;

[0033] The friction between the contact point with the ground and the ground is not greater than the maximum static sliding friction;

[0034] The contact point not in contact with the ground tracks the position and velocity in the state variable optimization result under the relaxation effect of the first relaxation variable;

[0035] The floating base of the robot tracks the floating base posture in the state variable optimization result under the relaxation effect of the second relaxation variable;

[0036] The arm joints of the robot track the joint positions and velocities in the state variable optimization results under the relaxation effect of the third relaxation variable;

[0037] The contact point of the robot tracks the contact point vector in the state variable optimization result under the relaxation effect of the fourth relaxation variable.

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

[0039] A discretization module, used for discretizing the trajectory of the target movement into a plurality of trajectory points according to an action sequence of the target movement, wherein the action sequence includes a plurality of action stages, and there is at least one contact point with a different contact state between different action stages, and the trajectory point has a state variable;

[0040] an optimization module, configured to optimize the state variables of the trajectory points among the plurality of trajectory points to obtain optimization results of the state variables of the trajectory points, wherein the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence;

[0041] The control module is used to control the robot to execute the target motion according to the state variable optimization result of the trajectory point.

[0042] In one embodiment of the present disclosure, the optimization module is used to:

[0043] The state variables of the trajectory points among the plurality of trajectory points are optimized by using a pre-constructed cost function to obtain an optimization result of the state variables of the trajectory points, wherein the cost function is related to at least one of the following:

[0044] an error between a position vector of at least one key trajectory point among the plurality of trajectory points and a preset reference position;

[0045] a contact force of at least one trajectory point of the plurality of trajectory points;

[0046] A joint angular velocity of at least one trajectory point among the plurality of trajectory points.

[0047] In one embodiment of the present disclosure, the optimization module is used to:

[0048] The state variables of the trajectory points among the plurality of trajectory points are optimized under the constraint of a first constraint condition to obtain an optimization result of the state variables of the trajectory points, wherein the first constraint condition is related to the action sequence.

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

[0050] The state variables of each trajectory point conform to the dynamic model obtained from the center-of-mass dynamics and full kinematics;

[0051] The state variables of adjacent trajectory points satisfy the integral continuity;

[0052] The state variables of each trajectory point meet the relevant limit conditions;

[0053] The contact point with the ground has no relative motion with the ground, the supporting force between the contact point and the ground is not less than 0, and the friction between the contact point and the ground is not greater than the maximum static sliding friction;

[0054] The supporting force between the contact point without contact with the ground and the ground is 0, and the speed meets the speed requirement of the preset reference trajectory.

[0055] In one embodiment of the present disclosure, the optimization module is used to optimize the state variables of the trajectory points among the multiple trajectory points using the cost function under the constraint of the first constraint condition, and when obtaining the state variable optimization result of the trajectory point, is used to:

[0056] Under the constraint of the linearization processing result of the first constraint condition, the state variables of the trajectory points among the multiple trajectory points are optimized using the quadratic approximation result of the cost function to obtain the state variable optimization result of the trajectory point.

[0057] In one embodiment of the present disclosure, the device further comprises a linear module, which is used to:

[0058] A slack variable is added to the first constraint condition to perform linearization processing on the first constraint condition.

[0059] In one embodiment of the present disclosure, the state variables include at least one of the following: a momentum vector, a position vector, a contact point vector, and a joint velocity vector, wherein the momentum vector includes linear momentum and nonlinear momentum, the position vector includes a floating base position and a joint position, the floating base position includes a coordinate position and an attitude angle in the form of a unit quaternion, the contact point vector includes an external force acting on the contact point, and the joint velocity vector includes the velocity of the joint.

[0060] In one embodiment of the present disclosure, the control module is used to:

[0061] Tracking at least one slack variable under a second constraint condition to obtain an optimization result of the optimization variable, wherein at least one condition in the second constraint condition is related to the at least one slack variable, at least one condition in the second constraint condition is related to the optimization variable, and at least one condition in the second constraint condition is related to the optimization result of the state variable;

[0062] According to the optimization result of the optimization variable, the robot is controlled to execute the target motion.

[0063] In one embodiment of the present disclosure, the optimization variables include an acceleration vector, a contact point vector and a relaxation vector, wherein the acceleration vector includes the acceleration of the joint, the contact point vector includes the external force exerted on the contact point, and the relaxation vector includes the at least one relaxation variable.

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

[0065] Floating base dynamics consistency;

[0066] The contact point with the ground has no relative motion with the ground;

[0067] The friction between the contact point with the ground and the ground is not greater than the maximum static sliding friction;

[0068] The contact point not in contact with the ground tracks the position and velocity in the state variable optimization result under the relaxation effect of the first relaxation variable;

[0069] The floating base of the robot tracks the floating base posture in the state variable optimization result under the relaxation effect of the second relaxation variable;

[0070] The arm joints of the robot track the joint positions and velocities in the state variable optimization results under the relaxation effect of the third relaxation variable;

[0071] The contact point of the robot tracks the contact point vector in the state variable optimization result under the relaxation effect of the fourth relaxation variable.

[0072] 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.

[0073] 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.

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

[0075] The motion control method provided by the embodiment of the present disclosure discretizes the trajectory of the target motion into a plurality of trajectory points according to the action sequence of the target motion, optimizes the state variables of the trajectory points among the plurality of trajectory points, obtains the state variable optimization results of the trajectory points, and then controls the robot to execute the target motion according to the state variable optimization results of each trajectory point among the plurality of trajectory points. Since the action sequence includes a plurality of action stages, there is at least one contact point with a different contact state between different action stages, and the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence, the method can plan the trajectory of the target motion in real time, and control the robot to execute the target motion based on the planning results, and even if the target motion is a high-dynamic motion including a vacant state, the method can still plan the trajectory of the high-dynamic motion in real time, and control the robot to stably complete the high-dynamic motion based on the planning results, thereby reducing the impact of the collision impact at the moment of landing on the stability of the robot motion. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] 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.

[0077] Figure 1 A schematic diagram of the structure of a bipedal robot shown in an exemplary embodiment of the present disclosure;

[0078] Figure 2 A flowchart of a motion control method shown in an exemplary embodiment of the present disclosure;

[0079] Figure 3 A schematic diagram of contact points of a robot shown in an exemplary embodiment of the present disclosure;

[0080] Figure 4 A schematic diagram of an action sequence shown in an exemplary embodiment of the present disclosure;

[0081] Figure 5 A schematic diagram of a control frame shown in an exemplary embodiment of the present disclosure;

[0082] Figure 6 A schematic structural diagram of a motion control device shown in an exemplary embodiment of the present disclosure;

[0083] Figure 7 A structural block diagram of a robot shown in an exemplary embodiment of the present disclosure. DETAILED DESCRIPTION

[0084] 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.

[0085] 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.

[0086] 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".

[0087] 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, thereby bringing convenience to users. In related technologies, when robots perform some high-dynamic movements that include a vacant state (i.e., high-dynamic movements are movements that include a vacant state), the robot's motion stability is often affected by the collision impact at the moment of landing.

[0088] For example, a robot can perform high-dynamic motion by tracking its reference trajectory, and its reference trajectory is often a trajectory that is pre-planned offline and configured in the robot. This trajectory cannot adapt to situations such as collisions and impacts in high-dynamic motion that are prone to trajectory tracking errors, and has poor robustness, resulting in large errors and poor stability during the robot's high-dynamic motion, and may even be unable to complete high-dynamic motion or cause danger.

[0089] 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.

[0090] Exemplarily, the method can be applied to the planning and control of high-dynamic motion of a robot, and high-dynamic motion can refer to motion that includes a completely airborne phase. For example, the method can be a real-time planning and control method for high-dynamic motion based on a combination of model predictive control (MPC) and whole-body control (WBC). The robot can use a model predictive control (MPC) module and a whole-body control (WBC) module to complete the method, that is, use the MPC module to plan the trajectory of high-dynamic motion in real time, and use the WBC module to control the robot to perform high-dynamic motion according to the real-time planning results of the MPC module. The above modules can run in different threads at their respective frequencies, and generally the control frequency of the WBC module is not lower than the operating frequency of the MPC module.

[0091] The present disclosure first establishes a dynamic model of the robot system, and then constructs the complete process of the method based on the dynamic model. Therefore, before introducing the specific process of the method, the above dynamic model and its construction process are first introduced in detail.

[0092] The legged robot can be modeled as a multi-rigid body system with a floating base B, to which the legs and arms are connected; the motion of the entire system can be described relative to a fixed inertial system I (i.e., the world coordinate system); the position vector q and velocity vector v of the system are respectively:

[0093]

[0094]

[0095] In the above formula, q b =[r IB ,qIB ] T ∈R 7 is the position of the floating base in the coordinate system I, and its position is the position r of the floating base relative to the origin of the inertial coordinate system IB Indicates that the attitude angle is expressed by the unit quaternion q IB =[q w ,q x ,q y ,q z ] T Indicates that this avoids universal joint lock issues and improves applicability. j represents the joint angle, v b represents the velocity of the floating base B, I v IB represents the position of the floating base expressed in coordinate system B relative to the velocity of inertial system I, B ω IB is the angular velocity of the floating base expressed in coordinate system B relative to the inertial system I. j , Represents the joint angular velocity. n j is the number of robot joints.

[0096] Furthermore, the full model multi-rigid body dynamics equation with contact of the legged robot system (hereinafter referred to as the first dynamics model) can be written as:

[0097]

[0098] Where M, b represent the inertia matrix and nonlinear terms (the expression of Coriolis force, centrifugal force and gravity in generalized coordinate space) respectively; is the acceleration vector, including the acceleration of the robot's floating base and the acceleration of each joint; τ is the torque vector, including the torque of each joint of the robot; is the selection matrix of the drivable joints; J c is the velocity Jacobian matrix of the contact point, f c is the contact point vector, which contains the external force on each contact point of the robot.

[0099] Furthermore, the floating-based dynamics model of the legged robot system (hereinafter referred to as the second dynamics model) can be:

[0100]

[0101] In the above formula, is the floating basis selection matrix.

[0102] Furthermore, the center of mass dynamics of the legged robot system is combined with the full kinematic model

[0103] In order to improve computational efficiency, the full model multi-rigid body dynamics of the robot is not considered in the MPC module. We use a model that combines centroidal dynamics and full kinematics, referred to as kino-dynamics. If we have enough control over the joints, we can control the robot to generate any external force to affect the center of mass momentum of the whole machine, thereby controlling the overall movement of the robot.

[0104] Center of mass dynamics is the Newton-Euler equations applied to the robot's center of mass (CoM):

[0105]

[0106] In the above formula, 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 direction of the coordinate axis is the same as that of the inertial coordinate system I. is the position of contact point i relative to the center of mass, and and are the contact force and torque on contact point i, respectively. The present disclosure uses the four vertices of the sole as contact points to describe the contact between the foot-type robot and the outside world. Therefore, when the robot is a humanoid robot, the center of mass dynamics model includes a total of 8 contact forces corresponding to the contact points, and no contact torque, that is,

[0107] Through the centroidal momentum matrix (CMM) Relating the center of mass dynamics to whole-body kinematics yields the following equation:

[0108]

[0109] According to the properties of quaternion algebraic operations, we can get:

[0110]

[0111] In the above formula,

[0112] Assume the state and control input of the robot system are as follows:

[0113]

[0114] In the above formula, v j represents the joint angular velocity, n c is the number of points in contact with the outside world.

[0115] Based on the above center-of-mass dynamics, full kinematics, and the state and control input of the system, the robot's continuous dynamics model kino-dynamics (hereinafter referred to as the third dynamics model) can be obtained as follows:

[0116]

[0117] Please refer to the attached Figure 2 , which shows the process of the method, including steps S201 to S203. Among them, steps S201 to S202 can be executed by the MPC module to plan the trajectory of high dynamic motion online in real time; step S203 can be executed by the WBC module to enable the robot to track the trajectory of the high dynamic motion and complete the high dynamic motion.

[0118] In step S201, according to the action sequence of the target motion, the trajectory of the target motion is discretized into a plurality of trajectory points, wherein the action sequence includes a plurality of action stages, different action stages have at least one contact point with a different contact state, and the trajectory points have state variables.

[0119] The action sequence of the target motion can be pre-selected and constructed. The target motion can be a high-dynamic motion. During the high-dynamic motion, the contact state between the robot and the ground often varies at different stages. The contact state between the robot and the ground includes the contact state between each contact point of the robot and the ground. For example, a set of arrays that change over time can be used to represent the contact state of each contact point at different times. This array can be called a contact state array, denoted by c. c i = c[i] = 1 means that the contact point numbered i is in contact with the outside world (the contact state with the ground is in contact), c i = c[i] = 0 means that the contact point numbered i is not in contact with the outside world (the contact state with the ground is not in contact). The dimension of C is the number of all contact points of the robot; please refer to the attached Figure 3 , taking the example of a biped robot with 4 contact points on each foot, the array dimension is 8.

[0120] Based on the above analysis, according to the target motion, the event in which the contact state between at least one contact point of the first joint model and the ground changes can be determined as a switching event, and the motion between adjacent switching events can be determined as an action stage, wherein each action stage is described by a dynamic equation, that is, different action stages are described by different dynamic equations. Figure 1 Taking the biped robot shown in FIG. 1 as an example, the robot performs a backflip motion (i.e., the target motion is a backflip), and the following example can be constructed in the above manner: Figure 4The action sequence shown includes four motion stages: full foot contact, toe contact, no contact, and full foot contact, as well as a heel lift switching event between full foot contact and toe contact, a toe lift switching event between toe contact and no contact, and a landing switching event between no contact and full foot contact.

[0121] 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. Figure 5 , t0, t1, t2... are control frames, and the time interval between adjacent control frames is δt, that is, the control frequency is

[0122] Please continue to refer to the attached Figure 5 When this step is executed in each control frame, it is possible to construct a time T in the future. horizon The trajectory is obtained by discretizing the trajectory according to a preset time step Δt, and a plurality of trajectory points with an interval of Δt are obtained. The states and control inputs corresponding to these trajectory points are the target variables for trajectory planning of the method, which are referred to as state variables below. Exemplarily, the state variables may include at least one of the following: a momentum vector, a position vector, a contact point vector, and a joint velocity vector, wherein the momentum vector includes linear momentum and nonlinear momentum, the position vector includes a floating base position and the position of (each) joint, the floating base position includes a coordinate position and an attitude angle in the form of a unit quaternion, the contact point vector includes the contact state of (each) contact point with the ground, and the joint velocity vector includes the velocity of (each) joint. For example, the state variable X is shown as follows:

[0123] X={h[k],q[k],f c [k],v j [k]}, for all k=1,...,N

[0124] In the above formula, k is one of the N knot nodes obtained after the motion trajectory is discretized, i is one of the nc contact points, c is an nc×N two-dimensional matrix, each column corresponds to a trajectory point, and each column includes the contact point vector on the corresponding trajectory point; h[k] is the momentum vector of the kth trajectory point, q[k] is the position vector of the kth trajectory point, and f c [k] is the contact point vector of the kth trajectory point, v j [k] is the joint velocity vector of the kth trajectory point, which contains the angular velocity of each joint of the robot.

[0125] It is understandable that the action sequence may also include the time, state x and control input u of the key trajectory points in the above trajectory, such as the height of the floating base at the highest point of the somersault motion in the air phase.

[0126] In step S102, the state variables of the trajectory points among the plurality of trajectory points are optimized to obtain the optimization results of the state variables of the trajectory points, wherein the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence.

[0127] Exemplarily, a pre-constructed cost function is used to optimize the state variables of the trajectory points among the multiple trajectory points to obtain the state variable optimization results of the trajectory points, wherein the cost function is related to at least one of the following: the error between the position vector of at least one key trajectory point among the multiple trajectory points and a preset reference position; the contact force of at least one trajectory point among the multiple trajectory points; the joint angular velocity of at least one trajectory point among the multiple trajectory points.

[0128] As another example, the state variables of the trajectory points among the multiple trajectory points are optimized under the constraint of a first constraint condition to obtain an optimization result of the state variables of the trajectory points, wherein the first constraint condition is related to the action sequence.

[0129] Also exemplarily, under the constraint of a first constraint condition, a pre-constructed cost function is used to optimize the state variables of the trajectory points among the multiple trajectory points to obtain an optimization result of the state variables of the trajectory points, wherein the first constraint condition is related to the action sequence, and the cost function is related to at least one of the following: an error between a position vector of at least one key trajectory point among the multiple trajectory points and a preset reference position; a contact force of at least one trajectory point among the multiple trajectory points; and a joint angular velocity of at least one trajectory point among the multiple trajectory points.

[0130] Next, the cost function and the first constraint mentioned in the above three examples are illustrated.

[0131] For example, the cost function may be expressed as follows:

[0132]

[0133] In the above formula, k # is the key trajectory point, K # is a set of key trajectory points, which can include the trajectory of the target movement (for example, T after the current moment horizon All key trajectory points in the trajectory in time); q[k # ] is k # The position vector on ref [k #] is k # The reference position on the graph; W1 and W2 are weight matrices respectively.

[0134] For example, the first constraint condition includes at least one of the following:

[0135] The first item: The state variables of each trajectory point conform to the dynamic model obtained based on the center of mass dynamics and full kinematics (i.e., the third dynamic model).

[0136] The second term is that the state variables of adjacent trajectory points satisfy the integral continuity, that is:

[0137]

[0138] Third, the state variable of each trajectory point meets the relevant limit conditions, that is, at least one of the following:

[0139]

[0140]

[0141] In the above formula, q min is the lower limit of the joint position, q max is the upper limit of the joint position, is the upper limit of the joint velocity.

[0142] Item 4: The contact point with the ground has no relative motion with the ground, the support force between the contact point and the ground is not less than 0, and the friction force between the contact point and the ground is not greater than the maximum static sliding friction force, that is:

[0143]

[0144] f c,z [k]≥0, if c i [k]==1

[0145]

[0146] In the above formula, is the velocity vector of the kth trajectory point; c i [k] is the contact state of the ith contact point of the kth trajectory point, 1 for contact and 0 for no contact; f c,z [k] is the support force between the (i-th) contact point in the k-th trajectory and the ground, f c,x [k], f c,y [k] is the friction force between the (i-th) contact point in the k-th trajectory point and the ground along the X-axis and along the Y-axis, and μ is the maximum static friction coefficient.

[0147] Item 5: The support force between the contact point without contact with the ground and the ground is 0, and the speed meets the speed requirements of the preset reference trajectory, that is:

[0148] f c,z [k]=0,if c i [k]==1

[0149]

[0150] In the above formula, v c [k] is the velocity of the (i-th) contact point in the k-th trajectory point, is the reference velocity of the (i-th) contact point in the k-th trajectory point, is the reference position of the (i-th) contact point in the k-th trajectory point, r c (t) is the actual position of the (i-th) contact point in the k-th trajectory point.

[0151] The above example constructs the constraint conditions and / or cost functions for state variable optimization. Further, when the constraint conditions and / or cost functions are used to optimize the state variables, the first constraint condition may be linearized (for example, the first constraint condition may be linearized as: In the formula, Represents the nonlinear term A linearized approximation of the first constraint condition is performed, and a quadratic approximation is performed on the cost function, so that under the constraint of the linearized processing result of the first constraint condition, the state variables of the trajectory points among the multiple trajectory points are optimized using the quadratic approximation result of the cost function to obtain the state variable optimization result of the trajectory point.

[0152] The linearization of constraints and / or the quadratic approximation of cost functions can simplify the optimization process of state variables, that is, simplify the process into a sequential quadratic programming problem (SQP) consisting of quadratic programming problems (QP) at each trajectory point, thereby improving the efficiency of online real-time planning trajectories.

[0153] Optionally, a slack variable is added to the first constraint condition to linearize the first constraint condition. For example, in the initial QP, the value of the slack variable is large; after solving the first QP problem, the value of the slack variable is gradually reduced, the constraint condition is linearized again and the QP problem is constructed and solved; this cycle is repeated until the convergence accuracy is met to obtain the state variable optimization result.

[0154] The above steps S201 and S202 have completed the trajectory planning of high-dynamic motion, and the target motion (in the future period of time Thorizon The state variable optimization result of the trajectory point on the trajectory of the inner (inside), the state variable optimization result characterizes the state and control input of the trajectory point. Combining the details of the above two steps, it can be seen that the MPC module can construct the trajectory planning of high dynamic motion as an optimal control problem (OCP, Optimal Control Problem), discretize the original problem by direct multiple-shooting, add cost functions and constraints at each discretized trajectory point, and perform second-order approximation on the optimization target (such as cost function), perform linear approximation on the constraints, and finally convert the original problem into a series of QP problems (i.e., SQP) that gradually approach the original nonlinear problem. These QPs are solved in turn, and finally the solution to the original problem is obtained, i.e., the trajectory of high dynamic motion.

[0155] It should be understood that after the MPC module completes step S102 to obtain the state variable optimization result of each trajectory point (obtained after the trajectory is discretized) (i.e., the motion trajectory of online planning), the state variable optimization result of each trajectory point (obtained after the trajectory is discretized) can be sent to the WBC module, so that the WBC module controls the robot to move according to step S203.

[0156] In step S203, the robot is controlled to execute the target motion according to the state variable optimization result of each trajectory point in the multiple trajectory points.

[0157] Exemplarily, step S203 may be performed in each control frame in the following manner: first, tracking at least one slack variable under the second constraint condition to obtain an optimization result of the optimization variable; next, controlling the robot to perform the target motion according to the optimization result of the optimization variable.

[0158] Wherein, the optimization variables include an acceleration vector, a contact point vector and a relaxation vector, wherein the acceleration vector includes the acceleration of (each) joint, the contact point vector includes the contact state between (each) contact point and the ground, and the relaxation vector includes the at least one relaxation variable.

[0159] At least one of the second constraints is related to the at least one slack variable, at least one of the second constraints is related to the optimization variable, and at least one of the second constraints is related to the optimization result of the state variable. For example, the second constraint includes at least one of the following:

[0160] The first term: floating basis dynamics consistency, that is:

[0161]

[0162] The various parameters in the above formula have been introduced in detail when introducing the second kinetic model, and will not be repeated here.

[0163] The second item: The contact point with the ground has no relative motion with the ground, that is:

[0164]

[0165] In the above formula, is the acceleration vector. The other parameters have been introduced in detail in the previous article and will not be repeated here.

[0166] The third item: The friction between the contact point with the ground and the ground is not greater than the maximum static sliding friction, that is:

[0167] ||f c,x ||≤μf c,z ,||f c,y ||≤μf c,z ,if c i ==1

[0168] The various parameters in the above formula have been introduced in detail in the previous article and will not be repeated here.

[0169] Fourth item: The contact point that is not in contact with the ground tracks the position and velocity in the state variable optimization result under the relaxation effect of the first relaxation variable, that is:

[0170]

[0171] In the above formula, The contact point position in the state variable optimization result, r c The actual location of the contact point, The contact point velocity in the state variable optimization results is, is the actual velocity of the contact point, k p , k d , w1 are the proportional coefficient, differential coefficient and the first relaxation coefficient respectively. The other parameters have been introduced in the previous article and will not be repeated here.

[0172] Item 5: The floating base of the robot tracks the floating base pose in the state variable optimization result under the relaxation of the second relaxation variable, that is:

[0173]

[0174] In the above formula, J b is a floating-basis Jacobian matrix, The floating basis position in the optimization result for the state variable, q b is the actual position of the floating base, Floating basis velocities in state variable optimization results, is the actual velocity of the floating base, and w2 is the second relaxation coefficient.

[0175] Item 6: The arm joints of the robot track the joint positions and velocities in the state variable optimization results under the relaxation of the third relaxation variable, that is:

[0176]

[0177] In the above formula, S arm Select the matrix for the arm joint, The position of the arm joint in the state variable optimization result, q arm is the actual position of the arm joint, The velocity of the arm joints in the state variable optimization result, is the actual velocity of the arm joint, and w3 is the third relaxation coefficient.

[0178] Item 7: The contact point of the robot tracks the contact point vector in the state variable optimization result under the relaxation effect of the fourth relaxation variable, that is:

[0179]

[0180] In the above formula, is the external force on the contact point in the state variable optimization result, f c is the actual external force on the contact point, and w4 is the fourth relaxation variable.

[0181] For example, the QP problem represented by the following formula is constructed to optimize the optimization variables:

[0182]

[0183] In the above formula, Q is a positive definite matrix, and the optimization variables of QP are generalized acceleration, external force and relaxation variables The goal is to minimize the slack variable w of some equality constraints according to the weight coefficients.

[0184] This step obtains After that, the torque command of the joint end can be obtained through the inverse dynamics calculation of the following formula:

[0185]

[0186] The WBC module sends the above torque command to the tube stage driver, thereby driving the joint to perform movement and enabling the robot to complete the target movement.

[0187] The motion control method provided by the embodiment of the present disclosure discretizes the trajectory of the target motion into a plurality of trajectory points according to the action sequence of the target motion, optimizes the state variables of the trajectory points among the plurality of trajectory points, obtains the state variable optimization results of the trajectory points, and then controls the robot to execute the target motion according to the state variable optimization results of each trajectory point among the plurality of trajectory points. Since the action sequence includes a plurality of action stages, there is at least one contact point with a different contact state between different action stages, and the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence, the method can plan the trajectory of the target motion in real time, and control the robot to execute the target motion based on the planning results, and even if the target motion is a high-dynamic motion including a vacant state, the method can still plan the trajectory of the high-dynamic motion in real time, and control the robot to stably complete the high-dynamic motion based on the planning results, thereby reducing the impact of the collision impact at the moment of landing on the stability of the robot motion.

[0188] The present invention discloses a high-dynamic motion real-time planning control method based on the combination of model predictive control (MPC) and whole-body control (WBC) for humanoid / legged robots. Specifically, the real-time planning controller proposed in this patent consists of an online model predictive control (MPC) module and a whole-body control (WBC) module. The two modules run on different threads at their respective frequencies. Generally, the control frequency of the WBC module is not lower than the operating frequency of the MPC module. In the construction of the optimization problem of the MPC module, the dynamics of the system adopts the kino-dynamics model (i.e., the third dynamics model mentioned above) that combines the robot's center of mass dynamics and full kinematics, and uses multiple shooting methods and reduced-order approximation processing to transform the complex original nonlinear optimization problem into a sequential quadratic programming problem (SQP), thereby reducing the computational time and achieving online solution. The optimal solution obtained is sent to the WBC module as the expected trajectory. In the optimization problem construction of the WBC module, the dynamics of the system adopts full-rigid-dynamics with contact (i.e., the first dynamic model mentioned above). The WBC module tracks the desired trajectory provided by MPC while satisfying multiple physical constraints, and constructs the control problem as a quadratic programming (QP) problem. The solution of QP is used as the control instruction of the joint end. The joint end executes the control instruction to complete the high-dynamic action.

[0189] According to a second aspect of the present disclosure, a motion control device is provided. Figure 6 , the device comprises:

[0190] A discretization module 601 is used to discretize the trajectory of the target movement into a plurality of trajectory points according to an action sequence of the target movement, wherein the action sequence includes a plurality of action stages, and there is at least one contact point with a different contact state between different action stages, and the trajectory point has a state variable;

[0191] The optimization module 602 is used to optimize the state variables of the trajectory points among the plurality of trajectory points to obtain the optimization results of the state variables of the trajectory points, wherein the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence;

[0192] The control module 603 is used to control the robot to execute the target motion according to the state variable optimization result of the trajectory point.

[0193] In one embodiment of the present disclosure, the optimization module is used to:

[0194] The state variables of the trajectory points among the plurality of trajectory points are optimized by using a pre-constructed cost function to obtain an optimization result of the state variables of the trajectory points, wherein the cost function is related to at least one of the following:

[0195] an error between a position vector of at least one key trajectory point among the plurality of trajectory points and a preset reference position;

[0196] a contact force of at least one trajectory point of the plurality of trajectory points;

[0197] A joint angular velocity of at least one trajectory point among the plurality of trajectory points.

[0198] In one embodiment of the present disclosure, the optimization module is used to:

[0199] The state variables of the trajectory points among the plurality of trajectory points are optimized under the constraint of a first constraint condition to obtain an optimization result of the state variables of the trajectory points, wherein the first constraint condition is related to the action sequence.

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

[0201] The state variables of each trajectory point conform to the dynamic model obtained from the center-of-mass dynamics and full kinematics;

[0202] The state variables of adjacent trajectory points satisfy the integral continuity;

[0203] The state variables of each trajectory point meet the relevant limit conditions;

[0204] The contact point with the ground has no relative motion with the ground, the supporting force between the contact point and the ground is not less than 0, and the friction between the contact point and the ground is not greater than the maximum static sliding friction;

[0205] The supporting force between the contact point without contact with the ground and the ground is 0, and the speed meets the speed requirement of the preset reference trajectory.

[0206] In one embodiment of the present disclosure, the optimization module is used to optimize the state variables of the trajectory points among the multiple trajectory points using the cost function under the constraint of the first constraint condition, and when obtaining the state variable optimization result of the trajectory point, is used to:

[0207] Under the constraint of the linearization processing result of the first constraint condition, the state variables of the trajectory points among the multiple trajectory points are optimized using the quadratic approximation result of the cost function to obtain the state variable optimization result of the trajectory point.

[0208] In one embodiment of the present disclosure, the device further comprises a linear module, which is used to:

[0209] A slack variable is added to the first constraint condition to perform linearization processing on the first constraint condition.

[0210] In one embodiment of the present disclosure, the state variables include at least one of the following: a momentum vector, a position vector, a contact point vector, and a joint velocity vector, wherein the momentum vector includes linear momentum and nonlinear momentum, the position vector includes a floating base position and a joint position, the floating base position includes a coordinate position and an attitude angle in the form of a unit quaternion, the contact point vector includes an external force acting on the contact point, and the joint velocity vector includes the velocity of the joint.

[0211] In one embodiment of the present disclosure, the control module is used to:

[0212] Tracking at least one slack variable under a second constraint condition to obtain an optimization result of the optimization variable, wherein at least one condition in the second constraint condition is related to the at least one slack variable, at least one condition in the second constraint condition is related to the optimization variable, and at least one condition in the second constraint condition is related to the optimization result of the state variable;

[0213] According to the optimization result of the optimization variable, the robot is controlled to execute the target motion.

[0214] In one embodiment of the present disclosure, the optimization variables include an acceleration vector, a contact point vector and a relaxation vector, wherein the acceleration vector includes the acceleration of the joint, the contact point vector includes the external force exerted on the contact point, and the relaxation vector includes the at least one relaxation variable.

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

[0216] Floating base dynamics consistency;

[0217] The contact point with the ground has no relative motion with the ground;

[0218] The friction between the contact point with the ground and the ground is not greater than the maximum static sliding friction;

[0219] The contact point not in contact with the ground tracks the position and velocity in the state variable optimization result under the relaxation effect of the first relaxation variable;

[0220] The floating base of the robot tracks the floating base posture in the state variable optimization result under the relaxation effect of the second relaxation variable;

[0221] The arm joints of the robot track the joint positions and velocities in the state variable optimization results under the relaxation effect of the third relaxation variable;

[0222] The contact point of the robot tracks the contact point vector in the state variable optimization result under the relaxation effect of the fourth relaxation variable.

[0223] In one embodiment of the present disclosure, a robot is provided. Figure 7 It shows the structure of the robot, which 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 execute the computer instructions based on the motion control method in any of the aforementioned embodiments.

[0224] In one embodiment of the present disclosure, a computer-readable storage medium is provided, on which a computer program is stored. When the program is executed by a processor, the motion control method in any of the aforementioned embodiments is implemented.

[0225] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the disclosure disclosed herein. The present disclosure 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 to be considered exemplary only, and the true scope and spirit of the present disclosure are indicated by the following claims.

[0226] 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: According to an action sequence of the target movement, the trajectory of the target movement is discretized into a plurality of trajectory points, wherein the action sequence includes a plurality of action stages, different action stages have at least one contact point with a different contact state, and the trajectory point has a state variable; Optimizing the state variables of the trajectory points among the plurality of trajectory points to obtain optimization results of the state variables of the trajectory points, wherein the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence; According to the state variable optimization result of the trajectory point, the robot is controlled to execute the target movement.

2. The motion control method according to claim 1, characterized in that: The step of optimizing the state variables of the trajectory points among the plurality of trajectory points to obtain the optimization results of the state variables of the trajectory points includes: The state variables of the trajectory points among the plurality of trajectory points are optimized by using a pre-constructed cost function to obtain an optimization result of the state variables of the trajectory points, wherein the cost function is related to at least one of the following: an error between a position vector of at least one key trajectory point among the plurality of trajectory points and a preset reference position; a contact force of at least one trajectory point of the plurality of trajectory points; A joint angular velocity of at least one trajectory point among the plurality of trajectory points.

3. The motion control method according to claim 1 or 2, characterized in that: The step of optimizing the state variables of the trajectory points among the plurality of trajectory points to obtain the optimization results of the state variables of the trajectory points includes: The state variables of the trajectory points among the plurality of trajectory points are optimized under the constraint of a first constraint condition to obtain an optimization result of the state variables of the trajectory points, wherein the first constraint condition is related to the action sequence.

4. The motion control method according to claim 3, characterized in that: The first constraint condition includes at least one of the following: The state variables of each trajectory point conform to the dynamic model obtained from the center-of-mass dynamics and full kinematics; The state variables of adjacent trajectory points satisfy the integral continuity; The state variables of each trajectory point meet the relevant limit conditions; The contact point with the ground has no relative motion with the ground, the supporting force between the contact point and the ground is not less than 0, and the friction between the contact point and the ground is not greater than the maximum static sliding friction; The supporting force between the contact point without contact with the ground and the ground is 0, and the speed meets the speed requirement of the preset reference trajectory.

5. The motion control method according to claim 4, characterized in that: Under the constraint of the first constraint condition, optimizing the state variables of the trajectory points among the plurality of trajectory points by using the cost function to obtain the state variable optimization result of the trajectory point includes: Under the constraint of the linearization processing result of the first constraint condition, the state variables of the trajectory points among the plurality of trajectory points are optimized using the quadratic approximation result of the cost function to obtain the state variable optimization result of the trajectory point.

6. The motion control method according to claim 5, characterized in that: The method further comprises: A slack variable is added to the first constraint condition to perform linearization processing on the first constraint condition.

7. The motion control method according to claim 1, characterized in that: The state variables include at least one of the following: a momentum vector, a position vector, a contact point vector, and a joint velocity vector, wherein the momentum vector includes linear momentum and nonlinear momentum, the position vector includes a floating base position and a joint position, the floating base position includes a coordinate position and an attitude angle in the form of a unit quaternion, the contact point vector includes an external force on the contact point, and the joint velocity vector includes the velocity of the joint.

8. The motion control method according to claim 1, characterized in that: The step of controlling the robot to execute the target motion according to the state variable optimization result of the trajectory point comprises: Tracking at least one slack variable under a second constraint condition to obtain an optimization result of the optimization variable, wherein at least one condition in the second constraint condition is related to the at least one slack variable, at least one condition in the second constraint condition is related to the optimization variable, and at least one condition in the second constraint condition is related to the optimization result of the state variable; According to the optimization result of the optimization variable, the robot is controlled to execute the target motion.

9. The motion control method according to claim 8, characterized in that: The optimization variables include an acceleration vector, a contact point vector and a relaxation vector, wherein the acceleration vector includes the acceleration of the joint, the contact point vector includes the external force exerted on the contact point, and the relaxation vector includes the at least one relaxation variable.

10. The motion control method according to claim 8, characterized in that: The second constraint condition includes at least one of the following: Floating base dynamics consistency; The contact point with the ground has no relative motion with the ground; The friction between the contact point with the ground and the ground is not greater than the maximum static sliding friction; The contact point not in contact with the ground tracks the position and velocity in the state variable optimization result under the relaxation effect of the first relaxation variable; The floating base of the robot tracks the floating base posture in the state variable optimization result under the relaxation effect of the second relaxation variable; The arm joints of the robot track the joint positions and velocities in the state variable optimization results under the relaxation effect of the third relaxation variable; The contact point of the robot tracks the contact point vector in the state variable optimization result under the relaxation effect of the fourth relaxation variable.

11. A motion control device, characterized in that: The device comprises: A discretization module, used for discretizing the trajectory of the target movement into a plurality of trajectory points according to an action sequence of the target movement, wherein the action sequence includes a plurality of action stages, and there is at least one contact point with a different contact state between different action stages, and the trajectory point has a state variable; an optimization module, configured to optimize the state variables of the trajectory points among the plurality of trajectory points to obtain optimization results of the state variables of the trajectory points, wherein the optimization direction of the state variables includes: making the trajectory of the target motion conform to the action sequence; The control module is used to control the robot to execute the target motion according to the state variable optimization result of the trajectory point.

12. 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 10 when executing the computer instructions.

13. 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 10 is implemented.

Images