A method for quadruped robot optimal foot force distribution and trajectory tracking

Abstract

The application discloses a method for optimizing foot force distribution and trajectory tracking of a quadruped robot, which comprises the following steps: (1) during a support phase of the quadruped robot, converting a force distribution problem among a virtual force of a trunk, a virtual torque and a given force of a foot end into a layered quadratic programming problem, processing an optimal solution, and realizing priority switching between different tasks at a moment of support phase subsection switching, and respectively realizing robot posture stabilization and center of mass trajectory tracking in the first half and the second half of the support phase; and (2) during a swing phase of the quadruped robot, establishing a numerical relationship between virtual spring stiffness of the swing phase and accelerations of each point of the swing phase, realizing virtual stiffness acceleration adaptive control of the swing phase of the quadruped robot, and combining with proportional and differential control of joint angles of the swing phase to reduce trajectory tracking error of the swing phase. The application optimizes foot force distribution and trajectory tracking performance of the quadruped robot, improves smoothness of the whole control process, and enables the quadruped robot to maintain good dynamic stability during a walking process.

Description

Technical Field

[0001] This invention relates to a method for the distribution of foot force and trajectory tracking of a quadruped robot. This method is optimized based on virtual model control and belongs to the field of quadruped robot control technology. Background Technology

[0002] Quadruped robots are a type of legged robot that has seen rapid development in recent years. Compared with tracked or wheeled robots, they have significant advantages in dealing with complex environmental conditions and have been widely developed in recent years. Common gaits for quadruped robots include trot, bounding, and gallop. Foot force distribution and trajectory tracking performance directly affect the stability of quadruped robots.

[0003] The principle of virtual model control (VMC) is to virtualize each leg of a quadruped robot as a spring and damper. By analyzing the virtual forces acting on the robot's feet, a mapping relationship between virtual forces and joint forces is established using a Jacobian matrix. In terms of control, only the parameters of the corresponding virtual components need to be adjusted to achieve the desired motion characteristics of the controlled object. VMC simplifies the control of the quadruped robot body to the independent control of the virtual rigid body's height, velocity, and posture. Because VMC achieves good force control characteristics without considering complex dynamic calculations, this method can achieve various stable gaits for quadruped robots. While quadruped robots can solve for optimal foot force using quadratic programming, it cannot handle situations with multiple task requirements. In Hierarchical Quadratic Optimization (HQP), the multi-task control problem is redefined as a hierarchical optimization problem. Each level of the hierarchical objective is modeled as a quadratic programming problem. HQP can solve the hierarchical structure of equality and inequality by adding additional linear constraints to lower-priority objectives, ensuring that the solutions to lower-priority objectives belong to the solution space of higher-priority objectives, thus achieving objective focus.

[0004] The gait of the quadruped robot Trot, controlled by a virtual model, is divided into support phase control and rocking phase control. Current virtual model control technology for quadruped robots has the following problems:

[0005] During the support phase, the virtual force of the torso is solved only by the traditional virtual model control algorithm, which results in a large fluctuation range of robot posture angle and ground reaction force of support leg, causing the quadruped robot to lose some walking stability and robustness. During the swing phase, the trajectory tracking error is large due to the influence of the virtual spring model, which in turn affects the foot landing point control during phase switching, making it impossible for the quadruped robot to achieve the expected movement speed. Summary of the Invention

[0006] This invention addresses the shortcomings of existing foot force distribution and trajectory tracking technologies in quadruped robots during walking. Based on virtual model control, it proposes an optimized foot force distribution and trajectory tracking method for quadruped robots, enabling the quadruped robot to maintain good dynamic stability during walking.

[0007] The method for optimizing foot force distribution and trajectory tracking in a quadruped robot according to the present invention adopts the following scheme:

[0008] (1) During the support phase of the quadruped robot, the support phase is divided into the first half and the second half. Hierarchical quadratic programming (HQP) is applied and combined with virtual model control algorithm to convert the two main tasks of posture stabilization and center of mass trajectory tracking in the support phase into quadratic programming problems and solve them. The final optimized foot force is obtained through null space projection. At the moment of segment switching in the support phase, the priority of different tasks is swapped. In the first half of the support phase, the virtual torque of the torso is adjusted to achieve the stability of the robot body after landing. In the second half of the support phase, the virtual force of the torso is adjusted to achieve the center of mass trajectory tracking of the robot before it leaves the ground.

