Quadruped robot walking stability control method based on virtual model

Abstract

The invention relates to the technical field of robot control, and discloses a quadruped robot walking stability control method based on a virtual model, a virtual model control method is used, and a motion model of a quadruped robot is simplified by a virtual work principle. And meanwhile, a pose transformation instruction is converted into a moment instruction of a leg joint, so that complete machine control is realized. Through calculation and simulation experiment analysis, under the condition of reasonable rigidity coefficient and damping coefficient, the advancing and yawing distance and the foot end track of the robot are improved, the body course and the body stability are also greatly improved, and meanwhile, the working efficiency of the robot is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and more particularly to a walking stability control method of a quadruped robot based on a virtual model. Background Art

[0002] During walking, robots may experience problems such as foot slippage and hind legs dragging on the ground due to external interference. Relying on pure kinematics for position control and trajectory optimization is not ideal. In this study, the hip joints of the quadruped robot's legs are used as the base coordinates. Based on the principle of virtual work, a virtual leg model control is established, converting the commands for body posture transformation into underlying torque control for each leg joint. To address the problem of heading control during movement, a lateral virtual force is added to the hip joint when the robot is in the support phase, which is converted into the joint torque required for the robot to turn, thereby achieving stable walking and heading control. Summary of the Invention

[0003] In order to overcome the above-mentioned defects in the prior art, the present invention provides a walking stability control method for a quadruped robot based on a virtual model, which improves the robot's travel and yaw distance and foot trajectory, greatly enhances the body heading and body stability, and also improves the robot's work efficiency.

[0004] The above technical objectives of the present invention are achieved through the following technical solutions: A method for controlling the walking stability of a quadruped robot based on a virtual model, comprising the following steps:

[0005] S1. Given the desired motion trajectory of the quadruped robot;

[0006] S2, using sensors to measure the actual parameters of the robot under foot-end trajectory drive, namely heading angle and heading speed;

[0007] S3. Using the error relationship between the desired heading angle and heading angular velocity, and the actual heading angle and heading angular velocity, and integrating the virtual model control, the lateral virtual force required for the body to turn in the world coordinate system is calculated; and the change in the body's lateral virtual force is added to the foot end virtual force;

[0008] S4. Convert the virtual force at the foot end into the joint torque required for the steering of each joint to achieve stable walking and heading control of the quadruped robot.

[0009] Furthermore, step S1 specifically includes: building a quadruped robot model and simulation environment, and setting a desired trajectory.

[0010] Furthermore, in step S3, the control extension principle of the fused virtual model is:

[0011] (1) Taking the right front leg as an example, as shown in the following formula (6), the coordinate position of the quadruped robot foot end at the side swing joint is expressed as:

[0012]

[0013] (2) The calculation of the virtual force at the foot end is shown in the following formula (10):

[0014]

[0015] Where K x , K y , K z is the stiffness coefficient in three directions, B x 、B y 、B z is the damping coefficient; x d 、y d 、z d are the expected coordinates of the foot end track in three directions, and x, y, and z are the actual coordinate values ​​of the foot end position;

[0016] (3) The change in the lateral virtual force of the body is calculated based on the expected parameters and the actual parameters obtained by feedback, which is expressed as Δf y , we get the following formula (12):

[0017]

[0018] Where, ψ d and ψ are the desired heading angle and the actual heading angle, and are the desired and actual heading angular velocities, and W is the lateral distance from the robot's center of mass to the side swing joint;

[0019] The calculation formula for adding the change of the body's lateral virtual force to the foot end virtual force is as follows (13):

[0020]

[0021] Where, f x 、f y and f z Represent the virtual forces in three directions respectively.

[0022] Furthermore, in step S4, the partial derivatives of the three joint angles in the coordinates of the three directions are calculated separately to obtain the Jacobian matrix of the right front leg of the quadruped robot. The calculation formula is shown in the following formula (7):

[0023]

[0024] Where s i and ci represents sinθ i and cosθ i ;

