Footed robot long time domain and reactivity combined motion trajectory generation method

Abstract

The application discloses a long-time-domain and reaction type combined motion trajectory generation method of a foot-type robot, which comprises the following steps: establishing an equivalent single-particle model of the robot; establishing an equivalent single-particle model cost function based on a center-of-mass acceleration vector and a foot position vector; and establishing a spatial constraint equation of a system state; then, a nonlinear optimization equation is constituted, an optimization solver is used to generate a long-time-domain center-of-mass trajectory and a foot sequence from a starting time to a termination time; a model prediction control and a velocity field guided trajectory are combined to converge the short-time-domain reaction type trajectory to the long-time-domain motion trajectory, and a global motion trajectory is generated. The motion trajectory generation method considers a motion target position, robot kinematics / dynamics and environmental constraints, allows simultaneous foot and center-of-mass trajectory optimization, and still can quickly converge to an expected position when a center-of-mass position deviates from a long-time-domain motion trajectory due to external disturbance, and has the characteristics of strong anti-disturbance ability.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of foot-type robot motion control, in particular to a long-time domain and reactive combined motion trajectory generation method for foot-type robot. BACKGROUND

[0002] As an important branch of mobile robots, foot-type robots are divided into single-foot, double-foot and multi-foot robots. The foot points of foot-type robots are discrete rather than continuous, and the position planning of the foot points has great relevance to the centroid trajectory, which determines the motion flexibility and terrain adaptability of foot-type robots. Therefore, foot-type robots are widely used in complex terrain rescue and disaster relief, cargo transportation and other fields.

[0003] However, due to the complex mechanical structure and high-dimensional system dynamics, it is difficult to generate the motion trajectory of the foot-type robot. The under-actuated characteristics of the floating base dynamics itself cannot directly control the centroid trajectory of the foot-type robot, but is affected by the ground reaction force and the center of pressure interacting with the environment. How to make the foot-type robot reach the target position with the optimal trajectory, and when the foot-type robot is disturbed by external force, the foot points and the centroid trajectory can be quickly adjusted and converged to the long-time domain optimal trajectory at a faster speed. In addition, foot-type robots, especially single-foot, double-foot and four-foot robots, often have walking and jumping gaits and need to switch gaits on different terrains. The above problems pose some challenges to the motion trajectory generation method of foot-type robots. Based on this, the present application proposes a long-time domain and reactive combined motion trajectory generation method for foot-type robots to optimize the centroid trajectory and foot point position at the same time. SUMMARY

[0004] Based on the unified model and nonlinear optimization equation construction, the method is used in single-foot, double-foot and multi-foot robots, considering the influence of robot kinematics and dynamics, target position and environmental constraints such as obstacles and terrain, and a long-time domain and reactive motion trajectory generation method for simultaneously optimizing the centroid trajectory and foot point is designed. This method combines model predictive control and velocity field guidance, which can ensure that the reactive trajectory of the foot-type robot converges to the long-time domain motion trajectory, and has strong robustness to uncertainty and external disturbance. In addition, since the method introduces the centroid acceleration constraint into the trajectory generation optimization problem, the reactive trajectory is converged to the long-time domain motion trajectory through model predictive control, thereby generating a global motion trajectory.

[0005] To achieve the above purpose, the solution adopted by the present application is as follows: the present application discloses a long-time domain and reactive combined motion trajectory generation method for foot-type robot, which comprises the following steps:

[0006] Step 1, establish the equivalent single-particle model of the foot-type robot, and analyze the motion characteristics based on the foot landing position and single-particle dynamics;

[0007] Based on the motion characteristics of the foot-type robot, the equivalent single-particle model is extracted, the robot motion is simulated using the equivalent single-particle model, the geometric center position coordinate value of the foot landing point is taken as the expected value of the pressure center according to the relationship between the foot landing position and the pressure center, and a pressure center constraint domain based on support deformation is established; the relationship between the center of mass trajectory and the pressure center is established based on the relationship between the pressure center, the ground contact force and the center of mass acceleration;

[0008] Step 2, through the analysis of the motion characteristics of the equivalent single-particle model, an equivalent single-particle model cost function based on the center of mass acceleration vector and the foot landing position vector is established, which is expressed as:

[0009]

[0010] In the formula: J U(t) is the cost function; t0 and t F respectively represent the start time and end time of the entire motion time domain; ω U is the weight coefficient; f(U(t)) and respectively represent the predicted performance index and the expected performance index;

[0011] Wherein, the expression of the to-be-optimized variable U(t) is as follows:

[0012]

[0013] In the formula: is the acceleration vector of the center of mass, p f (t) is the foot landing position vector;

[0014] Step 3, based on the step length, step height and end point center of mass state in the motion characteristics of the foot-type robot, the pressure center and ground contact force vector in the dynamics characteristics, the acceleration, terrain and obstacle avoidance constraints in different gaits in walking, jumping and gait conversion, the spatial constraint equation of the system state is established;

[0015] Based on the system state, the spatial constraint equation of the to-be-optimized variable is established, which is expressed as:

[0016]

[0017] In the formula: U upp and U low are the maximum and minimum values of the to-be-optimized variable U(t) at each system state, respectively;

[0018] Step 4, combine the cost function and the spatial constraint equation to form a nonlinear optimization equation, and use the motion optimization solver to generate the long-time domain centroid trajectory and the landing point sequence from the start point to the end point:

[0019] According to the equivalent single-particle model cost function of step 2, combined with the constraint equation of step 3, the nonlinear optimization equation of the legged robot is formed as:

[0020]

[0021]

[0022] Based on the nonlinear optimization equation of the legged robot, the long-time domain centroid trajectory and the landing point sequence from the start time to the end time are generated;

[0023] Step 5, combine model predictive control with velocity field guided trajectory convergence to converge the short-time domain reactive trajectory to the long-time domain motion trajectory, and generate the global motion trajectory:

[0024] Considering the real-time nature of short-time domain reactive trajectory optimization, the constructed nonlinear optimization equation of the legged robot is used for iterative optimization. The short-time domain local optimal centroid trajectory and landing point sequence starting from the current time deviate from the expected trajectory when encountering external disturbances. In order to ensure that the short-time domain motion trajectory converges to the long-time domain motion trajectory, the velocity field is used to generate the expected combined velocity sequence in the short-time domain, and the model predictive control is used to converge the reactive trajectory to the long-time domain motion trajectory, thereby generating the global motion trajectory.

[0025] Further, the equivalent single-particle model cost function in step 2 includes:

[0026] The legged robot tracks the user input speed and acceleration command, and the centroid state cost function is:

[0027]

[0028] where t0 and t F represent the start time and end time of the entire motion time domain, respectively; is the weight coefficient of the centroid state cost function; c a,v and represent the predicted and expected centroid states, respectively; a, v represent the robot centroid acceleration and velocity states, respectively;

[0029] The landing point position cost function is:

[0030]

[0031] where ω pis the weight coefficient of the foothold position; r(t) represents the position vector of the foothold position and the center of mass; f(t) represents the foot-ground contact force vector from the pressure center to the center of mass;

[0032] The centroid height cost function is:

[0033]

[0034] Where: ω z is the weight coefficient of the centroid height; r z (t) represents the actual center of mass height of the legged robot; Indicates the desired center of mass height of the legged robot.

[0035] Furthermore, the spatial constraint equations in step 3 include:

[0036] According to the gait and structure of the legged robot, the center of mass constraint equation of the legged robot is established as follows:

[0037]

[0038] Where: c v,p (t) represents the actual velocity and position state of the center of mass; Indicates the upper limit of the center of mass velocity and position state constraints; Indicates the lower limit of the velocity and position state of the center of mass; the superscripts v and p respectively indicate the velocity and position state of the center of mass;

[0039] According to the length of the robot leg rod, the robot leg length constraint equation is established as:

[0040]

[0041] Where: ||r(t)|| represents the modulus of the vector from the foothold position to the center of mass position; Indicates the upper limit of the square of the magnitude of the vector from the foothold position to the center of mass position; The lower limit of the square of the vector from the foothold position to the center of mass position;

[0042] According to the limitations of the robot's leg length and stride length, the robot's stride length constraint equation is established as:

[0043]

[0044] Where: p f,xy Indicates the x and y coordinates of the landing point; Indicates the upper limit of stride constraint; represents the lower limit of the stride constraint; T step Indicates the time period of stepping;

[0045] The position of the foot point keeps unchanged in each step time period as:

[0046] p f (t)≡C,t∈[t step0 ,t step0 +T step ]

[0047] wherein t step0 denotes the starting time of each step time period;

[0048] According to the contact sequence of the foot-type robot walking, the conservative constraint equation of the position of the pressure center is established, specifically as follows:

[0049]

[0050] wherein p p,xy (t) denotes the coordinates of the position of the pressure center in the x and y directions; p f,xy (t) denotes the coordinates of the current foot point geometric center position in the x and y directions; denotes the upper limit of the distance constraint between the position of the pressure center and the current foot point geometric center position; denotes the lower limit of the distance constraint between the position of the pressure center and the current foot point geometric center position;

[0051] According to the walking and jumping gaits of the foot-type robot and the conversion between the gaits, the centroid acceleration constraint equation of the foot-type robot is established, specifically as follows:

[0052]

[0053] wherein c a (t) denotes the actual acceleration state of the centroid; denotes the upper limit of the acceleration state constraint of the centroid; denotes the lower limit of the acceleration state constraint of the centroid; the superscript a denotes the acceleration of the centroid;

[0054] According to the terrain foot-ground contact restriction, the foot point position constraint equation of the robot is established, specifically as follows:

[0055] p f,z (t)=f terr (p f,x (t),p f,y (t))

[0056] wherein p f,z (t) denotes the z-direction coordinate of the foot point position; p f,x (t) denotes the x-direction coordinate of the foot point position; p f,y (t) denotes the y-direction coordinate of the foot point position; f terr () denotes the ground model function;

[0057] According to the cylindrical envelope of the obstacle, the foot robot landing point position obstacle avoidance and centroid motion trajectory obstacle avoidance constraint equations are established, specifically as follows:

[0058]

[0059]

[0060] In the formula: r obs is the radius of the cylinder, r sec is the obstacle avoidance safety threshold radius; P f,x represents the x-direction landing point position; P f,y represents the y-direction landing point position; represents the x-direction obstacle origin position; represents the y-direction obstacle origin position.

[0061] Preferably, when the foot robot is in line contact with the ground, the pressure center position is located on the contact line formed by the foot-ground line contact, that is, the cross product of the landing point position to centroid position vector and the foot-ground contact force vector of the pressure center position to centroid position, and the pressure center position constraint equation is as follows:

[0062] [r(t)×f(t)]·r 线接触 (t)=0

[0063] In the formula: r(t) represents the landing point position to centroid position vector, f(t) represents the foot-ground contact force vector of the pressure center position to centroid position, r 线接触 (t) represents the vector formed by the foot-ground line contact; wherein:

[0064]

[0065] In the formula: p1 represents the first end point of the foot-ground line contact; and p2 represents the second end point of the foot-ground line contact.

[0066] In a further preferred embodiment, when the robot jumping gait is in the flight phase, the centroid acceleration constraint equation is as follows:

[0067]

[0068] In the formula: [0 0 -g] T represents the gravity acceleration vector.

[0069] In a preferred embodiment, according to the set foot robot motion target speed, the final time centroid velocity constraint equation is established, specifically as follows:

[0070]

[0071] In the formula: represents the center of mass velocity at the final time; represents the upper limit of the center of mass velocity at the final time; represents the lower limit of the center of mass velocity at the final time;

[0072] According to the set motion target position of the legged robot, the final time center of mass position constraint equation is established, specifically:

[0073]

[0074] In the formula: c xy (t F ) represents the x, y direction center of mass position coordinates at the final time; represents the upper limit of the x, y direction center of mass position at the final time; represents the lower limit of the x, y direction center of mass position at the final time.

[0075] Preferably, the expected combined velocity in step 5 is specifically:

[0076]

[0077] In the formula: represents the center of mass expected combined velocity at time k; represents the long-term expected velocity at time k; represents the convergence velocity of the long-term motion trajectory at time k;