[0009] First, given the center-of-mass trajectory and expected RPY angle of the quadruped robot's support phase, a virtual model control algorithm is used to solve for the virtual force and virtual torque of the torso. The force distribution problem among the virtual force, virtual torque, and given foot force is transformed into a hierarchical quadratic programming (HQP) problem. The two main tasks of the support phase—robot posture stabilization and center-of-mass trajectory tracking—are converted into two QP problems. The optimal solution is processed using the null space projection method, and the priorities of different tasks are swapped at the moment of support phase segment switching to achieve the emphasis between different stages of adjustment and prevent excessive foot virtual force. The two quadratic optimization problems are solved using qpOASES to obtain the optimal foot virtual force, which is then mapped to the torque of each joint using the Jacobian matrix.

[0010] The optimal solution of the hierarchical quadratic programming (HQP) for the first half of the support phase is:

[0011]

[0012] The optimal solution of the hierarchical quadratic programming (HQP) for the latter half of the support phase is:

[0013]

[0014] in This relates to the solution of the trajectory tracking QP problem. It concerns the solution to the attitude stability QP problem. Representing matrix A i The null projection matrix; the null projection matrix is ​​constructed using the pseudo-inverse of the matrix, i.e. Where I i It is the identity matrix. It is matrix A i The false rebellion.

[0015] (2) During the swing phase of the quadruped robot, the expected trajectory of the swing phase is differentiated twice to obtain the expression of the acceleration of each point of the swing phase with respect to time t. The numerical relationship between the virtual spring stiffness of the swing phase and the acceleration of each point of the swing phase is established to realize the virtual stiffness acceleration adaptive control of the swing phase of the quadruped robot. Combined with the joint angle proportional differential control of the swing phase, the tracking error of the swing phase trajectory is reduced.

[0016] Specifically, during the swing phase of the quadruped robot, firstly, based on the expected velocities in each direction of the body, a proportional control of the error between the actual velocity and the target velocity is introduced. The zero-degree angles between the foot and the ground in each direction are calculated, and a smooth expected trajectory for the swing phase is obtained through interpolation polynomials based on these zero-degree angles, achieving smooth movement of the foot during the swing phase. Then, the second derivative function of the expected trajectory for the swing phase is solved to obtain the expression for the acceleration of each point in the swing phase with respect to time t. The ratio of the acceleration at a certain moment to the acceleration at the next moment is defined as the stiffness ratio β at the current moment. A numerical relationship is established between the virtual spring stiffness of the swing phase and the acceleration of each point in the swing phase, achieving virtual stiffness acceleration adaptive control of the quadruped robot's swing phase. Based on this, the expected angles of each joint are calculated through inverse kinematics according to the concept of position control, and compared with the actual angles of each joint, achieving proportional differential control of the joint angles in the swing phase.

[0017] The numerical relationship between the virtual spring stiffness of the oscillating phase and the acceleration at each point of the oscillating phase (the expression for the virtual spring stiffness of the oscillating phase at a certain moment with respect to time t) is as follows:

[0018]

[0019]

[0020]

[0021] stiffnessx(0), stiffness y (0) and stiffness z (0) represents the initial virtual spring coefficients in each direction of the oscillating phase. and Let be the expression for the acceleration in each direction of the oscillating phase with respect to time.

[0022] This invention optimizes the foot force distribution and trajectory tracking performance of a quadruped robot through improvements to the control algorithm, while also enhancing the smoothness of the entire control process, enabling the quadruped robot to maintain good dynamic stability during movement. It has the following characteristics:

[0023] 1. Heuristic methods are applied to analyze the support phase function of a quadruped robot. Hierarchical quadratic programming (HQP) is applied to transform the two main tasks of posture stabilization and center of mass trajectory tracking of the support phase into quadratic programming problems and solve them. The final optimized foot force is obtained through null space projection. At the moment of segment switching of the support phase, the priority of different tasks is swapped. The robot posture stabilization is emphasized in the first half of the support phase, and the center of mass trajectory tracking is emphasized in the second half of the support phase.

[0024] 2. Solve the second-order analytical model of the expected trajectory of the swing phase with respect to time t, obtain the acceleration expression of each point of the swing phase with respect to time t, define the ratio of the acceleration at a certain moment to the acceleration at the next moment as the stiffness ratio β at the current moment, establish the numerical relationship between the virtual spring stiffness of the swing phase and the acceleration of each point of the swing phase, realize the virtual stiffness acceleration adaptive control of the swing phase of the quadruped robot, and optimize the trajectory tracking performance of the swing phase by combining joint angle proportional-derivative control. Attached Figure Description

[0025] Figure 1 This is a block diagram of the control process of the quadruped robot of the present invention.