[0025] The joint torque is calculated according to the virtual model control principle, as shown in the following formula (4):

[0026] τ=J T F (4)

[0027] Where τ∈R n×1 is the joint torque vector, F∈R n×1 is the force acting in the external environment, J∈R n×m It is the velocity Jacobian matrix, which converts the angular velocity value in the joint space into the velocity value at the foot end.

[0028] In summary, the present invention has the following beneficial effects: By employing a virtual model control method, the present application simplifies the kinematic model of a quadruped robot based on the principle of virtual work. Simultaneously, posture transformation commands are converted into torque commands for the leg joints, enabling complete robot control. Calculation and simulation experiments have shown that, under reasonable stiffness and damping coefficients, the robot's travel and yaw distances and foot trajectory are improved, while the body heading and stability are significantly enhanced, thereby increasing the robot's operating efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 2. This is a schematic diagram of VMC control of the legs of a quadruped robot according to an embodiment of the present invention;

[0030] Figure 2 2 is a schematic diagram of yaw control of a quadruped robot according to an embodiment of the present invention;

[0031] Figure 3 This is a schematic diagram of the overall VMC control principle of the quadruped robot in an embodiment of the present invention;

[0032] Figure 4 is a comparison diagram of foot end trajectory shapes under different damping coefficients in an embodiment of the present invention;

[0033] Figure 5 1 is a comparison diagram of forward distances under different damping coefficients in an embodiment of the present invention;

[0034] Figure 6 is a comparison diagram of yaw distances under different damping coefficients in an embodiment of the present invention;

[0035] Figure 7 1 is a comparison diagram of the center of mass height changes under different damping coefficients in an embodiment of the present invention;

[0036] Figure 8 is a comparison diagram of foot end trajectory shapes under different stiffness coefficients in an embodiment of the present invention;

[0037] Figure 9 1 is a comparison diagram of the forward distance under different stiffness coefficients in an embodiment of the present invention;

[0038] Figure 10 is a comparison diagram of yaw distances under different stiffness coefficients in an embodiment of the present invention;

[0039] Figure 11 1 is a comparison diagram of the changes in center of mass height under different stiffness coefficients in an embodiment of the present invention;

[0040] Figure 12 is a diagram showing displacement of the center of mass of the machine body in three directions in an embodiment of the present invention;

[0041] Figure 13 The following are comparison diagrams of displacement changes in three directions of the robot in the embodiment of the present invention; (a) displacement comparison in the x-direction; (b) displacement comparison in the y-direction; (c) displacement comparison in the z-direction;

[0042] Figure 14 The following are comparison diagrams of the yaw angle and angular velocity changes of the robot in the embodiment of the present invention; (a) yaw angle comparison; (b) yaw angular velocity comparison;

[0043] Figure 15 is a walking trajectory diagram of a quadruped robot in an embodiment of the present invention;

[0044] Figure 16 is a diagram showing changes in the heading angular velocity and heading angle of the aircraft body in an embodiment of the present invention;

[0045] Figure 17 1 is a displacement diagram of three directions during the movement of the machine body in an embodiment of the present invention;

[0046] Figure 18 1 is a diagram showing the change in the lateral swing joint torque of each leg in an embodiment of the present invention; (a) lateral swing torque of the left front leg; (b) lateral swing torque of the left hind leg; (c) lateral swing torque of the right front leg; (d) lateral swing torque of the right hind leg;

[0047] Figure 19 1 is a diagram showing the change in torque of three joints of a single leg in an embodiment of the present invention; (a) change in torque of the lateral swing joint; (b) change in torque of the hip joint; and (c) change in torque of the knee joint. DETAILED DESCRIPTION

[0048] The following is combined with Figure 1-19 The present invention is described in further detail.

[0049] Example: The principle of virtual model control is to use non-existent imaginary components to establish a relationship between the force points of the mechanism and the external environment, calculate the virtual forces and virtual torques, and enable the robot to achieve the desired movement.