[0078]

[0079] In the formula: v c represents the maximum safe convergence velocity; ε k represents the perpendicular distance between the center of mass at time k and the long-term motion trajectory; d represents the maximum deviation distance threshold of the convergence space; κ≥1 represents the velocity field attraction coefficient.

[0080] Further, in the velocity field guidance of step 5, the expected combined velocity sequence for model predictive control expected velocity tracking is specifically:

[0081] The future convergence velocity sequence of each loop is calculated by using an iterative algorithm:

[0082]

[0083] In the formula: represents the short-term range;

[0084] The expected combined velocity sequence is:

[0085]

[0086] Compared with the prior art, the application has the beneficial effects that:

[0087] (1) The motion trajectory generation method can be used for single-foot, double-foot and multi-foot robots, and can realize walking gait, jumping gait and gait conversion between walking and jumping gaits according to a terrain model due to consideration of terrain constraints and flight constraints.

[0088] (2) The motion trajectory generation method considers motion target position, robot kinematics / dynamics and environmental constraints, allows simultaneous foot placement and center of mass trajectory optimization, combines the advantages of model predictive control and velocity field guidance method, and can still quickly converge to the expected position when the center of mass position and long-time-domain motion trajectory are deviated due to external disturbance, and has the characteristics of strong anti-disturbance ability.

[0089] (3) Compared with the traditional motion trajectory generation method based on a linear inverted pendulum model, the method adopts a unified single-particle model, can generate more natural and adaptive gaits such as walking, jumping and gait conversion, and is more in line with actual working conditions.

[0090] (4) The application considers the gravitational acceleration in the flight and landing phases, establishes upper and lower boundaries of the center of mass acceleration constraint, and limits the center of mass acceleration constraint in the flight phase to the gravitational acceleration, so that the motion planning of the walking gait and the jumping gait can be realized at the same time, and the conversion between the walking gait and the jumping gait can be realized. BRIEF DESCRIPTION OF DRAWINGS

[0091] Figure 1 The flowchart of the long-time-domain and reactive motion trajectory generation method of the leg part of the embodiment of the application;

[0092] Figure 2 The schematic diagram of the equivalent single-particle model established by the application;

[0093] Fig. 3a-3b are schematic diagrams of the walking and advancing states of the equivalent single-particle model of the application;

[0094] Figures 4a-4d The schematic diagram of the center of pressure constraint of the four-foot and two-foot robot gaits in one specific embodiment of the application;

[0095] Figure 5 The schematic diagram of the convergence trajectory and the expected combined velocity after the center of mass deviates from the long-time-domain trajectory in one specific embodiment of the application;

[0096] Figure 6 The schematic diagram of the center of mass trajectory convergence field of the velocity field guidance in one specific embodiment of the application;

[0097] Figure 7Fig. 1 is a schematic diagram of walking and jumping gait transition in an embodiment of the present application;

[0098] Figure 8 Fig. 4 is a walking gait trajectory generated in a rugged terrain and obstacle environment in an embodiment of the present application. DETAILED DESCRIPTION

[0099] Embodiments of the present application will be described below with reference to the accompanying drawings.

[0100] The motion trajectory generation method of the foot-type robot proposed in the present application can be used for single-foot, double-foot and multi-foot robots. Meanwhile, due to the consideration of terrain constraints and flight constraints, the motion trajectory generation method can realize walking gait, jumping gait and gait transition between walking and jumping gaits according to terrain models. The motion trajectory generation method proposed in the embodiment of the present application considers the motion target position, robot kinematics / kinetics and environmental constraints, allows simultaneous optimization of footfall points and center of mass trajectories, and combines the advantages of model predictive control and velocity field guidance method. When the center of mass position and long-time domain motion trajectory deviate due to external disturbances, the method can still quickly converge to the expected position, and has the characteristics of strong anti-disturbance ability. Figure 1 Fig. 1 is a schematic diagram of walking and jumping gait transition in an embodiment of the present application;

[0101] The embodiment of the present application provides a motion trajectory generation method of a foot-type robot combining long-time domain and reaction. In order to prove the applicability of the present application, the present application is applied to an example, which specifically includes the following steps:

[0102] S1: simplify the foot-type robot into an equivalent single-particle model, as shown in Fig. 1; Figure 2 Fig. 1 is a schematic diagram of a simplified general equivalent single-particle model, which analyzes the motion characteristics based on the footfall position and single-particle dynamics model; Figure 2 wherein A represents the center of mass motion reachable space, and B represents the center of pressure constraint space;

[0103] Based on the motion characteristics of single-foot, double-foot and multi-foot robots, the equivalent single-particle model is abstracted and extracted, the robot motion is simulated by using the equivalent single-particle model, the geometric center position of the footfall point is taken as the expected value of the center of pressure based on the mutual relationship between the footfall position and the center of pressure, and a center of pressure constraint domain based on support deformation is established; the relationship between the center of mass trajectory and the center of pressure is established based on the relationship among the center of pressure, the foot-ground contact force and the center of mass acceleration;

[0104] S2: Through the analysis of the motion characteristics of the single point model, as shown in FIG. 3a-3b are single point model walking forward diagram, the virtual leg in FIG. 3 is equivalent to human leg, when a person is walking, one is the landing leg, and one is the virtual leg, when the landing leg steps on the ground, the other leg is the leg that lifts and steps; and the connecting dotted line of the position of the mass center c represents the moving track of the mass center. According to this, the equivalent single point model cost function based on the mass center acceleration vector and the foot position vector is established, and the general form can be expressed as: FIG. 3a is the state and motion track of the virtual leg and the landing leg at the first step, the left side in FIG. 3a is the state at the start, and the right side in FIG. 3a is the state before the foot-ground contact switches; FIG. 3b is the motion track from the first step to the stop, the left side in FIG. 3b is the state after the foot-ground contact switches, and the right side in FIG. 3b is the state at the stop):

[0105]

[0106] In the formula: J U(t) is the cost function; t0 and t F respectively represent the start time and the end time of the whole motion time domain; ω U is the weight coefficient; f(U(t)) and respectively represent the predicted performance and the expected performance;

