Quadruped robot covariance self-adaptive control method for spine joint adjustment

Abstract

The invention discloses a covariance self-adaptive control method for a quadruped robot for spine joint adjustment. The method comprises the following steps that 1, a spine quadruped robot kinematics model with spine joints, a robot trunk dynamics model with the spine joints and a robot leg multi-rigid-body dynamics model are established; 2, based on the energy stability margin theorem and the spine joint coupling effect, a posture control method of the spine joint quadruped robot is provided by combining a covariance self-adaptive evolutionary strategy, and the angle, rigidity and damping coefficient of spine joints are estimated and adjusted according to the terrain gradient; 3, extracting the spatial relationship between the trunk mass center and the foot ends of the robot as an expected reference, updating the expected mass center position in real time, and constructing a model prediction control method to control the posture of the spinal joint quadruped robot; 4, the position of the foot falling point of the swing leg is designed in advance in combination with the additional freedom degree provided by the spine joint and MPC prediction state information. The motion stability of the robot in the complex terrain is improved.

Description

Technical Field

[0001] The present invention relates to the field of quadruped robot control, in particular to a quadruped robot covariance adaptive control method with spinal joint adjustment. Background Art

[0002] In recent years, with the rapid development of artificial intelligence and robotics, quadruped robots, with their exceptional adaptability to terrain, have become a key player in autonomous mobility in complex environments. They are widely used in challenging scenarios such as disaster relief, field inspections, and exploration in extreme terrain. Traditional wheeled and tracked robots often struggle to perform mobility tasks in unstructured terrain, such as steep slopes, steps, gravel, and soft soil, due to poor accessibility and instability. However, quadruped robots, with their multi-legged contact redundancy and flexible gait planning capabilities, exhibit greater stability and environmental adaptability.

[0003] However, existing quadruped robots mostly use rigid body structures. In complex terrain environments, this rigid structure not only restricts the movement of the legs but also causes a lag in posture adjustment response, thus affecting overall motion performance and dynamic stability. In contrast, quadruped robots with active spinal joints, due to the introduction of additional degrees of freedom, exhibit greater flexibility and adjustment capabilities in torso posture adjustment, and have greater potential for motion coordination.

[0004] However, current mainstream control methods focus on coupling leg joint control with terrain perception, failing to fully exploit the dynamic adjustment potential of spinal joints and the ability to expand the reach of the foot during posture adjustment. This limits their performance on unstructured terrain. In particular, in trunk posture adjustment and foot trajectory planning, there is a lack of a systematic approach to integrating spinal joints into the optimization process, making it difficult to achieve improved dynamic stability through spinal joint coordination. Summary of the Invention

[0005] In order to overcome the shortcomings of existing quadruped robots in the absence of spinal joints, such as a single posture adjustment method and limited foot end motion space when moving on a slope, the present invention provides a covariance adaptive control method for a quadruped robot with spinal joint adjustment to improve the dynamic stability of the robot on irregular slopes. The method actively adjusts the trunk posture through the spinal joints to expand the posture adjustment and center of gravity control range of the quadruped robot, and optimizes the foot end space layout and trajectory planning strategy in combination with the coupling characteristics of the spinal joints, which can significantly improve the stability and passability of the quadruped robot in the environment. The present invention provides a covariance adaptive control method for a quadruped robot with spinal joint adjustment, which is suitable for quadruped robots with spinal joints. It combines terrain perception and optimization algorithms to solve the optimal posture solution, and uses the foot end space increased by the spinal joints to optimize the foot end trajectory, further improving the passability and motion performance of the robot during movement.

[0006] In order to solve the above technical problems, the present invention provides the following technical solutions:

[0007] A covariance adaptive control method for a quadruped robot with spinal joint adjustment, the method comprising the following steps:

[0008] Step 1: Establish a kinematic model of a quadruped robot with spinal joints. Analyze the effects of spinal angle changes on foot end space, center of mass adjustment, and trunk moment of inertia. Use single-body dynamics to model the robot trunk with spinal joints. Use Newton-Euler formulas to model the robot legs with multi-body dynamics. This provides a theoretical basis for controlling quadruped robots with spinal joints.

[0009] Step 2: Introduce a terrain slope estimation algorithm that integrates the inertial measurement unit (IMU) and foot contact point information to obtain current ground information in real time. Based on the energy stability margin theorem and the spinal joint coupling effect, propose a covariance adaptive evolutionary strategy that integrates spinal joint adjustment: Utilize the covariance adaptive evolutionary strategy (CMA-ES) to solve the optimal solution for the robot posture offline, and use an online table lookup interpolation method to calculate the optimal solution in real time during the robot's motion. Adjust the angle, stiffness, and damping coefficient of the spinal joint according to the estimated terrain slope to achieve adaptive adjustment of the spinal joint under different terrains.

[0010] Step 3: Based on the optimal posture solution obtained in Step 2, the spatial relationship between the robot's trunk center of mass and the robot's foot is extracted as the desired reference. The desired center of mass position is updated in real time, and the spinal quadruped robot's trunk posture is controlled using the model predictive control (MPC) method. Based on the dynamic analysis of the single leg in Step 1, the joint motor output torque is precisely adjusted through a control method that combines position-velocity feedback with feedforward compensation, enabling the foot to track the planned trajectory.

[0011] Step 4: Combine the additional degrees of freedom provided by the spinal joints with the MPC predicted state information to pre-design the landing point of the swing leg. Combined with the ground information, a third-order Bezier curve is used to generate the foot trajectory to enhance the foot's ability to fit the terrain and further improve the robot's stability in complex terrain.

[0012] Furthermore, step 1 includes the following sub-steps:

[0013] Step 1.1: The spinal quadruped robot structure is designed as follows. The robot has two trunks, the front and rear trunks connected by a joint motor fixed to the rear trunk. The relative pitch angle of the front and rear trunks is adjusted by controlling the spinal joint motor. The front and rear trunks are connected to two legs respectively. Each leg has three joint motors corresponding to the hip joint, thigh joint, and calf joint.

[0014] Step 1.2: Establish a kinematic model for a quadruped robot with spinal joints. Set the quadruped robot's body coordinate B at the midpoint of the trunk, which is the geometric midpoint of the spinal joints. The front and rear trunk lengths and widths are equal. Without considering the movement of the spinal joints, mathematical modeling can be used to determine the foot positions of the four legs in the body coordinate system. Because the trunk will change due to changes in the angle of the spinal joints, it is necessary to multiply the above mathematical modeling by a rotation matrix around the y-axis to obtain the exact foot position of each foot in the body coordinate system. The formula is as follows:

[0015]

[0016] in, is the foot end position in the body coordinate system, Represents the coordinates of the foot end in the body coordinates, where i is represented by FL, FR, RL, and RR, representing the front left leg, front right leg, back left leg, and back right leg, respectively. is the rotation matrix generated by the rotation of the spinal joint, where θ s is the angle of rotation of the spinal joint. Since the front and rear trunks are symmetrical in this invention, the front and rear trunks rotate separately. θ0, θ1, and θ2 are the angle vectors of the hip, thigh, and shank joints, respectively. L is the distance from the body coordinate origin to the thigh joint in the x-axis direction, a1 is the thigh link length, a2 is the shank link length, W1 is the distance from the thigh joint to the shank joint in the y-axis direction, and W0 is the width from the body coordinate origin to the center of the hip joint along the y-axis. Because the joint motors of the left and right legs and the front and back legs of the real robot are installed in different directions, their rotation directions will be different in the same coordinate system. Therefore, α and β are set as symbolic variables to distinguish the positions of the quadruped robot legs: α = 1 for the front leg, α = -1 for the back leg; β = 1 for the left leg, β = -1 for the right leg;

[0017] When controlling the motion of a quadruped robot, the partial derivative relationship of the foot position to each joint angle, i.e., the Jacobian Matrix, is needed in the gait planning and force control process. When controlling the foot trajectory, the Jacobian Matrix is ​​needed to map the change of the joint angle to the foot velocity. When implementing contact force control, the Jacobian Matrix is ​​needed to convert the force / torque in the foot space into the force / torque in the joint space. In order to construct the Jacobian Matrix, the foot position needs to be The partial derivatives of the joint variables θ0, θ1, and θ2 are used respectively. After sorting, the Jacobian matrix of the spine joint quadruped robot can be obtained. The formula is as follows:

[0018]

[0019] Step 1.3: Establish a dynamic model of a spinal joint quadruped robot. Considering the complex structure of the spinal quadruped robot and its more complex trunk structure compared to ordinary quadruped robots, directly establishing multi-rigid body dynamics is not conducive to calculation and control. Therefore, in the present invention, the trunk and legs are separated for dynamic modeling. First, the dynamic model of the spinal quadruped robot is simplified and divided into two parts: trunk control containing spinal joints and leg control. The trunk is dynamically modeled using single rigid body dynamics. It is necessary to know the position of the center of mass of the entire trunk in the body coordinates and the world coordinates. Therefore, it is necessary to calculate through the relationship between the world coordinates and the body coordinates, that is, the posture of the body coordinate system and the coordinates of the body origin in the world coordinate system. The formula is as follows:

[0020]

[0021] Among them, in the body coordinate system, the coordinates of the center of mass of the rear rigid body are Coordinates of the center of mass of the front rigid body It can be calculated by the rotation of the spinal joints. is the position of the system center of mass in the body coordinates, is the position of the system center of mass in world coordinates, R WB is the orientation of the robot, that is, the posture of the body coordinate system, is the position of the robot, that is, the coordinate of the origin of the robot in the world system;

[0022] The trunk of a quadruped robot with a spine consists of two parts. The moment of inertia of the entire trunk changes with the change of the spine angle. In order to model the entire trunk using single-body dynamics, it is necessary to calculate the new moment of inertia of the system in advance. The two trunk segments connected by the spine are considered as a whole, and the single-body dynamics is established based on this. The formula is as follows:

[0023]

[0024] in, is the moment of inertia of the robot in world coordinates, R i is the orientation of rigid body i in the world system, is the local inertia tensor of the rigid body at its own center of mass, m i is the mass of the front and rear torso, is the position of the rigid body's center of mass relative to the system's center of mass (in world coordinates); f s(i) To support the force of the legs and feet on the trunk, P gi is the distance from the foot end to the total center of mass, is the angular velocity and angular acceleration of the total center of mass in world coordinates;

[0025] The dynamic modeling of the quadruped robot legs is carried out to achieve precise control of the foot ends of each leg of the quadruped robot. The recursive formula of the Newton-Euler method is used to establish the single-leg dynamic model. During the modeling process, the angular velocity ω of each joint is derived step by step. i , angular acceleration and the center of mass a ci The recursive relationship of acceleration is used to obtain the motion state of each connector under the chain mechanism. The recursive relationship is as follows:

[0026]

[0027] Among them, all quantities are expressed in the coordinate system of the link origin of each rigid body, q is the joint angle vector, z i is the current joint axis direction (unit vector), r ci is the vector from the connecting rod origin to the center of mass, is the rotation matrix from the previous level coordinates to the current level coordinates. After obtaining the motion state of each link, we can further solve the relationship between its momentum change and force, and construct the standard form of the joint space dynamics equation: Where M(q) is the inertia matrix, is the Coriolis / centrifugal term, and G(q) is the gravity term.

[0028] Furthermore, step 2 includes the following sub-steps:

[0029] Step 2.1: Analyze the effect of spinal joints on the stability of quadruped robots. When a rigid-torso quadruped robot is on a slope, its center of gravity needs to be shifted inward to prevent the body from tipping over. The center of gravity adjustment needs to be achieved by adjusting the height of the front and rear legs to improve the stability of the robot system. However, this method will inevitably compress the motion space of the foot end, resulting in a limited range of foot end trajectory generation, which is not conducive to gait planning and dynamic motion performance. However, robots with spinal joints can achieve fine-tuning of the body's posture in the pitch direction by actively adjusting the spinal pitch angle. The spinal joint adjusts the angle θ to achieve fine-tuning of the body's posture in the pitch direction.s When the foot is in the air, it can cause the center of gravity to shift on the horizontal axis and the vertical axis, which improves stability while still allowing the foot to have a larger trajectory generation range.

[0030] Step 2.2: After the spinal robot enters the motion initialization state, the terrain slope θ of the environment is calculated by combining the inertial measurement units of the front and rear trunks with the spinal robot kinematic equation established in step 1. slope , and is continuously updated according to the information of the supporting leg in the subsequent movement. The formula is as follows:

[0031]

[0032] Where, is the position vector of the foot end in the world coordinate system, R WB Represents the rotation matrix from the body coordinate system to the world coordinate system; is the position vector of the foot end in the body coordinate system; Represents the position vector of the robot body in the world coordinate system; record the position of the foot end when it touches the ground, and estimate the ground information based on these point sets. The ground information can be expressed as ax+by+cz+d=0, where n=[a,b,c] T is the ground normal vector, d is the offset, and the terrain slope θ is obtained by calculating the angle between the ground normal vector and the z-axis of the world coordinate system slope , where e z Represents the vertical unit vector in the world coordinate system;

[0033] The spinal joint has the most obvious influence on the pitch angle of the robot. For a quadruped robot with a front and rear trunk, a core reference value θ of the robot's posture is obtained by calculation. b , the formula is as follows:

[0034]

[0035] Among them, the rear trunk posture θ r , front trunk posture θ f , spine angle θ s ,θ f It can be achieved through θ r The angle θ of the spinal joint s Calculated;

[0036] Step 2.3: Construct a mathematical model based on the spinal joint posture adjustment method. This method enables the quadruped robot to maintain the stability of the fuselage and the integrity of the foot end motion space in complex terrain with a certain slope, thereby expanding the range of foot end trajectory generation. Based on the above analysis of the relationship between the robot and the terrain, it is necessary to design a cost function. According to this objective function and the constraints, the optimal posture solution is obtained. In the cost function, the robot's posture angle needs to be as small as possible, and the front and rear legs are stretched as equally as possible so that the front and rear feet have sufficient movement space. When going up and down slopes, the rear and front halves are set as horizontal as possible, and the distance between the body's center of gravity and the supporting foot end is increased, thereby increasing the energy required for overturning and improving the robot's stability. The cost function is shown in the following formula:

[0037]

[0038] Where, J1(q) is the function of the trunk pitch angle; J2(q) is the function of the distance from the front and back feet to the hip joint; represents the z coordinate of the foot end, h terrain (x i ,y i ) represents the height of the terrain at that location. The entire formula indicates that the foot of the supporting leg needs to touch the ground; J3(q) is the function that sets the rear and front halves of the robot to be as horizontal as possible when going uphill and downhill; J4(q) is the function that maximizes the energy stability margin required by the robot;

[0039] In order to improve the solution efficiency and real-time performance of this method in practical applications, the present invention adopts an offline and online combined optimization strategy. In this invention, the covariance adaptive evolutionary strategy (CMA-ES) is used for offline optimization. According to the above objective function, a more reasonable optimal posture solution is obtained. The core formula is as follows:

[0040]

[0041] in: represents the kth child in the g+1th generation, m (g) represents the mean of the g-th generation search distribution, C represents the covariance matrix, σ represents the step size, N(0,C (g) ) represents a multivariate normal distribution with mean 0 and covariance C, E||N(0,I)|| represents the expectation of the Euclidean norm of a random vector distributed under N(0,I), and w i Represents the positive weight coefficient for recombination, used to calculate the mean of the selection points, p c Represents the evolutionary path corresponding to each generation, p σ represents the evolutionary path of each generation step length, c1 is the learning rate of the covariance matrix rank one update, c μ is the learning rate for updating the covariance matrix rank μ, cσ is the cumulative learning rate controlled by step size, d σ is the decay parameter for step size update;

[0042] The process of the covariance adaptive evolutionary strategy CMA-ES is as follows: first, set the initial posture, step size, covariance matrix and other parameters, then sample a batch of new posture parameters, each group x is a complete posture, generate dozens of different postures, calculate according to the established objective function J, make J as small as possible, then select the best solutions of the first few groups for subsequent guidance updates, take the best performing groups of postures as the center, adjust the mean posture m, and update the covariance matrix based on this, so that the search area is explored in the direction of a better solution, and update the search scale, that is, the step size. If the progress of several consecutive generations is large, the step size is enlarged, otherwise the step size is shortened for a more detailed search. Finally, if it tends to be stable or reaches the set maximum number of generations, return the current optimal posture parameters, including the angular vector q of each joint of the quadruped robot;

[0043] During the actual operation of the robot, the solution provided by the offline model is used as the initial value, and the online table lookup interpolation method is used for calculation. According to the current terrain state and center of mass state, the offline posture database is retrieved for the closest samples to the current state. The retrieved samples are interpolated and reasoned, and the inferred posture angle is sent as a control instruction to the spine joint and leg joint controllers to adjust the robot's body posture in real time.

[0044] Step 2.4: Construct a spinal joint control strategy by combining ground information and the optimal posture solution. In order to reduce the amount of MPC calculation, after calculating the optimal solution of the spinal joint, the angle of the spinal joint is first controlled. The control target is the torque output of the spinal joint. When the current angle is less than the target angle, a torque is output to push it closer to the target angle. When the current angle is greater than the target angle, a reverse torque is generated to reduce the angle. And according to the terrain slope θ slope The robot actively adjusts the damping and stiffness of the spinal joints according to the changes in the slope, so that the robot has a certain shock absorption effect on terrain with a small slope and a certain spinal joint stiffness on steep terrain. When the slope is small, such as a gentle slope or flat ground, shock absorption is prioritized, the stiffness is reduced, and the damping is moderately increased. When the slope is large, such as a steep slope or steps, stability is prioritized, the stiffness is increased, and the damping is moderately reduced. The formula is as follows:

[0045]

[0046] Among them, θ slop is the terrain slope, θ maxslop is the preset maximum slope, K(θ slop ) is the stiffness adjustment function, K max , K min is the upper and lower limits of stiffness, D(θslop ) is the damping adjustment function, D max 、D min The flatter the terrain, the closer the stiffness is to K. min , the closer the damping is to D max ; The steeper the terrain, the closer the stiffness is to K max , the closer the damping is to D min . p and q are power parameters that control the shape of the curve.

[0047] Furthermore, the step 3 includes the following sub-steps:

[0048] Step 3.1: Construct a model predictive control (MPC) method to control the trunk posture of the spinal quadruped robot. In this invention, the robot motion control adopts a predictive control method based on a single rigid body dynamics model. Considering that the center of mass and posture of the quadruped robot change dramatically during the interaction between the body and the terrain, a set of discrete-time dynamic state transfer equations are constructed for MPC solution; these include the robot center of mass motion equation and the robot posture motion equation (around the center of mass). The dynamic model is based on the robot body center of mass position c k , center of mass velocity Body angular velocity ω k and attitude angle θ k As a state variable, the control input is the contact force f applied to each supporting foot end i,k , and integrated into the standard x k+1 =f(x k ,u k ) is solved. Specifically, the updated form of the system state in each control cycle k is as follows:

[0049]

[0050] Where h is the discrete time step, representing the length of the MPC control cycle; M is the overall mass of the robot; is the moment of inertia matrix of the fuselage in the world coordinate system; g is the gravity acceleration vector. The force f applied by each foot end i,k To optimize the variables, MPC is used to minimize the objective function to achieve predictive control of the robot's body stability and motion performance;

[0051] When constructing the objective function of MPC, the optimal posture solution obtained by nonlinear optimization in step 2 is introduced, and the spatial relationship between the ideal foot position and the ideal center of mass position is extracted and defined as the ideal relative position vector. The ideal relative position between the foot end and the center of mass of the i-th supporting leg in the ideal state is assumed to be:

[0052]

[0053] Where, is the ideal center of mass position, For the ideal foot position of the supporting leg, these two values ​​are obtained by using the joint angle vector obtained in step 2.3 according to the kinematic equation in step 1; r i opt represents the position vector of the center of mass relative to the i-th foot end in the optimal posture state;

[0054] When performing attitude control, let N s The leg is in the ground contact phase, so for each control cycle k, according to the current actual foot position of the robot Combined with the above ideal relative position relationship between the center of mass and the foot of the supporting leg, r i opt , calculate the expected center of mass position c at the current moment in real time ref,k , the formula is as follows:

[0055]

[0056] The c calculated above is ref,k As the target position in predictive control, the final MPC objective function is constructed. The current center of mass position, the desired center of mass position, the posture angle, and the foot end force constraint are used as the MPC input to solve the foot end of the supporting leg. Then, the torque required to output by each joint motor is solved through the Jacobian matrix to control the quadruped robot torso posture. The formula is as follows:

[0057]

[0058] Where: c k ,θ k are respectively the center of mass position and attitude angle at the kth moment under MPC prediction; θ ref,k is the attitude reference angle obtained by nonlinear optimization; f i,k is the contact force of the i-th foot end; Q, R, W are weight matrices used to adjust the optimization priority of each part; It is a friction cone constraint to prevent the foot end from sliding;

[0059] Step 3.2 is further based on the leg dynamics model of step 1, and the end trajectory tracking method is used to design the swing leg control. The control input is the target position, velocity and acceleration of the trajectory planning. The control goal is to make the foot end accurately reach the specified landing point and control the velocity direction before touching the ground to achieve anti-slip and stable landing. The controller uses the position acceleration feedback method, combined with the feedforward compensation term solved based on the dynamic model, and uses the Jacobian matrix obtained in step 1 to solve the torque required by each joint motor to achieve the torque control output in the joint space. The formula is as follows:

[0060]

[0061] Where, τ i Control the output joint torque for the swing leg; is the foot-end Jacobian matrix obtained in step 1; is the reference foot end position and velocity in the body coordinate system; P i B 、V i B is the current foot end position and speed in the body coordinate system; K p , K d is the foot-end PD controller gain; τ i,ff It is the feedforward compensation torque used to offset inertia and gravity; is the inertia matrix of the operating space; is the expected foot acceleration in the body coordinate system; for

[0062] Non-inertial acceleration of the foot; is the compensation term for centrifugal force and Coriolis force; G i is the gravity torque compensation term.

[0063] Furthermore, the step 4 includes the following sub-steps:

[0064] Step 4.1 Plan the foot-end landing point and foot-end trajectory. Due to the role of the spinal joint, the foot-end motion space of the spinal quadruped robot in various environments is increased, so the foot-end trajectory can be better planned for terrain adaptability. The center of mass trajectory is composed of the center of mass position of the optimal posture and the expected robot motion speed and other information. The expected robot motion speed is related to the swing leg. The swing leg control needs to determine the position of the next landing point. However, when the swing leg swings, the robot torso will be adjusted due to the control of MPC, so the center of mass position predicted by MPC needs to be used. Estimate the footfall point and incorporate ground information into it using the following formula:

[0065]

[0066] Where, Indicates the closest step in the current MPC prediction. If the number of steps obtained in the current prediction is insufficient, the value cached in the previous prediction round is used; T swing is the duration of the swing phase; v des is the desired forward speed of the robot (determined by the current motion state). To increase system stability, Δ(θ) is set as the slope correction term, where a controls the forward and backward correction term and b controls the vertical lift.

[0067] Step 4.2: After knowing the foothold position and ground information, the foot end motion space increased by the spinal joint can be used to plan the foot end trajectory, so that the foot end trajectory can be autonomously adjusted according to changes in the terrain; when the robot is going uphill, the foot end steps on the ground in a backward and downward direction, and when going downhill, it steps on the ground in a forward and downward direction. In order to improve the stability of the quadruped robot, the present invention uses a third-order Bezier curve to generate the foot end trajectory. The formula is as follows:

[0068]

[0069] In the formula, the starting point is P0; the landing point is P3; P1 is the first control point, which is set at the ratio α1 between the starting point and the landing point, and a Z-axis lift h(θ) is added on this basis, where α1 is the offset ratio and h(θ) is the lift function; P2 is the second control point, which is set above the landing point, where β is the slope factor, which is set here as: β = 1 when uphill, β = -1 when downhill, d f is the advance distance along the terrain tangent, t terrain is the terrain tangent direction;

[0070] The present invention compares the end movement direction and the ideal direction d ideal The angle between the two ends of the trajectory is used to determine whether the trajectory end design is reasonable. If the angle is less than 20 degrees, it means that the trajectory end design is relatively successful. The formula is as follows:

[0071]

[0072] Where n terrain is the terrain normal vector, t terrain is the terrain tangent vector, α is the weighting coefficient, and the expected landing angle can be set by setting the weighting coefficient;

[0073] Step 4.3: The overall motion control process is as follows. The motion control process begins with external command input, generating a center of mass trajectory based on the command. Terrain slope information is estimated based on the contact position of the quadruped's foot. This terrain estimation information is used in two key areas: spinal joint control and leg swing control. Terrain slope information is transmitted to the spinal joints for controlling spinal joint angle, stiffness, and damping, as well as for determining the optimal posture.

[0074] The support leg control part uses the solved optimal posture information, combined with the current relative position of the center of mass and the foot, and inputs it into the model predictive controller. The MPC controller then solves the control information required for the support leg, including joint output torque, to ensure that the robot can still maintain balance and the desired trajectory on uneven terrain.

[0075] In the swing leg control module, the system inputs terrain information and the center of mass position information output by the MPC into the foot landing point planning module to generate the optimal foot landing point. Based on the terrain information, the complete foot trajectory is calculated to guide the spatiotemporal trajectory control of the swing leg, achieving efficient landing and leg lifting movements.

[0076] Finally, the control output information of the supporting leg and the swinging leg is integrated to generate the final joint torque output matrix, which drives the robot to complete the entire dynamic motion control process. The entire system has good adaptability and robustness, and can maintain the motion stability and flexibility of the quadruped robot under complex terrain conditions.

[0077] Compared with the prior art, the present invention has the following outstanding advantages and technical effects:

[0078] The present invention provides a covariance adaptive control method for a quadruped robot with spinal joint adjustment, which is suitable for quadruped robots with spinal joints. It combines terrain perception with an optimization algorithm to solve the optimal posture solution, and uses the foot-end space increased by the spinal joints to optimize the foot-end trajectory, further improving the robot's passability and motion performance during movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 Schematic diagram of a flow chart of an embodiment of the present invention;

[0080] Figure 2 This is a structural diagram of the spinal quadruped robot of the present invention, wherein 1 represents the calf joint, 2 represents the thigh joint, 3 represents the hip joint, 4 represents the front trunk, 5 represents the spinal joint, 6 represents the rear trunk, and 7 represents the inertial measurement unit (IMU);