[0050] A key point to note about virtual model control is that the virtual force acts on an imaginary virtual component, not a real-world mechanism. The virtual force is the resultant force acting on the robot's foot-end point of application, and in actual motion, this is achieved through joint actuation. There is a mapping relationship between foot-end forces and joint torques, determined by the robot's mechanical structure. The robot's forward kinematics model determines the foot's position relative to the robot's leg's lateral swing joint coordinate system, yielding the mapping relationship between the foot's workspace and the joint space:

[0051] x=f(q) (1)

[0052] where x=[x1,x2,...,x n ] is the posture vector of the robot’s body foot relative to space;

[0053] q=[q1,q2,...,q m ] is the position vector in the joint space.

[0054] Further obtain the partial derivative of x with respect to q:

[0055]

[0056] Right now:

[0057]

[0058] According to the principle of virtual work τ T δq+(-F) T δx=0, combining the above formulas, we get:

[0059] τ=J T F (4)

[0060] where τ∈R n×1 is the joint torque vector, F∈R n×1 is the force acting in the external environment, J∈R n×m The velocity Jacobian matrix converts the angular velocity in the joint space to the velocity at the foot. The J matrix changes with the robot joint angle q.

[0061] Virtual model control is a subset of workspace control. For multi-joint robotic arms, precise control requires a complete and complex dynamic model. However, for quadruped robots, the body coordinate system is a floating base coordinate system, lacking absolute position. Its structure resembles a floating body and four robotic arms in parallel. Using workspace control, establishing a dynamic model would be extremely complex, making it unsuitable for quadruped robot control.

[0062] Virtual model control methods are simple and direct, but they cannot achieve precise position or force control. This direct control method does not require precise dynamic modeling. Compared to workspace control, it lacks the high control accuracy required for industrial robots. In contrast, the current goal of motion control for legged robots is to sense environmental interference and impacts on the robot during movement toward a given target and respond promptly with corrective actions. This does not necessarily require high control accuracy. During walking and running, the height and trajectory of the foot do not require strict constraints. Virtual model control simplifies the complex motion model of a quadruped robot, transforming high-level body posture transformation commands into low-level torque control of each leg joint, thereby controlling the robot's motion state. This control method can simply and directly reflect the robot's motion state, effectively enabling interaction between the robot and the external environment and maintaining body stability.

[0063] When controlling a legged robot, spring elements and damping elements are used as virtual components of control. The basic formula for calculating the virtual force is as follows:

[0064]

[0065] In the formula: F u is the virtual force generated by the virtual component; B is the damping coefficient, K is the stiffness coefficient; u d and represents the desired position coordinates and velocity of the foot end; u and Indicates the actual position coordinates and speed.

[0066] Due to the inherent characteristics of this control method, it inevitably introduces control errors during the motion control of a legged robot. The desired joint torques, calculated under the assumptions of a simplified robot model, must also account for inherent system factors such as compensation for inertia, centrifugal force, and friction between structural components. Consequently, the resulting motion will differ from the ideal. By carefully selecting and adjusting the damping and stiffness coefficients in the control model, these differences can be minimized, approaching the target state, and achieving stable walking control for a legged robot, leveraging the algorithm's simplicity and efficiency.

[0067] Leg virtual model control

[0068] Taking the right front leg as an example, the coordinate position of the quadruped robot foot end at the side swing joint derived in the above section is expressed as:

[0069]

[0070] The partial derivatives of the three joint angles are calculated for the coordinates in the three directions, and the Jacobian matrix of the right front leg of the quadruped robot is obtained:

[0071]

[0072] Where s i and c i represents sinθ i and cosθ i .

[0073] When the legs are in the support phase, the virtual forces in the x, y, and z directions are calculated as follows:

[0074]

[0075] When the legs are in the swing phase:

[0076]

[0077] Where K x , K y , K z is the stiffness coefficient in three directions, B x 、B y 、B z is the damping coefficient; x d 、y d 、z d are the expected coordinates of the foot end trajectory in three directions, and x, y, and z are the actual coordinate values ​​of the foot end position.