[0026] Figure 2 This is a schematic diagram of a virtual spring model of the quadruped robot's torso.

[0027] Figure 3 This is a priority ranking diagram for applying null space projection in hierarchical quadratic programming.

[0028] Figure 4 This is a schematic diagram of a virtual spring model of the swinging leg of a quadruped robot. Detailed Implementation

[0029] The entire movement of the quadruped robot is divided into a support phase and a swinging phase, which are alternately switched by a state machine. This invention controls the support and swinging phases of the quadruped robot separately, with the control architecture as follows: Figure 1 As shown.

[0030] During the support phase, in order to track the desired movement and posture of the torso, the torso and supporting foot are treated as a virtual spring model, such as... Figure 2 Since the mass of the legs is very small compared to the mass of the torso, the mass of the legs is ignored during the control process. Force analysis of the supporting foot reveals the virtual forces and virtual moments acting on the torso, which can be obtained from the following equations:

[0031]

[0032] Among them, F b and T b These are the virtual force and virtual torque acting on the robot's torso, f.F and f H The forces are applied to the ends of the front and rear supporting feet, respectively. b P F and b P H These represent the positions of the front and rear supporting feet in the torso coordinate system. Based on the virtual model control, the virtual forces and virtual moments acting on the torso can be obtained from the following formula:

[0033]

[0034]

[0035]

[0036]

[0037]

[0038]

[0039] Among them, F x F y F z T x T y and T z These represent the virtual forces and virtual moments acting on the center of mass in each direction after applying the virtual model control. Stiffness and damping are the virtual spring coefficient and virtual damping coefficient of the applied virtual spring model, respectively. d y d z d α d β d and γ d These represent the desired position and desired attitude angle of the robot's center of mass in each direction, respectively, while x, y, z, α, β, and γ represent the real-time position and real-time attitude angle of the center of mass in each direction, respectively. and These are the expected velocities and expected attitude angular velocities in each direction of the center of mass. and These are the actual velocities and actual attitude angular velocities in each direction of the center of mass.

[0040] After a quadruped robot switches to the support phase, it needs to achieve body stability and center of mass trajectory tracking. During the support phase of the quadruped robot, this invention divides the support phase into a first half and a second half, focusing on achieving different performance requirements in different segments; in the first half of the support phase, the focus is on adjusting the virtual torque of the torso to achieve body stability after the robot lands; in the second half of the support phase, the focus is on adjusting the virtual force of the torso to achieve center of mass trajectory tracking before the robot leaves the ground.

[0041] To transform the virtual forces and virtual moments acting on the torso into two separate quadratic programming (QP) problems, and to achieve hierarchical quadratic programming (HQP), we can obtain the following from the above equation:

[0042]

[0043]

[0044] Among them, [F x F y F z ] T and [T] x T y T z ] T These are matrices B1 and B2 from the QPs problem. and These are the A1 and A2 matrices from the QPs problem, [f Fx f Fy f Fz f Hx f Hy f Hz ] T x is the solution to the proposed hierarchical quadratic programming problem.

[0045] To simplify the effect of gravity, we assume that the roll and pitch angles of the robot's torso are very small, and then introduce a gravity term into the virtual force in the vertical direction.

[0046] Based on the above definition, this invention transforms the solution for optimal plantar force based on the desired virtual trunk force and virtual moment into an HQP problem. HQP uses null space projection to process the final optimized solution, making the final solution more focused on completing one of the tasks, thereby achieving priority control of virtual trunk force and virtual moment at different stages of the support phase. Therefore:

[0047]

[0048] Where, x i This is the same decision variable vector for all QPs, representing the forces acting on the front and rear supports in each direction. The subscript i indicates the task priority. Q∈R 3×3 This is a weighted diagonal matrix used to control the magnitude of the influence of plantar force on virtual forces and virtual moments in the torso. W∈R 6×6 It is a penalty diagonal matrix to prevent excessive plantar force. To prevent foot slippage, a friction cone constraint is added, where μ is the friction coefficient, and a constraint with a vertical force greater than 0 is also added.

[0049] In this invention, since the quadruped robot has only two task priorities during the support phase, and the priorities of the attitude stabilization and centroid tracking tasks are swapped at the instant of switching between the first and second halves of the support phase, the robot's attitude stabilization performance has a higher priority in the first half of the support phase in order to reduce the change in the robot's body attitude and enable the robot to quickly reach the desired state after landing. Therefore, the optimal solution for HQP in the first half of the support phase is:

[0050]