[0081] Figure 3 This is a simplified model diagram of the spinal joint quadruped robot of the present invention;

[0082] Figure 4 This is the energy stability margin analysis diagram of the present invention;

[0083] Figure 5 A schematic diagram of the robot estimating terrain information according to the present invention;

[0084] Figure 6 Flowchart of the covariance adaptive evolutionary strategy (CMA-ES) of the present invention;

[0085] Figure 7 This is a schematic diagram of the optimal posture solution of the present invention;

[0086] Figure 8 This is a control flow chart of the support phase and swing phase of the quadruped robot according to the present invention;

[0087] Figure 9This is a schematic diagram of the foot end trajectory of the present invention;

[0088] Figure 10 This is a schematic diagram of the third-order Bessel foot trajectory planning of the present invention;

[0089] Figure 11 This is the overall flow chart of the motion control of the spinal quadruped robot of the present invention. DETAILED DESCRIPTION

[0090] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the following embodiments will further illustrate the present invention in conjunction with the accompanying drawings. It should be understood that the specific embodiments described herein are only intended to explain the present invention and are not intended to limit the present invention. On the contrary, the present invention encompasses any substitutions, modifications, equivalent methods and solutions made to the spirit and scope of the present invention as defined by the claims. Furthermore, in order to provide the public with a better understanding of the present invention, certain specific details are described in detail in the following detailed description of the present invention. Those skilled in the art can fully understand the present invention without the description of these details.

[0091] Reference Figures 1 to 11 A covariance adaptive control method for a quadruped robot with spinal joint adjustment is proposed to solve the problem that the existing quadruped robots have a single posture adjustment method and limited foot movement space when affected by terrain without spinal joints.

[0092] like Figure 1 The present invention proposes a covariance adaptive control method for a quadruped robot with spinal joint adjustment, comprising the following steps:

[0093] Step 1: Build a kinematic model of a quadruped robot with a spinal joint and analyze the effects of spinal angle changes on foot end space, center of mass alignment, and trunk moment of inertia. Use single-body dynamics to model the robot's trunk with spinal joints, and use Newton-Euler equations to model the robot's legs with multi-body dynamics. This provides a theoretical basis for controlling a quadruped robot with a spinal joint.

[0094] Step 1 includes the following sub-steps:

[0095] Step 1.1: Figure 2 This is a structural diagram of the spinal quadruped robot described in the present invention. The robot has two trunks, the front and rear trunks are connected by a spinal joint motor, which is fixed to the rear trunk. The relative pitch angle of the front and rear trunks can be adjusted by controlling the spinal joint motor. The front and rear trunks are respectively connected to two legs, and each leg has three joint motors, one for each hip joint, one for each thigh joint, and one for each calf joint.

[0096] Step 1.2: Establish the kinematic model of the spine joint quadruped robot. Figure 3 This is a simplified model of the spinal joint quadruped robot. The quadruped robot body coordinate B is set at the midpoint of the entire trunk, that is, the geometric midpoint of the spinal joint. The length and width of the front and rear trunks are equal. Without considering the movement of the spinal joints, the foot positions of the four legs in the body coordinate system can be obtained through mathematical modeling. Since the trunk will change due to the angle change of the spinal joints, it is necessary to multiply the above mathematical modeling by a rotation matrix rotating around the y-axis to obtain the accurate foot position of each foot in the body coordinate system. The formula is as follows:

[0097]

[0098] in, is the position of each foot end in the body coordinate system, Represents the coordinates of the foot end in the body coordinates, where i is represented by FL, FR, RL, and RR, representing the front left leg, front right leg, back left leg, and back right leg, respectively. is the rotation matrix generated by the rotation of the spinal joint, where θ s The angle of rotation of the spinal joint. Since the front and rear trunks are symmetrical in this invention, the front and rear trunks are set to rotate separately. θ0, θ1, and θ2 are the angle vectors of the hip, thigh, and shank joints, respectively. L is the distance from the body coordinate origin to the thigh joint in the x-axis direction, a1 is the thigh link length, α2 is the shank link length, W1 is the distance from the thigh joint to the shank joint in the y-axis direction, and W0 is the width from the body coordinate origin to the center of the hip joint along the y-axis. Because the motors of the left and right legs and the front and back legs of a real robot are mounted in different directions, their rotation directions may differ within the same coordinate system. Therefore, α and β are set as symbolic variables to distinguish the positions of the quadruped robot legs: α = 1 for the front leg, α = -1 for the back leg; β = 1 for the left leg, β = -1 for the right leg.

[0099] When controlling the motion of a quadruped robot, the partial derivative relationship of the foot position to each joint angle, namely the Jacobian Matrix, is needed in the gait planning and force control process. When controlling the foot trajectory, the Jacobian Matrix is ​​needed to map the change of the joint angle to the foot velocity. When realizing contact force control, the Jacobian Matrix is ​​needed to convert the force / torque in the foot space into the force / torque in the joint space. In order to construct the Jacobian Matrix, the foot position needs to be The partial derivatives of the joint variables θ0, θ1, and θ2 are used to obtain the Jacobian matrix of the spine joint quadruped robot. The formula is as follows:

[0100]

[0101] Step 1.3: Establish a dynamic model of a spinal joint quadruped robot. In the present invention, considering that the spinal quadruped robot has a complex structure and a more complex trunk structure than an ordinary quadruped robot, directly establishing multi-rigid body dynamics is not conducive to calculation and control. Therefore, in the present invention, the trunk and legs are separated for dynamic modeling. First, the dynamic model of the spinal quadruped robot is simplified and divided into two parts: trunk control containing spinal joints and leg control. In the present invention, single rigid body dynamics is used to perform dynamic modeling of the trunk. It is necessary to know the position of the center of mass of the entire trunk in the body coordinates and the world coordinates. Therefore, it is necessary to calculate through the relationship between the world coordinates and the body coordinates, that is, the posture of the body coordinate system and the coordinates of the body origin in the world coordinate system. The formula is as follows:

[0102]

[0103] Among them, in the body coordinate system, the coordinates of the center of mass of the rear rigid body are Coordinates of the center of mass of the front rigid body It can be calculated by the rotation of the spinal joints. is the position of the system center of mass in the body coordinates, is the position of the system center of mass in world coordinates, R WB is the orientation of the robot, that is, the posture of the body coordinate system, is the position of the robot, that is, the coordinate of the origin of the robot in the world system;

[0104] The trunk of a quadruped robot with a spine consists of two parts. The moment of inertia of the entire trunk changes with the change of the spine angle. In order to model the entire trunk using single-body dynamics, it is necessary to calculate the new moment of inertia of the system in advance. The two trunk segments connected by the spine are considered as a whole, and the single-body dynamics is established based on this. The formula is as follows:

[0105]

[0106] in, is the moment of inertia of the robot in world coordinates, R i is the orientation of rigid body i in the world system, is the local inertia tensor of the rigid body at its own center of mass, m i is the mass of the front and rear torso, is the position of the rigid body's center of mass relative to the system's center of mass (in world coordinates); f s(i) To support the force of the legs and feet on the trunk, P gi is the distance from the foot end to the total center of mass, is the angle and angular velocity of the total center of mass in world coordinates;

[0107] The dynamic modeling of the quadruped robot legs is carried out. In order to achieve precise control of the foot ends of each leg of the quadruped robot, the present invention uses a recursive method based on the Newton-Euler method to establish the dynamic model of a single leg. In the modeling process, the angular velocity ω of each joint is derived step by step. i , angular acceleration and the center of mass a ci The recursive relationship of acceleration is used to obtain the motion state of each connector under the chain mechanism. The recursive relationship is as follows:

[0108]

[0109] Among them, all quantities are expressed in the coordinate system of the link origin of each rigid body, q is the joint angle vector, z i is the current joint axis direction (unit vector), r ci is the vector from the connecting rod origin to the center of mass, is the rotation matrix from the previous level coordinates to this level coordinates. After obtaining the motion state of each link, we can further solve the relationship between its momentum change and force, and construct the standard form of the joint space dynamics equation: Where M(q) is the inertia matrix, is the Coriolis / centrifugal term, G(q) is the gravity term;

[0110] Step 2: Introduce a terrain slope estimation algorithm that integrates the inertial measurement unit (IMU) and the foot contact point information to obtain the current ground information in real time. Based on the energy stability margin theorem and the spinal joint coupling effect, a covariance adaptive evolutionary strategy that integrates spinal joint adjustment is proposed: the covariance adaptive evolutionary strategy (CMA-ES) is used to solve the optimal solution of the robot posture offline, and the online table lookup interpolation method is used to calculate the optimal solution of the posture in real time during the robot's movement. The angle, stiffness and damping coefficient of the spinal joint are adjusted according to the terrain slope estimation to achieve adaptive adjustment of the spinal joint under different terrains.

[0111] The step 2 includes the following sub-steps:

[0112] Step 2.1: Analyze the influence of spinal joints on the stability of the quadruped robot, such as Figure 4 As shown, Figure 4 Figure a shows the state of the rigid trunk quadruped robot on the slope without adjusting its posture, and figure b shows the state of the rigid trunk quadruped robot on the slope after adjusting its posture. In order to prevent the body from overturning, its center of gravity needs to be shifted toward the inside of the slope to improve the stability of the robot system. Figure c shows the posture adjustment of the quadruped robot with spinal joints on the slope. In the figure, you can see the ESM 11 <ESM 21 ≈ESM 31Through the analysis of energy stability margin, it can be found that b and c have similar stability, and both b and c are more stable than a. By comparing a and b, it can be found that the center of gravity adjustment process of the rigid trunk robot needs to be achieved by adjusting the height of the front and rear legs; however, this method will inevitably compress the foot end motion space of the hind legs, resulting in a limited range of foot end trajectory generation, which is not conducive to gait planning and dynamic motion performance. However, robots with spinal joints can achieve fine-tuning of the body's posture in the pitch direction by actively adjusting the spinal pitch angle. The spinal joint adjusts the angle θ s When the foot is in the air, it can cause the center of gravity to shift on the horizontal axis and the vertical axis, which improves stability while still allowing the foot to have a larger trajectory generation range.

[0113] Step 2.2: Figure 5 This is a schematic diagram of the robot estimating terrain information. Combining the inertial measurement units of the front and rear trunks with the spinal robot kinematic equation established in step 1, the terrain slope θ of the environment is calculated by the foot contact position of the quadruped robot. slope , and is continuously updated according to the information of the supporting leg in the subsequent movement. The formula is as follows:

[0114]

[0115] Where, is the position vector of the foot end in the world coordinate system, R WB Represents the rotation matrix from the body coordinate system to the world coordinate system; is the position vector of the foot end in the body coordinate system; Represents the position vector of the robot body in the world coordinate system; record the position of the foot end when it touches the ground, and estimate the ground information based on these point sets. The ground information can be expressed as ax+by+cz+d=0, where n=[a,b,c] T is the ground normal vector, and d is the offset. The terrain slope θ is obtained by calculating the angle between the ground normal vector and the z axis of the world coordinate system. slope , where e z Represents the vertical unit vector in the world coordinate system;

[0116] The spinal joint has the most obvious influence on the pitch angle of the robot. For a quadruped robot with a front and rear trunk, a core reference value θ of the robot's posture is obtained by calculation. b , the formula is as follows:

[0117]

[0118] Among them, the rear trunk posture θ r , front trunk posture θ f , spine angle θ s,θ f It can be achieved through θ r The angle θ of the spinal joint s Calculated;

[0119] Step 2.3: Construct a mathematical model based on the spinal joint posture control method. This method enables the quadruped robot to maintain the stability of the fuselage and the integrity of the foot end motion space in complex terrain with a certain slope, thereby expanding the range of foot end trajectory generation. Based on the above analysis of the relationship between the robot and the terrain, it is necessary to design a cost function. According to this objective function and the constraints, the optimal posture solution is obtained. In the cost function, the robot's posture angle needs to be as small as possible, and the front and rear legs are stretched as equally as possible so that the front and rear feet have sufficient movement space. When going up and down slopes, the rear and front halves are set as horizontal as possible, and the distance between the body's center of gravity and the supporting foot is increased, thereby increasing the energy required for overturning and improving the robot's stability. The cost function is shown in the following formula:

[0120]

[0121] Where, J1(q) is the function of the trunk pitch angle; J2(q) is the function of the distance from the front and back feet to the hip joint; represents the z coordinate of the foot end, h terrain (x i ,y i ) represents the height of the terrain at that location. The entire formula indicates that the foot of the supporting leg needs to touch the ground. J3(q) is the function that sets the rear and front halves of the robot to be as horizontal as possible when going uphill or downhill. J4(q) is the function that maximizes the energy stability margin required by the robot.

[0122] In order to improve the efficiency and real-time performance of this method in practical applications, the present invention adopts an offline and online combined optimization strategy. In this invention, the covariance adaptive evolutionary strategy (CMA-ES) is used for offline optimization. According to the above objective function, a more reasonable optimal posture solution is obtained. The core formula is as follows:

[0123]

[0124] in: represents the kth child in the g+1th generation, m (g) represents the mean of the g-th generation search distribution, C represents the covariance matrix, σ represents the step size, N(0,C ( g ) ) represents a multivariate normal distribution with mean 0 and covariance C, E||N(0,I)|| represents the expectation of the Euclidean norm of a random vector distributed under N(0,I), and w i Represents the positive weight coefficient for recombination, used to calculate the mean of the selection points, pc Represents the evolutionary path corresponding to each generation, p σ represents the evolutionary path of each generation step length, c1 is the learning rate of the covariance matrix rank one update, c μ is the learning rate for updating the covariance matrix rank μ, c σ is the cumulative learning rate controlled by step size, d σ is the decay parameter for step size update;

[0125] Figure 6 This is the flow chart of the covariance adaptive evolutionary strategy (CMA-ES). First, set the initial posture, step size, covariance matrix and other parameters. Then, sample a batch of new posture parameters. Each group x is a complete posture, generating dozens of different posture groups. Calculate according to the established objective function J to make J as small as possible. Then select the best posture solutions of the first few groups for subsequent guidance updates. With the best performing posture groups as the center, adjust the mean posture m. Based on this, update the covariance matrix to explore the search area in the direction of a better solution. Update the search scale, that is, the step size. If the progress of several consecutive generations is large, the step size is enlarged. Otherwise, the step size is shortened for a more refined search. Finally, it tends to stabilize or reaches the set maximum number of generations. Return the current optimal posture parameters, including the angular vector q of each joint of the quadruped robot.

[0126] During the actual operation of the robot, the solution provided by the offline model is used as the initial value, and the online table lookup interpolation method is used for calculation. According to the current terrain state and center of mass state, the offline posture database is retrieved for the closest samples to the current state. The retrieved samples are interpolated and reasoned, and the inferred posture angle is sent as a control instruction to the spine joint and leg joint controllers to adjust the robot's body posture in real time.

[0127] Step 2.4: Combine the ground information and the optimal posture to construct the spinal joint control strategy. In order to reduce the amount of MPC calculation, after calculating the optimal solution of the spinal joint, the spinal joint angle is first controlled. The control target is the torque output of the spinal joint. When the current angle is less than the target angle, a torque will be output to push it closer to the target angle. When the current angle is greater than the target angle, a reverse torque will be generated to reduce the angle. And according to the terrain slope θ slope The robot actively adjusts the damping and stiffness of the spinal joints according to the changes in the slope, so that the robot has a certain shock absorption effect on terrain with a small slope and a certain spinal joint stiffness on steep terrain. When the slope is small, such as a gentle slope or flat ground, shock absorption is prioritized, the stiffness is reduced, and the damping is moderately increased; when the slope is large, such as a steep slope or steps, stability is prioritized, the stiffness is increased, and the damping is moderately reduced. The formula is as follows:

[0128]

[0129] Among them, θ slop is the terrain slope, θ maxslop is the preset maximum slope, K(θ slop ) is the stiffness adjustment function, K max , K min is the upper and lower limits of stiffness, D(θ slop ) is the damping adjustment function, D max 、D min The flatter the terrain, the closer the stiffness is to K. min , the closer the damping is to D max ; The steeper the terrain, the closer the stiffness is to K max , the closer the damping is to D min p and q are power parameters that control the shape of the curve;

[0130] Step 3: Based on the optimal posture solution obtained in step 2, the spatial relationship between the robot's trunk center of mass and the robot's foot is extracted as the desired reference. The desired center of mass position is updated in real time. The model predictive control (MPC) method is used to control the trunk posture of the spinal quadruped robot. Based on the dynamic analysis of the single leg in step 1, the joint motor output torque is precisely adjusted through a control method that combines position-velocity feedback with feedforward compensation to achieve tracking of the planned trajectory by the foot.

[0131] Step 3 includes the following sub-steps:

[0132] Step 3.1: Construct a model predictive control (MPC) method to control the trunk posture of the spinal quadruped robot. In this invention, the robot motion control adopts a predictive control method based on a single rigid body dynamics model. Considering that the center of mass and posture of the quadruped robot change dramatically during the interaction between the fuselage and the terrain, a set of dynamic state transfer equations in discrete time are constructed for MPC solution. These include the robot center of mass motion equation and the robot posture motion equation (around the center of mass). The dynamic model is based on the robot body center of mass position c k , center of mass velocity Body angular velocity ω k and attitude angle θ k As a state variable, the control input is the contact force f applied to each supporting foot end i,k , and integrated into the standard x k+1 =f(x k ,u k ) is solved. Specifically, the updated form of the system state in each control cycle k is as follows:

[0133]

[0134] Where h is the discrete time step, representing the length of the MPC control cycle; M is the overall mass of the robot; is the moment of inertia matrix of the fuselage in the world coordinate system; g is the gravity acceleration vector. The force f applied by each foot end i,k To optimize the variables, MPC is used to minimize the objective function to achieve predictive control of the robot's body stability and motion performance;