[0078] Finally, the desired joint torques acting on the three joints of the robot's legs are obtained by using the constructed virtual forces in the three translational directions using formula (4), thus realizing the translational walking motion of the robot.

[0079] like Figure 1 Figure 2 shows a schematic diagram of VMC control of the legs of a quadruped robot.

[0080] Airframe heading control

[0081] During the actual walking process, the quadruped robot does not continuously move in a straight line, so in addition to the translational motion, the robot's turning motion also needs to be considered.

[0082] F' is the virtual force of the support phase in three directions:

[0083]

[0084] The moving direction of the quadruped robot is adjusted by the heading angle. The heading angle of the robot can be adjusted by adding a set of virtual components at the position of the center of mass around the z-axis to generate a virtual torque.

[0085] The formula for controlling the virtual torque of the heading angle is:

[0086]

[0087] Formula ψ d and ψ are the desired heading angle and the actual heading angle, and is the desired angular velocity and the actual angular velocity. d and Given by the upper control instruction, ψ and is the actual measured fuselage heading angle.

[0088] During the movement, the force for the robot to turn is generated by the two legs in the supporting phase. When turning is required, the two legs in the supporting phase generate lateral support forces. The two support forces are equal in magnitude and opposite in direction. Firstly, this ensures that the total lateral force of the robot remains unchanged; secondly, these two forces exert two opposite forces on the head and tail of the robot, which can realize the rotation of the robot around the center of mass. The steering torque is calculated using the lateral force to realize the steering control of the robot. The change in the lateral virtual force of the robot is expressed as Δf y :

[0089]

[0090] Where W is the lateral distance from the robot's center of mass to the side swing joint.

[0091] By converting the lateral forces acting on the lateral swing joints of the two supporting legs into a steering torque acting on the center of mass, the robot can walk at a given desired heading angle. When the heading angle and angular velocity specified by the robot's upper controller change according to a time law, the robot can perform a steering movement. At this time, the virtual force acting on the supporting leg is:

[0092]

[0093] Combine the quadruped robot body posture transformation control and gait planning in the above chapters with the quadruped robot leg VMC control to achieve the overall VMC control of the quadruped robot. Figure 3 Shown is the overall VMC control principle diagram of the quadruped robot.

[0094] During the motion process, if the desired heading angle given by the upper control of the robot is large, the robot will have too large an instantaneous heading angular velocity in order to quickly track during the turning process. The robot's joint rotation angle is too large and exceeds the joint limit, causing the body to become unstable and roll over. To avoid this situation, the robot's heading angular velocity needs to be constrained so that

[0095] Simulation experiment and analysis:

[0096] Conduct simulation experiments in the MATLAB / Simulink simulation environment to verify the results. Improve the simulation framework in the Simulink simulation platform and encapsulate it into various control modules to improve the robot simulation model.

[0097] During motion control, the selection of the stiffness coefficient K and the damping coefficient B is crucial for robot movement. If the gain is too high, the torque acting on the robot's joints will be too large, resulting in excessive rigidity and unstable motion. If the gain is too low, the torque at each joint will be too low, making the robot's body unable to support itself. This can cause the robot's legs to slip during walking. Therefore, simulation experiments are needed to compare the forward and yaw distances, as well as the foot trajectory shape, under different coefficients to determine the optimal stiffness and damping coefficients.

[0098] The influence of damping and stiffness coefficients on body motion

[0099] (1) Analysis of the influence of damping coefficient B

[0100] The stiffness coefficient K in the three directions is set to 2000 N / m, and the damping coefficient B ranges from 50 N / m to 250 N / m, with a total of 5 groups. The foot trajectory uses the foot trajectory planned using the Bezier curve method in the previous chapter. The step length s in the foot trajectory is 0.22m, the leg lift height h is 0.13m, and the gait period T is 0.5s. The yaw angle ψ, pitch angle θ, and roll angle in the initial posture of the quadruped robot are All are set to 0rad, and the height of the aircraft is 0.35m.