[0107] In the formula, the expression of the to-be-optimized variable is as follows:

[0108]

[0109] In the formula: is the acceleration vector of the mass center, p f (t) is the foot position vector;

[0110] S3: Based on the step length, step height, end mass center state in the kinematics of the legged robot, the pressure center and the foot-ground contact force vector in the dynamics, the take-off / landing acceleration, terrain and obstacle avoidance constraints in different gaits such as walking, jumping and gait conversion, the spatial constraint equation is established;

[0111] The constraint equation is established based on the system state for the nonlinear optimization algorithm, and the system state constraint space can be expressed as:

[0112]

[0113] In the formula: U upp and U low are respectively the maximum value and the minimum value of each system state;

[0114] S4: Combine the cost function and constraint equation to form a nonlinear optimization equation, and use the motion optimization solver to generate the long-time domain centroid trajectory and landing point sequence from the starting point to the ending point, and generate the global motion trajectory:

[0115] According to the equivalent single particle model cost function and the constraint equation, the two are combined to form a nonlinear optimization equation of the legged robot, and the optimization problem is as follows:

[0116]

[0117]

[0118] Based on the optimization problem, the long-time domain centroid trajectory and landing point sequence from the starting time to the ending time are generated;

[0119] S5: Combine model predictive control and velocity field guided trajectory convergence to build a unified motion trajectory generation method that optimizes short-time domain and converges long-time domain trajectory:

[0120] Using the real-time short-time domain optimization of the model predictive control method, the short-time domain local optimal centroid trajectory and landing point sequence starting from the current time are iteratively optimized using the constructed optimization problem. When external disturbances cause the centroid state to deviate from the expected trajectory, in order to ensure that the short-time domain motion trajectory converges to the long-time domain motion trajectory, a velocity field guiding method is used to generate an expected velocity sequence in the short-time domain, and the reactive trajectory is converged to the long-time domain motion trajectory through model predictive control.

[0121] The equivalent single particle model cost function is as follows:

[0122] In order to make the legged robot track the user input speed and acceleration command as much as possible, the centroid state cost function is as follows:

[0123]

[0124] In the formula: t0 and t F represent the start time and end time of the entire motion task respectively; is the weight coefficient of the centroid state cost function; c a,v (t) and represent the predicted and expected centroid states respectively; a, v represent the robot centroid acceleration and velocity states respectively;

[0125] In order to ensure the stability of the robot motion and reduce the influence of GRF on the system angular momentum, the CoP should be as close to the landing point as possible, and the landing point position cost function is as follows:

[0126]

[0127] In the formula: ωp is the weight coefficient of the foot placement position; r(t) represents the position vector of the foot placement position and the center of mass; f(t) represents the foot-ground contact force vector from the center of pressure position to the center of mass position;

[0128] For most legged robots without effective energy storage components, significant fluctuations in the height of the center of mass will lead to motion instability, significant fluctuations in foot-ground contact forces, and increased energy consumption. To reduce fluctuations in the height of the center of mass, a center of mass height cost function is used, specifically:

[0129]

[0130] where: ω z is the weight coefficient of the height of the center of mass; r z (t) represents the actual height of the center of mass of the legged robot; represents the desired height of the center of mass of the legged robot.

[0131] In a preferred embodiment, based on the structure of the legged robot, gait, and environmental terrain and obstacles, the spatial constraint equation in step 3 is specifically:

[0132] According to the gait and structural constraints of the legged robot, the center of mass constraint equation of the legged robot is established, specifically:

[0133]

[0134] where: c v,p (t) represents the actual velocity and position state of the center of mass; represents the upper limit of the velocity and position state constraint of the center of mass; represents the lower limit of the velocity and position state constraint of the center of mass; superscripts v and p represent the velocity and position state of the center of mass, respectively;

[0135] According to the leg length of the robot, the leg length constraint equation of the robot is established, specifically:

[0136]

[0137] where: ||r(t)|| represents the modulus of the position vector from the foot placement position to the center of mass; represents the upper limit of the modulus square constraint of the position vector from the foot placement position to the center of mass; represents the lower limit of the modulus square constraint of the position vector from the foot placement position to the center of mass;

[0138] According to the leg length and stride length constraints of the robot, the stride length constraint equation of the robot is established, specifically:

[0139]

[0140] where: p f,xycoordinates of the stance position in x, y directions; represents the upper limit of the step constraint; represents the lower limit of the step constraint; step represents the step time period;

[0141] In each step time period, the position of the stance point remains unchanged, specifically:

[0142] p f (t)≡C,t∈[t step0 , t step0 +T step ]

[0143] In the formula: t step0 represents the starting time of each step time period;

[0144] The center of pressure constraint of the robot, similar to the contact sequence of biped walking, the diagonal trot of the quadruped robot is an effective gait among various gaits; the center of pressure constraint of the quadruped trot and biped motion is shown in Figures 4a-4d , wherein Figures 4a-4d the letter B in the formula represents the center of pressure constraint space; analysis of the walking process from the equivalent single particle model shows that the center of pressure cannot be too far away from the geometric center of the relevant stance point, because the center of pressure cannot be outside the support convex polygon formed by the supporting foot, and the equivalent single particle model does not take into account the momentum generated by the dynamics of the leg robot; the greater the distance between the center of pressure and the current stance point, the less the model matches; according to the green rectangle in Figures 4a-4d , a conservative constraint equation for the position of the center of pressure of the robot is established, specifically:

[0145]

[0146] In the formula: p p,xy (t) represents the coordinates of the x, y directions of the center of pressure position; p f,xy (t) represents the coordinates of the x, y directions of the geometric center position of the current stance point; represents the upper limit of the distance constraint between the center of pressure position and the geometric center position of the current stance point; represents the lower limit of the distance constraint between the center of pressure position and the geometric center position of the current stance point;