[0135] Figure 7 is a schematic diagram of the optimal posture solution. When constructing the objective function of MPC, the optimal posture solution obtained by nonlinear optimization in step 2 is introduced. The spatial relationship between the ideal foot position and the ideal center of mass position is extracted and defined as the ideal relative position vector. The ideal relative position between the foot end and the center of mass of the i-th supporting leg in the ideal state is assumed to be:

[0136]

[0137] Where, is the ideal center of mass position, For the ideal foot position of the supporting leg, both values ​​are obtained by using the joint angle vector obtained in step 2.3 according to the kinematic equation in step 1; r i opt represents the position vector of the center of mass relative to the i-th foot end in the optimal posture state;

[0138] When performing attitude control, let N s The leg is in the ground contact phase, so for each control cycle k, it is necessary to adjust the actual foot position of the robot according to its current position. Combined with the above ideal relative position relationship between the center of mass and the foot of the supporting leg, r i opt , calculate the expected center of mass position c at the current moment in real time ref,k , the formula is as follows:

[0139]

[0140] The c calculated above is ref,k As the target position in predictive control, the final MPC objective function is constructed. Figure 8 The figure shows the control diagram for the support phase, in which the current center of mass position, the desired center of mass position, the posture angle, and the foot force constraint are used as the input of the MPC to solve the foot end of the supporting leg. Then, the torque required to be output by each joint motor is solved through the Jacobian matrix to control the quadruped robot's trunk posture. The formula is as follows:

[0141]

[0142] Where: c k,θ k are respectively the center of mass position and attitude angle at the kth moment under MPC prediction; θ ref,k is the attitude reference angle obtained by nonlinear optimization; f i,k is the contact force of the i-th foot end; Q, R, W are weight matrices used to adjust the optimization priority of each part; It is a friction cone constraint to prevent the foot end from sliding;

[0143] Step 3.2: Based on the leg dynamics model in step 1, the end trajectory tracking method is used to design the swing leg control. Figure 8 The figure shows a flow chart of the swing phase control. The target position, velocity, and acceleration of the trajectory planning are input. The control goal is to make the foot end reach the specified landing point accurately and control the velocity direction before touching the ground to achieve anti-slip and stable landing. The controller uses position acceleration feedback, combined with the feedforward compensation term solved based on the dynamic model, and uses the Jacobian matrix obtained in step 1 to solve the torque required for each motor to achieve the torque control output in the joint space. The formula is as follows:

[0144]

[0145] Where, τ i Control the output joint torque for the swing leg; is the foot-end Jacobian matrix obtained in step 1; is the reference foot end position and velocity in the body coordinate system; P i B 、V i B is the current foot end position and speed in the body coordinate system; K p , K d is the foot-end PD controller gain; τ i,ff It is the feedforward compensation torque used to offset inertia and gravity; is the inertia matrix of the operating space; is the expected foot acceleration in the body coordinate system; for

[0146] Non-inertial acceleration of the foot; is the compensation term for centrifugal force and Coriolis force; G i is the gravity moment compensation term;

[0147] Step 4: Combining the additional degrees of freedom provided by the spinal joints with the MPC predicted state information, the swing leg landing point is pre-designed. Combined with ground information, a third-order Bezier curve is used to generate the foot trajectory, enhancing the foot's ability to conform to the terrain and further improving the robot's stability in complex terrain.

[0148] The step 4 includes the following sub-steps:

[0149] Step 4.1 Plan the foot-end landing point and foot-end trajectory. Due to the role of the spinal joint, the foot-end motion space of the spinal quadruped robot in various environments is increased, so the foot-end trajectory can be better planned for terrain adaptability. The center of mass trajectory is composed of the center of mass position of the optimal posture and the expected robot motion speed and other information. The expected robot motion speed is related to the swing leg. The swing leg control needs to determine the position of the next landing point. However, when the swing leg swings, the robot torso will be adjusted due to the control of MPC, so the center of mass position predicted by MPC needs to be used. Estimate the footfall point and incorporate ground information into it using the following formula:

[0150]

[0151] Where, Indicates the closest step in the current MPC prediction. If the number of steps obtained in the current prediction is insufficient, the value cached in the previous prediction round is used; T swing is the duration of the swing phase; v des is the desired forward speed of the robot (determined by the current motion state). To increase the stability of the system, Δ(θ) is set as the slope correction term, where a controls the forward and backward correction term, and b controls the vertical lift amount.

[0152] Step 4.2: After knowing the foot landing point and ground information, the foot end motion space increased by the spinal joint can be used to plan the foot end trajectory, so that the foot end trajectory can be autonomously adjusted according to changes in terrain information. Figure 9 This is a schematic diagram of the foot trajectory. In the figure, it can be seen that when the robot is going uphill, the foot steps on the ground in the backward and downward direction, and when going downhill, the foot steps on the ground in the forward and downward direction. In order to make the robot have stronger maneuverability and stability, the present invention uses a third-order Bezier curve to generate the foot trajectory. Figure 10 This is a schematic diagram of the third-order Bezier foot trajectory planning for uphill and downhill conditions. The formula is as follows:

[0153]

[0154] In the formula, the starting point is P0; the landing point is P3; P1 is the first control point, which is set at the ratio α1 between the starting point and the landing point, and a Z-axis lift h(θ) is added on this basis, where α1 is the offset ratio and h(θ) is the lift function; P2 is the second control point, which is set above the landing point, where β is the slope factor, which is set here as: β = 1 when uphill, β = -1 when downhill, d f is the advance distance along the terrain tangent, t terrain is the terrain tangent direction;

[0155] The present invention compares the end movement direction and the ideal direction d ideal The angle between the two ends of the trajectory is used to determine whether the trajectory end design is reasonable. If the angle is less than 20 degrees, it means that the trajectory end design is relatively successful. The formula is as follows:

[0156]

[0157] Where n terrain is the terrain normal vector, t terrain is the terrain tangent vector, α is the weighting coefficient, and the expected landing angle can be set by setting the weighting coefficient;

[0158] Step 4.3: The overall motion control process is as follows. It begins with external command input, generates a center of mass trajectory based on the command, and estimates terrain slope information based on the contact position of the quadruped's foot. This terrain estimation information is used in two key areas: spinal joint control and leg swing control. Terrain slope information is transmitted to the spinal joints for controlling spinal joint angle, stiffness, and damping, as well as for determining the optimal posture.

[0159] The support leg control part uses the solved optimal posture information, combined with the current relative position of the center of mass and the foot, and inputs it into the model predictive controller. The MPC controller then solves the control information required for the support leg, including joint output torque, to ensure that the robot can still maintain balance and the desired trajectory on uneven terrain.

[0160] In the swing leg control module, the system inputs terrain information and the center of mass position information output by the MPC into the foot landing point planning module to generate the optimal foot landing point. Based on the terrain information, the complete foot trajectory is calculated to guide the spatiotemporal trajectory control of the swing leg, achieving efficient landing and leg lifting movements.

[0161] Finally, the control output information of the supporting leg and the swinging leg is integrated to generate the final joint torque output matrix, which drives the robot to complete the entire dynamic motion control process. The entire system has good adaptability and robustness, and can maintain the motion stability and flexibility of the quadruped robot under complex terrain conditions.

[0162] The above embodiments are only preferred embodiments of the present invention and are not limitations on the technical solutions of the present invention. Any technical solution that can be implemented on the basis of the above embodiments without creative work should be deemed to fall within the scope of protection of the patent of the present invention.

Claims

1. A covariance adaptive control method for a quadruped robot with spinal joint adjustment, characterized in that: The method comprises the following steps: Step 1: Establish a kinematic model of a quadruped robot with spinal joints. Analyze the effects of spinal angle changes on foot end space, center of mass adjustment, and trunk moment of inertia. Use single-body dynamics to model the robot trunk with spinal joints. Use Newton-Euler formulas to model the robot legs with multi-body dynamics. This provides a theoretical basis for controlling quadruped robots with spinal joints. Step 2: Introduce a terrain slope estimation algorithm that integrates the inertial measurement unit (IMU) and foot contact point information to obtain current ground information in real time. Based on the energy stability margin theorem and the spinal joint coupling effect, propose a covariance adaptive evolutionary strategy that integrates spinal joint adjustment: Utilize the covariance adaptive evolutionary strategy (CMA-ES) to solve the optimal solution for the robot posture offline, and use an online table lookup interpolation method to calculate the optimal solution in real time during the robot's motion. Adjust the angle, stiffness, and damping coefficient of the spinal joint according to the estimated terrain slope to achieve adaptive adjustment of the spinal joint under different terrains. Step 3: Based on the optimal posture solution obtained in Step 2, the spatial relationship between the robot's trunk center of mass and the robot's foot is extracted as the desired reference. The desired center of mass position is updated in real time, and the spinal quadruped robot's trunk posture is controlled using the model predictive control (MPC) method. Based on the dynamic analysis of the single leg in Step 1, the joint motor output torque is precisely adjusted through a control method that combines position-velocity feedback with feedforward compensation, enabling the foot to track the planned trajectory. Step 4: Combine the additional degrees of freedom provided by the spinal joints with the MPC predicted state information to pre-design the landing point of the swing leg. Combined with the ground information, a third-order Bezier curve is used to generate the foot trajectory to enhance the foot's ability to fit the terrain and further improve the robot's stability in complex terrain.