[0101] Depend on Figure 4 It can be concluded that when the damping is small, the shape of the foot-end trajectory is not much different from the planned trajectory; as the damping coefficient B increases, during the actual walking process, the amplitude of the robot's foot-end trajectory change gradually decreases and the amplitude tends to be stable, but the difference between the foot-end trajectory and the planned ideal foot-end trajectory shape becomes greater.

[0102] Figure 5 、 Figure 6 The robot's forward distance and yaw distance were observed respectively. It can be seen from the figure that when the damping coefficient B is 50N / m, the yaw of the body is serious, and the turning torque applied to the body at the beginning of the movement is also greater, and the linear motion cannot be completed well; as the damping coefficient B becomes larger, the yaw situation also becomes better, and the amplitude of the change gradually stabilizes. The forward distance of the body becomes shorter due to the influence of the foot end trajectory.

[0103] (2) Analysis of the influence of stiffness coefficient K

[0104] When the damping coefficient B is 100N / m, the stiffness coefficient K is taken in the range of 800N / m to 3200N / m, with a total of six groups. The rest of the conditions remain unchanged, and the simulation is carried out to obtain the experimental data as follows Figure 8 shown.

[0105] from Figure 8 It can be observed that increasing the stiffness coefficient enhances the elastic properties of the object and increases the amplitude, which enables the robot foot trajectory to quickly track the ideal desired trajectory. Then, by continuing to increase the stiffness coefficient and coordinating with the damping coefficient, the deviation between the actual foot trajectory and the ideal foot trajectory will gradually decrease and tend to be stable.

[0106] Analysis of the stiffness coefficient K and damping coefficient B reveals that a larger stiffness coefficient is recommended to minimize the deviation between the actual foot trajectory and the ideal foot trajectory, allowing the foot trajectory to quickly track the desired ideal trajectory. During linear translation, a too large stiffness coefficient in the y-direction will cause lateral displacement when the robot's legs swing, resulting in a larger yaw error. To minimize the robot's yaw error, the stiffness coefficient in the y-direction should be smaller, while a larger damping coefficient is recommended to quickly stabilize lateral displacement deviations.

[0107] Finally, the stiffness coefficient and damping coefficient that enable the robot to move relatively stably under no yaw angle control are selected as follows:

[0108]

[0109]

[0110] Under this set of parameters, the displacement of the body in three directions is as follows Figure 12 shown.

[0111] Aircraft heading control experiment

[0112] Set the expected yaw angle to 0 rad, the simulation time to 60 s, analyze the displacement of the robot in three directions, set the stiffness coefficient in three directions to 3200 N / m, and the damping coefficient to 100 N / m.

[0113] from Figure 13 and Figure 14 As can be seen from the figure, when the desired heading angle is set to 0 rad, the robot yaws during its initial stage due to the rollover torque. Under heading control, the yaw angle decreases, and the yaw velocity stabilizes. Without heading control, the robot continues to move based on the yaw angle caused by the error. The yaw displacement in the y-direction is smaller than without yaw control, and the change in the center of mass height is also smaller. Once the yaw angle stabilizes, the change in yaw velocity remains similar.

[0114] Set the heading angular velocity constraint The robot's travel speed is 1.2m / s. Set the desired heading angle:

[0115]

[0116] Test the robot's ability to continuously turn. The simulation results are as follows: Figure 15 shown.

[0117] Depend on Figure 15 and Figure 16 It can be seen that the robot can complete continuous movement according to the desired heading angle, and the body can complete the required steering task within the yaw angular velocity constraint range. Figure 17 It can be seen that the center of mass of the body changes stably during the movement process. Even during the turning process, the center of mass of the body does not fluctuate greatly, which can ensure smooth movement.

[0118] Figure 18 The figure shows the torques acting on the side-swing joints of the robot's four legs during its turning motion. During the motion, the torques on the side-swing joints of the four legs change evenly. Figure 19 The torque applied to the three joints of a single leg. During steering, the torque required for steering is primarily provided by the yaw joint. Once steering is complete, the yaw joint torque stabilizes to a relatively low value. The joint torques of the hip and knee joints are responsible for maintaining stable walking in straight lines.