[0147] If the foot-type robot is in line contact with the ground, as shown in the quadruped robot diagonal gait in Figures 4a-4d , the center of pressure is in line contact with the ground, and the position of the center of pressure should be located on the contact line formed by the diagonally landing feet, that is, the cross product of the foot-ground contact force vector from the stance point position to the center of mass position and the foot-ground contact force vector from the center of mass position to the center of pressure position is the normal vector of the plane formed by the two vectors, and the contact line is orthogonal to the normal vector, so the center of pressure position constraint equation is:

[0148] [r(t) x f(t)] cdot r 线接触 (t) = 0

[0149] where: r(t) represents the vector from the foot position to the center of mass position, f(t) represents the vector of the ground contact force from the center of pressure position to the center of mass position, r 线接触 (t) represents the vector formed by the ground line contact; where:

[0150]

[0151] where: p1 represents the first end point of the ground line contact; p2 represents the second end point of the ground line contact;

[0152] Even if the r(t) vector and the f(t) vector coincide, the above equation still holds.

[0153] According to the walking, jumping gait and the transition between gaits of the legged robot, the center of mass acceleration constraint equation of the legged robot is established, specifically:

[0154]

[0155] where: c a (t) represents the actual acceleration state of the center of mass; represents the upper limit of the constraint of the center of mass acceleration state; represents the lower limit of the constraint of the center of mass acceleration state; the superscript a represents the center of mass acceleration; if it is a flight phase, the center of mass acceleration constraint equation is:

[0156]

[0157] where: [0 0 -g] T represents the gravity acceleration vector;

[0158] According to the terrain ground contact constraint, the foot position constraint equation of the robot is established, specifically:

[0159] p f,z (t) = f terr (p f,x (t), p f,y (t))

[0160] where: p f,z (t) represents the z-direction coordinate of the foot position; p f,x (t) represents the x-direction coordinate of the foot position; p f,y (t) represents the y-direction coordinate of the foot position; f terr () represents the ground model function;

[0161] According to the cylindrical envelope of the obstacle, the foot robot landing point position obstacle avoidance and centroid motion trajectory constraint equation is established, specifically:

[0162]

[0163]

[0164] In the formula: r obs is the radius of the cylinder, r sec is the obstacle avoidance safety threshold radius; P f,x represents the x direction landing point position; P f,y represents the y direction landing point position; represents the x direction obstacle origin position; represents the y direction obstacle origin position.

[0165] According to the foot robot motion target speed requirement, the final time centroid speed constraint equation is established, specifically:

[0166]

[0167] In the formula: represents the final time centroid speed; represents the upper limit of the final time centroid speed; represents the lower limit of the final time centroid speed.

[0168] In a special case, the centroid speed of the final target position should be equal to 0, that is However, ensuring that the centroid speed of the final target position is equal to zero will greatly reduce the range of feasible solutions; in addition, due to additional external disturbances and limited pressure center and foot landing point constraints, the optimization solving process will be extremely time-consuming, and even no solution will appear; in actual situations, there is a small non-zero final speed in the optimization problem, which is reasonable, and the centroid position can be forced to reduce the small non-zero speed to 0 by the landing foot. Therefore, the centroid speed constraint of the final target position can be appropriately expanded, the feasible range of the optimal solution is expanded, and the calculation amount is reduced.

[0169] According to the foot robot motion target position requirement, the final time centroid position constraint equation is established, specifically:

[0170]

[0171] In the formula: c xy (t F ) represents the x, y direction centroid position coordinates at the final time; represents the upper limit of the x, y direction centroid position at the final time; represents the lower limit of the x, y direction centroid position at the final time.

[0172] Likewise, if there is no strict limit to the final target position, the boundary of the final target position can be expanded. This application establishes a constraint condition for long-time domain optimization, in order to solve the continuous-time optimization problem by numerical method, the continuous-time optimization variable needs to be discretized into a finite number of variables, and combined with the constraint equation of the robot landing point, the discrete-time optimization variable can be expressed as:

[0173]

[0174] In the formula, T is the sampling interval of implementation; in the discrete-time optimization problem, the number of input steps and the duration T step , the number of samples is fixed.

[0175] As Figure 5 shown is the convergence trajectory after the actual mass center state deviates from the long-time domain motion trajectory and the expected combined velocity diagram, wherein Figure 5 the symbol D in the formula represents the long-time domain motion trajectory; E represents the reaction formula motion trajectory; F represents a trajectory parallel to the long-time domain motion trajectory D, and the expected combined velocity is:

[0176]

[0177] In the formula: represents the expected combined velocity of the mass center at time k; represents the long-time domain expected velocity at time k; represents the convergence velocity of the mass center at time k pointing to the long-time domain motion trajectory;

[0178]

[0179] In the formula: ε k represents the vertical distance between the mass center and the long-time domain motion trajectory at time k; d represents the maximum deviation distance threshold of the convergence space; κ≥1 represents the velocity field attraction coefficient; v c represents the maximum safe convergence velocity, which can be represented by a constant value:

[0180] v C ≡C

[0181] The velocity field guiding method is used for the expected combined velocity sequence of the model predictive control expected velocity tracking, specifically:

[0182] The iterative algorithm is used to calculate the future convergence velocity sequence of each cycle as:

[0183]

[0184] In the formula: represents a short time domain range; in combination with a desired combined velocity expression, a desired combined velocity sequence is:

[0185]

[0186] On the basis that the offline pre-planning can generate an optimal solution in a long time domain, online reactive motion planning is performed. The present application combines model predictive control and velocity field guidance to realize a reactive motion planner that can track the long time domain optimal solution as closely as possible while meeting current constraints. As shown in Figure 6 is a centroid convergence trajectory obtained by tracking the velocity of the desired combined velocity sequence with the red centroid deviation position as the starting point.

[0187] The long time domain and reactive motion trajectory generation method uses a unified nonlinear optimization equation. The motion trajectory generation method is used for a step terrain, and a legged robot uses a walking gait and a jumping gait to generate a motion trajectory for testing. As shown in Figure 7 The centroid trajectory and footfall position obtained by using the motion trajectory generation method of the present application can realize the legged robot to go up and down the steps, and to switch gaits from a walking gait to a jumping gait and then to a walking gait according to the step position. In addition, the motion trajectory generation method is used in a rugged terrain environment with multiple obstacles.