[0051] In the latter half of the support phase, to reduce the tracking error of the robot's center of mass trajectory and ensure that the robot reaches the desired position and has the acceleration for takeoff before leaving the ground, tracking the robot's center of mass trajectory has a higher priority. Therefore, the optimal solution for the latter half of the support phase is:

[0052]

[0053] in This relates to the solution of the trajectory tracking QP problem. It concerns the solution to the attitude stability QP problem. Representing matrix A i The null space projection matrix. This invention uses the pseudo-inverse of the matrix to construct the null space projection matrix, i.e., N. i =I i - Where I i It is the identity matrix. It is matrix A i The pseudo-inverse. The process of applying null space projection to process the solution is as follows: Figure 3 According to τ=-J T x * The optimal supporting foot force obtained at the end can be used to obtain the optimal input torque of each joint of the supporting foot through the Jacobian matrix.

[0054] The primary task of a quadruped robot during its wobbling phase is to control its speed by manipulating the angle between the virtual spring and the ground when it lands. Therefore, accurate tracking of the foot trajectories is particularly important during this phase. However, due to the hysteresis characteristic of the virtual spring, tracking the foot trajectories is less effective during the wobbling phase of a quadruped robot.

[0055] During the swaying phase, the swaying foot is considered as a virtual model, such as... Figure 4 First, the robot's speed in all directions is controlled by foot placement control. Simultaneously, proportional control is introduced to account for the error between the actual speed and the target speed, thus obtaining the actual coordinates of the foot placement point. To obtain a smooth trajectory curve for the swing phase, state constraints are applied to the foot trajectory of the swing phase, resulting in smooth polynomial interpolated foot trajectories in all directions of the swing phase.

[0056] Based on the virtual model control, the virtual force of the swing leg can be obtained as follows:

[0057]

[0058]

[0059]

[0060] Among them, F x F y and F z These are the virtual forces acting on the hindfoot in various directions, controlled by a virtual model. Stiffness and damping are the virtual spring coefficient and virtual damping coefficient of the applied virtual spring model, respectively. d y d and z d y and z represent the expected positions of the foot in each direction, respectively, while x, y, and z represent the real-time positions of the foot in each direction, respectively. and These are the expected velocities in each direction at the foot. and These represent the actual speeds of the feet in different directions.

[0061] Because a constant spring constant during the swing phase makes it difficult to guarantee accurate trajectory tracking, this invention introduces virtual stiffness acceleration adaptive control into the swing phase of the quadruped robot. This allows the virtual spring constant at the foot end to change with the acceleration of the foot end at different times during the swing phase. Firstly, since the virtual damping coefficient in the virtual model is very small, and it is assumed that the error between the desired and actual positions after trajectory tracking is very small, taking the x-direction as an example, i.e., x... d -x = ε, where ε is a very small constant. Therefore, the virtual forces in each direction of the swing leg can be simplified to expressions that depend only on the virtual spring constant:

[0062] F x ≈stiffness x *ε,

[0063] F y ≈stiffness y *ε,

[0064] F z ≈stiffness z *ε.

[0065] Solving for the second derivative of the trajectory in each direction of the oscillating phase yields the expression for the acceleration in each direction of the oscillating phase with respect to time. and According to F = ma, we can obtain:

[0066]

[0067]

[0068]

[0069] Where m * Given parameters relating to the mass of the oscillating foot, the stiffness in each direction at time t during the oscillating phase is:

[0070]

[0071]

[0072]

[0073] It can be seen that the virtual spring coefficient in each direction of the oscillating phase is proportional to the magnitude of the acceleration in that direction. Comparing the stiffness of the oscillating phase at time t with that at time (t+1), we can obtain:

[0074]

[0075]

[0076]

[0077] Where β x β y and β z It is the defined stiffness ratio of the oscillating phase at each moment, given the initial virtual spring coefficient of the oscillating phase as stiffness. x (0) Stiffness y (0) and stiffness z (0), then the expression for the virtual spring stiffness at a certain moment of the oscillating phase with respect to time t is:

[0078]

[0079]

[0080]

[0081] Since the acceleration in all directions of the oscillating phase is zero when t≥0, the virtual spring stiffness may be zero or too large. Therefore, the following constraint is added:

[0082] 0.1*stiffness x (0)≤stiffness x (t)≤10*stiffness x (0),

[0083] 0.1*stiffness y (0)≤stiffness y (t)≤10*stiffness y (0),

[0084] 0.1*stiffness z (0)≤stiffness z (t)≤10*stiffness z (0).