2. The covariance adaptive control method for a quadruped robot with spinal joint adjustment according to claim 1, characterized in that: The step 1 includes the following sub-steps: Step 1.1: The spinal quadruped robot structure is designed as follows. The robot has two trunks, the front and rear trunks connected by a joint motor fixed to the rear trunk. The relative pitch angle of the front and rear trunks is adjusted by controlling the spinal joint motor. The front and rear trunks are connected to two legs respectively. Each leg has three joint motors corresponding to the hip joint, thigh joint, and calf joint. Step 1.2: Establish a kinematic model for a quadruped robot with spinal joints. Set the quadruped robot's body coordinate B at the midpoint of the trunk, which is the geometric midpoint of the spinal joints. The front and rear trunk lengths and widths are equal. Without considering the movement of the spinal joints, mathematical modeling can be used to determine the foot positions of the four legs in the body coordinate system. Because the trunk will change due to changes in the angle of the spinal joints, it is necessary to multiply the above mathematical modeling by a rotation matrix around the y-axis to obtain the exact foot position of each foot in the body coordinate system. The formula is as follows: in, is the foot end position in the body coordinate system, Represents the coordinates of the foot end in the body coordinates, where i is represented by FL, FR, RL, and RR, representing the front left leg, front right leg, back left leg, and back right leg, respectively. is the rotation matrix generated by the rotation of the spinal joint, where θ s The angle of rotation of the spinal joints. Since the front and back trunks are symmetrical, the front and back trunks rotate independently. θ0, θ1 and θ2 are the angle vectors of the hip joint, thigh joint and calf joint respectively, L is the distance from the origin of the body coordinate system to the thigh joint in the x-axis direction, A1 is the length of the thigh link, A2 is the length of the calf link, W1 is the distance from the thigh joint to the calf joint in the y-axis direction, and W0 is the width from the origin of the body coordinate system to the center of the hip joint along the y-axis. Since the joint motors of the left and right legs and the front and rear legs are installed in different directions on the physical robot, their rotation directions will be different in the same coordinate system. Therefore, α and β are set as symbolic variables to distinguish the positions of the quadruped robot legs: α = 1 for the front leg, α = -1 for the rear leg; β = 1 for the left leg, β = -1 for the right leg. Construct the Jacobian matrix for the foot end position The partial derivatives of the joint variables θ0, θ1, and θ2 are used to obtain the Jacobian matrix of the spine joint quadruped robot. The formula is as follows: Step 1.3: Establish the dynamic model of the spinal joint quadruped robot. First, simplify the dynamic model of the spinal joint quadruped robot and divide it into two parts: the trunk control with spinal joints and the leg control. Use single rigid body dynamics to model the trunk dynamics. The relationship between the world coordinates and the body coordinates is used for calculation, that is, the posture of the body coordinate system and the coordinates of the body origin in the world coordinate system. The formula is as follows: Among them, in the body coordinate system, the coordinates of the center of mass of the rear rigid body are Coordinates of the center of mass of the front rigid body It can be calculated by the rotation of the spinal joints. is the position of the system center of mass in the body coordinates, is the position of the system center of mass in world coordinates, R WB is the orientation of the robot, that is, the posture of the body coordinate system, is the position of the robot, that is, the coordinate of the origin of the body in the world system; The trunk of a quadruped robot with a spine consists of two parts. The moment of inertia of the entire trunk changes with the change of the spine angle. In order to model the entire trunk using single-body dynamics, it is necessary to calculate the new moment of inertia of the system in advance. The two trunk segments connected by the spine are considered as a whole, and the single-body dynamics is established based on this. The formula is as follows: in, is the moment of inertia of the robot in world coordinates, R i is the orientation of rigid body i in the world system, is the local inertia tensor of the rigid body at its own center of mass, m i is the mass of the front and rear torso, is the position of the rigid body's center of mass relative to the system's center of mass; f s(i) To support the force of the legs and feet on the trunk, P gi is the distance from the foot end to the total mass center, is the angular velocity and angular acceleration of the total center of mass in world coordinates; The dynamic modeling of the quadruped robot legs is carried out to achieve precise control of the foot ends of each leg of the quadruped robot. The recursive formula of the Newton-Euler method is used to establish the single-leg dynamic model. During the modeling process, the angular velocity ω of each joint is derived step by step. i , angular acceleration and the center of mass a ci The recursive relationship of acceleration is used to obtain the motion state of each connector under the chain mechanism. The recursive relationship is as follows: Among them, all quantities are expressed in the coordinate system of the link origin of each rigid body, q is the joint angle vector, z i is the current joint axis direction, r ci is the vector from the connecting rod origin to the center of mass, is the rotation matrix from the previous level coordinates to this level coordinates. After obtaining the motion state of each link, we can further solve the relationship between its momentum change and force, and construct the standard form of the joint space dynamics equation: Where M(q) is the inertia matrix, is the Coriolis / centrifugal term, and G(q) is the gravity term.

3. The covariance adaptive control method for a quadruped robot with spinal joint adjustment according to claim 1 or 2, characterized in that: The step 2 includes the following sub-steps: Step 2.1: Analyze the influence of the spinal joint on the stability of the quadruped robot. The spinal joint adjusts the angle θ s When the foot is in the air, it can cause the center of gravity to shift on the horizontal axis and on the vertical axis, which improves stability while still allowing the foot to have a larger trajectory generation range. Step 2.2: After the spinal robot enters the motion initialization state, the terrain slope θ of the environment is calculated by combining the inertial measurement units of the front and rear trunks with the spinal robot kinematic equation established in step 1. slope , and is continuously updated according to the information of the supporting leg in the subsequent movement. The formula is as follows: Where, is the position vector of the foot end in the world coordinate system, R WB Represents the rotation matrix from the body coordinate system to the world coordinate system; is the position vector of the foot end in the body coordinate system; Represents the position vector of the robot body in the world coordinate system; record the position of the foot end when it touches the ground, and estimate the ground information based on these point sets. The ground information can be expressed as ax+by+cz+d=0, where n=[a,b,c] T is the ground normal vector, d is the offset, and the terrain slope θ is obtained by calculating the angle between the ground normal vector and the z-axis of the world coordinate system slope , where e z Represents the vertical unit vector in the world coordinate system; The spinal joint has the most obvious influence on the pitch angle of the robot. For a quadruped robot with a front and rear trunk, a core reference value θ of the robot's posture is obtained by calculation. b , the formula is as follows: Among them, the rear trunk posture θ r , front trunk posture θ f , spine angle θ s ,θ f It can be obtained by θ r The angle θ of the spinal joint s Calculated; Step 2.3: Construct a mathematical model based on the spinal joint posture adjustment method. Based on the above analysis of the relationship between the robot and the terrain, it is necessary to design a cost function. Based on this objective function and the constraints, the optimal posture solution is obtained. The cost function is shown in the following formula: Where, J1(q) is the function of the trunk pitch angle; J2(q) is the function of the distance from the front and back feet to the hip joint; represents the z coordinate of the foot end, h terrain (x i ,y i ) represents the height of the terrain at that location. The entire formula indicates that the foot of the supporting leg needs to touch the ground; J3(q) is the function that sets the rear and front halves of the robot to be as horizontal as possible when going uphill and downhill; J4(q) is the function that maximizes the energy stability margin required by the robot; The covariance adaptive evolution strategy CMA-ES is used for offline optimization to obtain a more reasonable optimal posture solution based on the above objective function. The core formula is as follows: in: represents the kth child in the g+1th generation, m (g) represents the mean of the g-th generation search distribution, C represents the covariance matrix, σ represents the step size, N(0,C (g) ) represents a multivariate normal distribution with mean 0 and covariance C, E||N(0,I)|| represents the expectation of the Euclidean norm of a random vector distributed under N(0,I), and w i Represents the positive weight coefficient for recombination, used to calculate the mean of the selection points, p c Represents the evolutionary path corresponding to each generation, p σ represents the evolutionary path of each generation step length, c1 is the learning rate of the covariance matrix rank one update, c μ is the learning rate for updating the covariance matrix rank μ, c σ is the cumulative learning rate controlled by step size, d σ is the decay parameter for step size update; The process of the covariance adaptive evolutionary strategy CMA-ES is as follows: first, set the initial posture, step size, and covariance matrix parameters, then sample a batch of new posture parameters, each group x is a complete posture, generate dozens of different posture groups, calculate according to the established objective function J, make J as small as possible, and then select the best solutions of the first few groups for subsequent guidance updates, take the best performing groups of postures as the center, adjust the mean posture m, and update the covariance matrix based on this, so that the search area is explored in the direction of a better solution, and update the search scale, that is, the step size. If the progress of several consecutive generations is large, the step size is enlarged, otherwise the step size is shortened for a more detailed search. Finally, if it tends to be stable or reaches the set maximum number of generations, return the current optimal posture parameters, including the angular vector q of each joint of the quadruped robot; During the actual operation of the robot, the solution provided by the offline model is used as the initial value, and the online table lookup interpolation method is used for calculation. According to the current terrain state and center of mass state, the offline posture database is retrieved for the closest samples to the current state. The retrieved samples are interpolated and reasoned, and the inferred posture angle is sent as a control instruction to the spine joint and leg joint controllers to adjust the robot's body posture in real time. Step 2.4: Combine the ground information and the optimal posture solution to construct a spinal joint control strategy. After calculating the optimal solution for the spinal joint, first control the angle of the spinal joint. The control target is the torque output of the spinal joint. The formula is as follows: Among them, θ slop is the terrain slope, θ maxslop is the preset maximum slope, K(θ slop ) is the stiffness adjustment function, K max , K min is the upper and lower limits of stiffness, D(θ slop ) is the damping adjustment function, D max 、D min The upper and lower limits of damping. The flatter the terrain, the closer the stiffness is to K min , the closer the damping is to D max ; The steeper the terrain, the closer the stiffness is to K max , the closer the damping is to D min , p and q are power parameters that control the shape of the curve.