[0188] The following describes the legged robot motion trajectory generation method of the present application in a specific embodiment, which includes the following steps:

[0189] Step 1, an equivalent single mass point model of a quadruped robot is established, and motion characteristics are analyzed based on footfall position and single mass point dynamics;

[0190] Based on the motion characteristics of the diagonal trot of the quadruped robot, an equivalent single mass point model is extracted, and the robot motion is simulated using the equivalent single mass point model. The geometric center position coordinate value of the footfall point is taken as the expected value of the center of pressure, and a constraint domain of the center of pressure based on the diagonal line of the support is established. The relationship between the center of pressure, the ground contact force and the centroid acceleration is used to establish the relationship between the centroid trajectory and the center of pressure;

[0191] Step 2, through the analysis of the motion characteristics of the equivalent single mass point model, an equivalent single mass point model cost function based on the centroid acceleration vector and the footfall position vector is established, which is represented as:

[0192]

[0193] In the formula: J U(t) is the cost function; t0 and t Frespectively represent the start time and the end time of the entire motion time domain; ω U is a weight coefficient; f(U(t)) and respectively represent the predicted performance index and the expected performance index;

[0194] wherein the to-be-optimized variable U(t) is expressed as follows:

[0195]

[0196] In the formula: is the acceleration vector of the center of mass, p f (t) is the foot position vector;

[0197] Step 3: Based on the step length, step height and end point center of mass state in the motion characteristics of the legged robot, the pressure center and foot-ground contact force vector in the dynamics characteristics, the acceleration, terrain and obstacle avoidance constraints in different gaits in walking, jumping and gait conversion, the spatial constraint equation of the system state is established;

[0198] Based on the system state, the spatial constraint equation of the to-be-optimized variable is established and expressed as:

[0199]

[0200] In the formula: U upp and U low are the maximum value and the minimum value of the to-be-optimized variable U(t) at each system state respectively;

[0201] Step 4: The cost function and the spatial constraint equation are combined to form a nonlinear optimization equation, and a motion optimization solver is used to generate the long-time domain center of mass trajectory and foot sequence from the start point to the end point:

[0202] According to the equivalent single particle model cost function of step 2, combined with the constraint equation of step 3, the nonlinear optimization equation of the legged robot is formed as:

[0203]

[0204]

[0205] Based on the nonlinear optimization equation of the legged robot, the long-time domain (0-15s) center of mass trajectory and foot sequence from the start time to the end time are generated;

[0206] Step 5: The short-time domain (1.5s) reactive trajectory is converged to the long-time domain motion trajectory by combining the model predictive control and the velocity field guided trajectory convergence, and the global motion trajectory is generated:

[0207] In combination with the real-time of short-time reactive trajectory optimization, the nonlinear optimization equation of the foot-type robot is iteratively optimized to generate the short-time local optimal centroid trajectory and the footfall sequence starting from the current time. When the centroid state deviates from the expected trajectory due to external disturbances, the velocity field is used to guide the generation of the expected combined velocity sequence in the short-time domain. The model predictive control is used to converge the reactive trajectory to the long-time motion trajectory, thereby generating the global motion trajectory.