[0085] After introducing virtual stiffness acceleration adaptive control for the oscillating phase, the virtual force of the oscillating phase becomes:

[0086]

[0087]

[0088]

[0089] After obtaining the virtual force at the foot tip at each moment of the swing phase, the input torque of each joint of the swing foot can be calculated using the Jacobian matrix. Furthermore, to further refine trajectory tracking performance, based on the concept of position control, the expected angle of each joint of the swing foot at each moment is calculated using inverse kinematics, and compared with the actual angle of each joint at each moment of the swing foot, thus achieving proportional-derivative control of the joint angles in the swing phase. Therefore, the final input torque of each joint in the swing phase is:

[0090]

[0091]

[0092]

[0093] Where, k θ and d θ These are the proportional and derivative coefficients for the joint angle proportional-derivative control, θ(t) and θ... d (t) represents the actual angle and the expected angle of each joint, respectively. and These are the actual angular velocity and the expected angular velocity of each joint, respectively.

Claims

1. A method for optimizing foot force distribution and trajectory tracking in a quadruped robot, characterized by: (1) During the support phase of the quadruped robot, the support phase is divided into the first half and the second half of the support phase. By applying hierarchical quadratic programming and combining virtual model control algorithm, the two main tasks of posture stability and center of mass trajectory tracking in the support phase are converted into quadratic programming problems and solved. The final optimized foot force is obtained through null space projection. At the moment of segment switching in the support phase, the priority of different tasks is swapped. In the first half of the support phase, the virtual torque of the torso is adjusted to achieve the stability performance of the robot body after landing. In the second half of the support phase, the virtual force of the torso is adjusted to achieve the center of mass trajectory tracking of the robot before leaving the ground. (2) During the swing phase of the quadruped robot, the expected trajectory of the swing phase is differentiated twice to obtain the expression of the acceleration of each point of the swing phase with respect to time t. The numerical relationship between the virtual spring stiffness of the swing phase and the acceleration of each point of the swing phase is established to realize the virtual stiffness acceleration adaptive control of the swing phase of the quadruped robot. Combined with the joint angle proportional differential control of the swing phase, the tracking error of the swing phase trajectory is reduced.

2. The method for optimizing foot force distribution and trajectory tracking of a quadruped robot according to claim 1, characterized in that: The specific process of (1) is as follows: First, given the center-of-mass trajectory and expected RPY angle of the quadruped robot's support phase, a virtual model control algorithm is used to solve for the virtual force and virtual torque of the torso. The force distribution problem among the virtual force, virtual torque, and given force at the foot is transformed into a hierarchical quadratic programming problem. The two main tasks of the support phase—robot posture stabilization and center-of-mass trajectory tracking—are converted into two QP problems. The optimal solution is processed using the null space projection method, and the priorities of different tasks are swapped at the moment of support phase segment switching to achieve the emphasis between the adjustment amounts at different stages and prevent excessive virtual force at the foot. The two transformed quadratic optimization problems are solved to obtain the optimal virtual force at the foot, which is then mapped to the torque of each joint using the Jacobian matrix.

3. The method for optimizing foot force distribution and trajectory tracking of a quadruped robot according to claim 1, characterized in that: The optimal solution of the hierarchical quadratic programming for the first half of the support phase in (1) is: in This relates to the solution of the trajectory tracking QP problem. It concerns the solution to the attitude stability QP problem. Representing matrix A i The null projection matrix; the null projection matrix is ​​constructed using the pseudo-inverse of the matrix, i.e. Where I i It is the identity matrix. It is matrix A i The false rebellion.

4. The method for optimizing foot force distribution and trajectory tracking of a quadruped robot according to claim 1, characterized in that: The optimal solution of the hierarchical quadratic programming for the latter half of the support phase described in (1) is: in This relates to the solution of the trajectory tracking QP problem. It concerns the solution to the attitude stability QP problem. Representing matrix A i The null projection matrix; the null projection matrix is ​​constructed using the pseudo-inverse of the matrix, i.e. Where I i It is the identity matrix. It is matrix A i The false rebellion.

5. The method for optimizing foot force distribution and trajectory tracking of a quadruped robot according to claim 1, characterized in that: The numerical relationship between the virtual spring stiffness of the oscillating phase and the acceleration at each point of the oscillating phase in (2) is as follows: stiffness x (0) Stiffness y (0) and stiffness z (0) represents the initial virtual spring coefficients in each direction of the oscillating phase. and Let be the expression for the acceleration in each direction of the oscillating phase with respect to time.

Images