4. The covariance adaptive control method for a quadruped robot with spinal joint adjustment according to claim 3 is characterized in that: Step 3 includes the following sub-steps: Step 3.1: Construct a model predictive control (MPC) method to control the trunk posture of the spinal quadruped robot. The robot motion control adopts a predictive control method based on a single rigid body dynamics model. A set of discrete time dynamic state transfer equations are constructed for NPC solution. These include the robot center of mass motion equation and the robot posture motion equation. The dynamic model is based on the robot body center of mass position c k , center of mass velocity Body angular velocity ω k and attitude angle θ k As a state variable, the control input is the contact force f applied to each supporting foot end i,k , and integrated into the standard x k+1 =f(x k ,u k ) is solved, and the updated form of the system state in each control cycle k is as follows: Where h is the discrete time step, representing the length of the MPC control cycle; M is the overall mass of the robot; is the moment of inertia matrix of the fuselage in the world coordinate system; g is the gravitational acceleration vector, and the force f applied by each foot end is i,k To optimize the variables, MPC is used to minimize the objective function to achieve predictive control of the robot's body stability and motion performance; When constructing the objective function of MPC, the optimal posture solution obtained by nonlinear optimization in step 2 is introduced, and the spatial relationship between the ideal foot position and the ideal center of mass position is extracted and defined as the ideal relative position vector. The ideal relative position between the foot end and the center of mass of the i-th supporting leg in the ideal state is assumed to be: Where, is the ideal center of mass position, For the ideal foot position of the supporting leg, both values ​​are obtained from the joint angle vectors obtained in step 2.3 according to the kinematic equations in step 1; represents the position vector of the center of mass relative to the i-th foot end in the optimal posture state; When performing attitude control, let N s The leg is in the ground contact phase, so for each control cycle k, according to the current actual foot position of the robot Combined with the above ideal relative position relationship between the center of mass and the foot of the supporting leg, r i opt , calculate the expected center of mass position c at the current moment in real time ref,k , the formula is as follows: The c calculated above is ref,k As the target position in predictive control, the final MPC objective function is constructed. The current center of mass position, the desired center of mass position, the posture angle, and the foot end force constraint are used as the MPC input to solve the foot end of the supporting leg. Then, the torque required to output by each joint motor is solved through the Jacobian matrix to control the quadruped robot torso posture. The formula is as follows: Where: c k ,θ k are respectively the center of mass position and attitude angle at the kth moment under MPC prediction; θ ref,k is the attitude reference angle obtained by nonlinear optimization; f i,k is the contact force of the i-th foot end; Q, R, W are weight matrices used to adjust the optimization priority of each part; It is a friction cone constraint to prevent the foot end from sliding; Step 3.2 is further based on the leg dynamics model of step 1, and the end trajectory tracking method is used to design the swing leg control. The control input is the target position, velocity and acceleration of the trajectory planning. The control goal is to make the foot end accurately reach the specified landing point and control the velocity direction before touching the ground to achieve anti-slip and stable landing. The controller uses the position acceleration feedback method, combined with the feedforward compensation term solved based on the dynamic model, and uses the Jacobian matrix obtained in step 1 to solve the torque required by each joint motor to achieve the torque control output in the joint space. The formula is as follows: Where, τ i Control the output joint torque for the swing leg; is the foot-end Jacobian matrix obtained in step 1; is the reference foot end position and velocity in the body coordinate system; P i B 、V i B is the current foot end position and speed in the body coordinate system; K p , K d is the foot-end PD controller gain; τ i,ff It is the feedforward compensation torque used to offset inertia and gravity; is the inertia matrix of the operating space; is the expected foot acceleration in the body coordinate system; for Non-inertial acceleration of the foot; is the compensation term for centrifugal force and Coriolis force; G i is the gravity torque compensation term.

5. The covariance adaptive control method for a quadruped robot with spinal joint adjustment according to claim 1 or 2, characterized in that: The step 4 includes the following sub-steps: Step 4.1 Plan the foot-end landing point and foot-end trajectory. Due to the role of the spinal joint, the foot-end motion space of the spinal quadruped robot in various environments is increased, so the foot-end trajectory can be better planned for terrain adaptability. The center of mass trajectory is composed of the center of mass position of the optimal posture and the expected robot motion speed information. The expected robot motion speed is related to the swing leg. The swing leg control needs to determine the position of the next landing point. However, when the swing leg swings, the robot torso will be adjusted due to the control of MPC, so the center of mass position predicted by MPC needs to be used. Estimate the footfall point and incorporate ground information into it using the following formula: Where, Indicates the closest step in the current MPC prediction. If the number of steps obtained in the current prediction is insufficient, the value cached in the previous prediction round is used; T swing is the duration of the swing phase; v des is the desired forward speed of the robot (determined by the current motion state). To increase the stability of the system, Δ(θ) is set as the slope correction term, where a controls the forward and backward correction term, and b controls the vertical lift amount. Step 4.2: After knowing the foothold location and ground information, the foot trajectory can be planned using the foot motion space added by the spinal joints. This allows the foot trajectory to adjust autonomously based on terrain changes. When the robot is walking uphill, the foot steps on the ground in a backward and downward direction, and when walking downhill, the foot steps on the ground in a forward and downward direction. To improve the stability of the quadruped robot, a third-order Bezier curve is used to generate the foot trajectory. The formula is as follows: In the formula, the starting point is P0; the landing point is P3; P1 is the first control point, which is set at the ratio α1 between the starting point and the landing point, and a Z-axis lift h(θ) is added on this basis, where α1 is the offset ratio and h(θ) is the lift function; P2 is the second control point, which is set above the landing point, where β is the slope factor, which is set here as: β = 1 when uphill, β = -1 when downhill, d f is the advance distance along the terrain tangent, t terrain is the terrain tangent direction; By comparing the end motion direction and ideal direction d ideal The angle between the two ends of the trajectory is used to determine whether the trajectory end design is reasonable. If the angle is less than 20 degrees, it means that the trajectory end design is relatively successful. The formula is as follows: Where n terrain is the terrain normal vector, t terrain is the terrain tangent vector, α is the weighting coefficient, and the expected landing angle can be set by setting the weighting coefficient; Step 4.3: The overall motion control process is as follows. The motion control process begins with external command input, generating a center of mass trajectory based on the command. The terrain slope information is estimated based on the position where the quadruped robot's foot contacts the ground. The terrain estimation information is used in two key areas: spinal joint control and leg swing control. The terrain slope information is transmitted to the spinal joints for controlling the spinal joint angle, stiffness, and damping, and is also used to solve the optimal posture. The support leg control part uses the solved optimal posture information, combined with the current relative position of the center of mass and the foot end, and inputs it into the model predictive controller. The MPC controller solves the control information required for the support leg, including the joint output torque, to ensure that the robot can still maintain balance and the desired trajectory on uneven terrain. In the swing leg control module, the system inputs terrain information and the center of mass position information output by the MPC into the foot landing point planning module to generate the optimal foot landing point. Based on the terrain information, the complete foot trajectory is calculated to guide the spatiotemporal trajectory control of the swing leg, achieving efficient landing and leg lifting movements. Finally, the control output information of the supporting leg and the swinging leg is integrated to generate the final joint torque output matrix, which drives the robot to complete the entire dynamic motion control process. The entire system has good adaptability and robustness, and can maintain the motion stability and flexibility of the quadruped robot under complex terrain conditions.

Images