[0208] The foot-type robot tracks the user input speed command (0.4 m / s) and the acceleration command (0 m / s 2 The centroid state cost function is specifically as follows:

[0209]

[0210] wherein t0 and t F respectively represent the start time and the end time of the entire motion time domain; is the weight coefficient of the centroid state cost function; c a,v (t) and respectively represent the predicted and expected centroid states; a and v respectively represent the robot centroid acceleration and velocity states;

[0211] The footfall position cost function is specifically as follows:

[0212]

[0213] wherein ω p is the weight coefficient of the footfall position; r(t) represents the position vector of the footfall position and the centroid; and f(t) represents the foot-ground contact force vector from the pressure center position to the centroid position.

[0214] The centroid height cost function is specifically as follows:

[0215]

[0216] wherein ω z is the weight coefficient of the centroid height; r z (t) represents the actual centroid height of the foot-type robot; represents the expected centroid height of the foot-type robot.

[0217] The spatial constraint equation includes: according to the gait and structure of the foot-type robot, the centroid constraint equation of the foot-type robot is established as follows:

[0218]

[0219] wherein c v,p (t) represents the actual velocity and position state of the centroid; represents the upper limit of the centroid velocity and position state constraint; represents the lower limit of the centroid velocity and position state constraint;

[0220] According to the length of the robot leg, the robot leg length constraint equation is established as:

[0221]

[0222] In the formula: ||r(t)|| represents the modulus of the foot position to the centroid position vector; 0.7 2 represents the upper limit of the modulus square of the foot position to the centroid position vector; 0.4 2 represents the lower limit of the modulus square of the foot position to the centroid position vector;

[0223] According to the length of the robot leg and the stride limit, the robot stride constraint equation is established as:

[0224]

[0225] In the formula: p f,xy represents the x, y direction coordinates of the foot position; represents the upper limit of the stride constraint; represents the lower limit of the stride constraint; T step =0.5s represents the step time period;

[0226] In each step time period, the position of the foot remains unchanged as:

[0227] p f (t)≡C,t∈[t step0 ,t step0 +T step ]

[0228] In the formula: t step0 represents the start time of each step time period;

[0229] According to the contact sequence of the legged robot walking, the conservative constraint equation of the center of pressure position is established, which is specifically:

[0230]

[0231] In the formula: p p,xy (t) represents the x, y direction coordinates of the center of pressure position; p f,xy (t) represents the x, y direction coordinates of the current foot geometric center position; represents the upper limit of the distance constraint between the center of pressure position and the current foot geometric center position; represents the lower limit of the distance constraint between the center of pressure position and the current foot geometric center position;

[0232] According to the walking gait of the legged robot, the acceleration constraint equation of the center of mass of the legged robot is established, which is:

[0233]

[0234] Where: c a (t) represents the actual acceleration state of the center of mass; Indicates the upper limit of the center of mass acceleration state constraint; Indicates the lower limit of the center of mass acceleration state constraint; the superscript a indicates the center of mass acceleration;

[0235] According to the foot-ground contact restriction of the terrain, the robot foothold position constraint equation is established, specifically:

[0236] p f,z (t) = f terr (p f,x (t),p f,y (t))

[0237] Where: p f,z (t) represents the z-direction coordinate of the foothold position; p f,x (t) represents the x-coordinate of the foothold position; p f,y (t) represents the y-coordinate of the foothold position; f terr () represents the ground model function;

[0238] According to the cylindrical envelope of the obstacle, the obstacle avoidance constraint equations for the foothold position and center of mass motion trajectory of the legged robot are established, specifically:

[0239]

[0240]

[0241] Where: r obs =0.3 is the radius of the cylinder, r sec =0.025 is the obstacle avoidance safety threshold radius; P f,x Indicates the foothold position in the x direction; P f,y Indicates the foothold position in the y direction; and 3 represent the origin positions of two ground obstacles in the x direction; and -0.2 represent the origin positions of the two ground obstacles in the y direction.

[0242] Additionally, set up air obstacles:

[0243]

[0244] Where: r obs =0.05 is the radius of the cylinder, rsec = 0.01 is an obstacle avoidance safety threshold radius; represents the x-direction aerial obstacle origin position; represents the y-direction aerial obstacle origin position.

[0245] As shown in Figures 4a-4d , the pressure center position should be on the line contact constraint of two diagonal support feet; located on the plane defined by three points: the centroid, the pressure center and the footfall position; therefore, the cross product of the footfall position to the centroid position vector and the foot contact force vector from the pressure center position to the centroid position generates a normal vector of the plane orthogonal to the plane formed by the footfall position to the centroid position vector and the foot contact force vector from the pressure center position to the centroid position, and as shown by the green line in Figures 4a-4d ; this relationship can be expressed by the following constraint equation:

[0246] [r(t) x f(t)] · r trot (t) = 0

[0247] In the formula: r(t) represents the footfall position to the centroid position vector, f(t) represents the foot contact force vector from the pressure center position to the centroid position, and r trot represents the vector formed by the foot line contact; wherein:

[0248]

[0249] In the formula: p HL represents the right rear foot of the foot line contact; and p FR represents the left front foot of the foot line contact.

[0250] As shown in Figure 8 , the centroid trajectory and the footfall position obtained by the motion trajectory generation method of the present application have terrain adaptation and obstacle avoidance capabilities. In summary, the results of the long-term domain and reaction type combined motion trajectory generation method of the foot-type robot of the present embodiment prove to have good effects.

[0251] The above embodiments only describe the preferred embodiments of the present application, and do not limit the scope of the present application. Without departing from the design spirit of the present application, various modifications and improvements to the technical solutions of the present application made by those skilled in the art shall fall within the protection scope determined by the claims of the present application.

Claims

1. A method for generating motion trajectories of a legged robot combining long-term and reactive motion, characterized in that: The following steps are involved: Step 1: Establish an equivalent single-mass model of the legged robot and analyze its motion characteristics based on the foothold position and single-mass dynamics; Based on the motion characteristics of the legged robot, an equivalent single-mass model is extracted and used to simulate the robot's motion. Based on the relationship between the foothold position and the pressure center, the coordinates of the foothold's geometric center are used as the expected value of the pressure center, and a pressure center constraint domain based on the support multi-deformation is established. The relationship between the center of mass trajectory and the pressure center is established based on the relationship between the pressure center, the foot-ground contact force, and the center of mass acceleration. Step 2: By analyzing the motion characteristics of the equivalent single-particle model, an equivalent single-particle model cost function based on the center of mass acceleration vector and the foothold position vector is established, which is expressed as: Where: J U(t) is the cost function; t0 and t F Respectively represent the start time and end time of the entire motion domain; ω U is the weight coefficient; f(U(t)) and denote the predicted performance index and the expected performance index respectively; The expression of the variable to be optimized U(t) is as follows: Where: is the acceleration vector of the center of mass, p f (t) is the foothold position vector; Step 3: Based on the step length, step height, and endpoint center of mass state in the legged robot's motion characteristics, the pressure center and foot-ground contact force vector in the dynamic characteristics, and the acceleration, terrain, and obstacle avoidance constraints in different gaits during walking, jumping, and gait transitions, establish the spatial constraint equations for the system state. Based on the system state, the spatial constraint equation of the variable to be optimized is established, which is expressed as: Where: U upp and U low are the maximum and minimum values ​​of the variable to be optimized U(t) in each system state; Step 4: Combine the cost function and the spatial constraint equation to form a nonlinear optimization equation, and use the motion optimization solver to generate a long-term center of mass trajectory and footfall sequence from the start to the end point: According to the equivalent single-mass model cost function in step 2 and the constraint equation in step 3, the nonlinear optimization equation of the legged robot is: Based on the nonlinear optimization equation of the legged robot, a long-term center-of-mass trajectory and footfall sequence from the start time to the end time are generated; According to the walking and jumping gaits of the legged robot and the transition between gaits, the acceleration constraint equation of the center of mass of the legged robot is established, which is as follows: Where: c a (t) represents the actual acceleration state of the center of mass; Indicates the upper limit of the center of mass acceleration state constraint; Indicates the lower limit of the center of mass acceleration state constraint; the superscript a indicates the center of mass acceleration; When the robot's jumping gait is in the air phase, the center of mass acceleration constraint equation is: Where: [0 0-g] T represents the gravitational acceleration vector; Step 5: Combine model predictive control with velocity field-guided trajectory convergence to converge the short-time domain reactive trajectory to the long-time domain motion trajectory and generate the global motion trajectory: Combining the real-time performance of short-term reactive trajectory optimization, the constructed nonlinear optimization equation for the legged robot is used for iterative optimization. The short-term local optimal center-of-mass trajectory and footfall sequence starting at the current moment are generated. When encountering external disturbances, the center-of-mass state deviates from the expected trajectory. To ensure that the short-term motion trajectory converges to the long-term motion trajectory, the velocity field is used as a guide to generate the expected combined velocity sequence in the short-term domain. The reactive trajectory is converged to the long-term motion trajectory through model predictive control, thereby generating the global motion trajectory. Based on terrain constraints and flight constraints, this motion trajectory generation method can realize walking gait, jumping gait, and gait conversion between walking and jumping gait according to the terrain model; By establishing upper and lower bounds for the center of mass acceleration constraint and limiting the center of mass acceleration constraint in the flight phase to gravity acceleration, motion planning for both walking and jumping gaits can be achieved simultaneously, and the transition between walking and jumping gaits can be realized.

2. The method for generating a motion trajectory of a legged robot combining long-term and reactive motion according to claim 1, characterized in that: The equivalent single-particle model cost function in step 2 includes: The legged robot tracks the user input speed and acceleration instructions, and the center of mass state cost function is specifically: Where: t0 and t F Respectively represent the start time and end time of the entire motion time domain; is the weight coefficient of the center of mass state cost function; c a,v (t) and Denote the predicted and expected center of mass states respectively; a, v denote the acceleration and velocity states of the robot center of mass respectively; The cost function of the landing point position is: Where: ω p is the weight coefficient of the foothold position; r(t) represents the position vector of the foothold position and the center of mass; f(t) represents the foot-ground contact force vector from the pressure center to the center of mass; The centroid height cost function is: Where: ω z is the weight coefficient of the centroid height; r z (t) represents the actual center of mass height of the legged robot; Indicates the desired center of mass height of the legged robot.

3. The method for generating a motion trajectory of a legged robot combining long-term and reactive motion according to claim 1, characterized in that: The spatial constraint equations in step 3 include: According to the gait and structure of the legged robot, the center of mass constraint equation of the legged robot is established as follows: Where: c v,p (t) represents the actual velocity and position state of the center of mass; Indicates the upper limit of the center of mass velocity and position state constraints; Indicates the lower limit of the velocity and position state of the center of mass; the superscripts v and p represent the velocity and position state of the center of mass respectively; According to the length of the robot leg rod, the robot leg length constraint equation is established as: Where: ||r(t)|| represents the modulus of the vector from the foothold position to the center of mass position; Indicates the upper limit of the square of the magnitude of the vector from the foothold position to the center of mass position; The lower limit of the square of the vector from the foothold position to the center of mass position; According to the limitations of the robot's leg length and stride length, the robot's stride length constraint equation is established as: Where: p f,xy Indicates the x and y coordinates of the landing point; Indicates the upper limit of stride constraint; represents the lower limit of the stride constraint; T step Indicates the time period of stepping; During each step time period, the foothold position remains unchanged: p f (t)≡C,t∈[t step0 ,t step0 +T step ] Where: t step0 Indicates the starting moment of each step time period; According to the contact sequence of the legged robot walking, the conservative constraint equation of the pressure center position is established, specifically: Where: p p,xy (t) represents the x and y coordinates of the pressure center; p f,xy (t) represents the x and y coordinates of the geometric center of the current foothold; Indicates the upper limit of the distance constraint between the pressure center and the geometric center of the current foothold; Indicates the lower limit of the distance constraint between the pressure center and the geometric center of the current foothold; According to the foot-ground contact restriction of the terrain, the robot foothold position constraint equation is established, specifically: p f,z (t)=f terr (p f,x (t),p f,y (t)) Where: p f,z (t) represents the z-direction coordinate of the landing point; p f,x (t) represents the x-coordinate of the foothold position; p f,y (t) represents the y-coordinate of the foothold position; f terr () represents the ground model function; According to the cylindrical envelope of the obstacle, the obstacle avoidance constraint equations for the foothold position and center of mass motion trajectory of the legged robot are established, specifically: Where: r obs is the radius of the cylinder, r sec is the obstacle avoidance safety threshold radius; P f,x Indicates the foothold position in the x direction; P f,y Indicates the foothold position in the y direction; Indicates the origin position of the obstacle in the x direction; Indicates the position of the obstacle origin in the y direction.

4. The method for generating a motion trajectory of a legged robot combining long-term and reactive motion according to claim 3, characterized in that: When the legged robot is in line contact with the ground, the pressure center is located on the contact line formed by the foot-ground line contact, that is, the cross product of the foot-ground contact force vector from the foot position to the center of mass position and the pressure center position to the center of mass position. The constraint equation for the pressure center position is: [r(t)×f(t)]·r 线接触 (t)=0 Where: r(t) represents the vector from the foothold position to the center of mass position, f(t) represents the foot-ground contact force vector from the pressure center position to the center of mass position, r 线接触 (t) represents the vector formed by the foot-ground contact; where: Where: p1 represents the first end point of the foot-ground line; p2 represents the second end point of the foot-ground line.

5. The method for generating a motion trajectory of a legged robot combining long-term and reactive motion according to claim 1 or 4, characterized in that: According to the set target speed of the legged robot, the velocity constraint equation of the center of mass at the final moment is established, which is: Where: represents the velocity of the center of mass at the final moment; Indicates the upper limit of the center of mass velocity at the final moment; Indicates the lower limit of the center of mass velocity at the final moment; According to the set target position of the legged robot, the constraint equation of the center of mass position at the final moment is established, which is: Where: c xy (t F ) represents the x and y coordinates of the center of mass at the final moment; Indicates the upper limit of the center of mass position in the x and y directions at the final moment; Indicates the lower limit of the center of mass position in the x and y directions at the final moment.

6. The method for generating a motion trajectory of a legged robot combining long-term and reactive motion according to claim 1, characterized in that: The expected combined velocity in step 5 is: Where: represents the expected total velocity of the center of mass at time k; represents the expected speed in the long time domain at time k; Indicates the convergence speed of the long-time domain motion trajectory at time k; Where: v c Indicates the maximum safe convergence speed; ε k represents the vertical distance between the center of mass at time k and the long-term motion trajectory; d represents the maximum deviation distance threshold of the convergence space; κ ≥ 1 represents the attraction coefficient of the velocity field.

7. The method for generating a motion trajectory of a legged robot combining long-term and reactive motion according to claim 6, characterized in that: In the velocity field guidance of step 5, the desired combined velocity sequence used for model predictive control desired velocity tracking is: The iterative algorithm is used to calculate the future convergence rate sequence of each cycle: Where: Indicates short time domain range; The expected total velocity sequence is:

Images