[0119] In summary, it is concluded that in order to solve the problems of the front leg ends of the quadruped robot slipping, the hind legs dragging on the ground and yaw under the trajectory planning control relying solely on kinematics, this application uses the virtual model control principle and the virtual work principle, and takes the coordinate system of each lateral swing joint of the robot as the base coordinate system to establish a virtual model control of the legs of the quadruped robot. By comparing the influence of different parameters on the motion under translational linear motion, the gain parameters with more ideal motion effect are determined; in order to solve the heading control problem of the robot during movement, when the robot legs are in the support phase, the error relationship between the target heading angle and the heading angular velocity, and the actual heading angle and the heading angular velocity is used to calculate the lateral virtual force required for the body to turn in the world coordinate system, and converted into the joint torque required for the turning of each joint through Jacobian matrix calculation, thereby realizing stable walking and heading control of the quadruped robot.

[0120] This specific embodiment is merely an explanation of the present invention and is not intended to limit the present invention. After reading this specification, those skilled in the art may make non-creative modifications to this embodiment as needed. However, as long as such modifications are within the scope of the claims of the present invention, they are protected by patent law.

Claims

1. A walking stability control method for a quadruped robot based on a virtual model, characterized in that: The following steps are involved: S1, given the desired motion trajectory of the quadruped robot; S2, using sensors to measure the actual parameters of the robot under foot-end trajectory driving, namely, heading angle and heading speed; S3, using the error relationship between the desired heading angle and the heading angular velocity, the actual heading angle and the heading angular velocity, integrating the virtual model control, and calculating the lateral virtual force required for the body to turn in the world coordinate system; and adding the change of the body's lateral virtual force to the foot end virtual force; S4. Convert the virtual force at the foot end into the joint torque required for the steering of each joint to achieve stable walking and heading control of the quadruped robot.

2. The method for controlling walking stability of a quadruped robot based on a virtual model according to claim 1, characterized in that: Step S1 specifically includes: building a quadruped robot model and a simulation environment, and setting a desired trajectory.

3. The walking stability control method of a quadruped robot based on a virtual model according to claim 1, characterized in that: In step S3, the control extension principle of the fused virtual model is: (1) Taking the right front leg as an example, as shown in the following formula (6), the coordinate position of the foot end of the quadruped robot at the side swing joint is expressed as: (2) The calculation of the virtual force at the foot end is shown in the following formula (10): In the formula, K x , K y , K z is the stiffness coefficient in three directions, B x , B y , B z is the damping coefficient; x d ,y d 、z d are the expected coordinates of the foot end track in three directions, and x, y, and z are the actual coordinate values ​​of the foot end position; (3) The change in the lateral virtual force of the body is calculated based on the expected parameters and the actual parameters obtained by feedback, expressed as Vf y , we get the following formula (12): In the formula, ψ d and ψ are the desired heading angle and the actual heading angle, ψ g d and ψ g is the desired angular velocity and the actual angular velocity, W is the lateral distance from the robot's center of mass to the side swing joint; The calculation formula for adding the change of the body lateral virtual force to the foot end virtual force is as follows (13): In the formula, f x 、f y and f z Represent the virtual forces in three directions respectively.

4. The walking stability control method of a quadruped robot based on a virtual model according to claim 3, characterized in that: In step S4, the partial derivatives of the three joint angles in the coordinates of the three directions are calculated respectively to obtain the Jacobian matrix of the right front leg of the quadruped robot, and the calculation formula is shown in the following formula (7): In the formula, s i and c i represents sinθ i and cosθ i ; The joint torque is calculated according to the virtual model control principle, and the calculation formula is shown in the following formula (4): τ=J T F (4) Among them, τ∈R n×1 is the joint torque vector, F∈R n×1 is the force acting on the external environment, J∈R n×m It is the velocity Jacobian matrix, which represents the conversion of the angular velocity value in the joint space to the velocity value at the foot end.

Images