MPC-based biped robot standing balance control method

Abstract

The invention provides a biped robot standing balance control method based on MPC, and relates to the technical field of robots. The method comprises the following steps: S1, acquiring an external reference signal in a future T time interval; s2, acquiring a current robot state through state estimation; s3, constructing a robot centroid dynamics prediction model based on a single rigid body model, setting a grounding sliding constraint and a sole overturning constraint, and converting a problem into a QP problem for solving; s4, solving an optimal input sequence according to an external reference signal and a robot state; s5, retaining the first-step supporting force and the moment of force of the optimal sequence, and abandoning the predicted values of the remaining steps; s6, calculating a force jacobian matrix of the robot; and S7, the supporting force and the torque are mapped to the joints and issued to all motors. The ground reaction force during standing is directly calculated through the MPC, the supporting force and torque distribution of the two feet are optimized, the precision and stability of balance control are improved, frequent state switching is avoided, foot sole overturning is effectively prevented, and the safety and reliability of the system are enhanced.

Description

Technical Field

[0001] The invention relates to the technical field of robots, and in particular to a standing balance control method of a biped robot based on MPC. Background Art

[0002] Humanoid robots are also called bipedal robots. With the support of technologies such as ChatGPT, large language models, and generative artificial intelligence, the field of humanoid robots has developed rapidly in recent years, and the popularity of related research has been pushed to a new high. Compared with fixed-base robots (such as robotic arms), maintaining standing balance for bipedal robots is a major problem. On the one hand, the bipedal robot model is complex, the joints have higher degrees of freedom, and it is a floating base coordinate system; on the other hand, considering conditions such as friction or unilateral contact, the contact between the sole of the bipedal robot and the ground may be interrupted under slight disturbances. The above two aspects greatly limit the design of control laws at the joint level, because any control law applied to the joints, reflected as the force applied to the contact points, may destroy these contacts and thus lose balance.

[0003] In current robot motion control solutions, force control frameworks are generally used for dynamic walking, but position control is still the main method for static standing. According to the angle information or torque information received by the motor, the robot's motion control can be divided into position control and torque control (referred to as position control and force control). The former requires the joint information required for the robot's motion trajectory in the world coordinate system to be obtained based on inverse kinematics, and the latter requires the joint torque information required for the robot to achieve the motion trajectory to be obtained in combination with the dynamic model. For the robot's balance maintenance in motion (i.e., stable walking or running without falling), the above two control methods can meet the needs. However, for static or semi-static standing maintenance, the existing technology adopts position control. Although position control has advantages over force control in terms of model complexity and solution speed, it still has shortcomings in practical applications. On the one hand, position control aims to control the joint angle. When the size of the robot's two feet is small, in order to maintain the body balance, the actual load of the motor will be large, and the hardware performance requirements are high; on the other hand, position control cannot achieve compliant control. In order to ensure the balance of the body, the robot is relatively hard as a whole, and it is more likely to lose balance as a whole when disturbed by the outside world.

[0004] The existing solutions to the standing balance of bipedal robots use position control or indirect force control. The robot is not flexible enough, especially when facing slightly uneven ground. The load on the ankle motor is too large, which can easily cause hardware damage, and there are restrictions on the hardware size of the robot's feet. In addition, when using position control to achieve standing, the robot must switch between position control and force control modes during the starting stage, which can easily cause the robot to stumble at the start, posing a safety hazard. Summary of the invention

[0005] In order to solve the technical problems in the prior art, the present invention provides a standing balance control method of a biped robot based on MPC.

[0006] The technical solution provided by the present invention is as follows:

[0007] The present invention provides a biped robot standing balance control method based on MPC, comprising:

[0008] S1. The robot obtains an external reference signal from the current moment to the future T time interval:

[0009]

[0010] Among them, the external reference signal Includes the angle at the robot's center of mass Position p, angular velocity ω and linear velocity Four parts, the time interval T includes the future k sampling intervals, and the sampling interval is Δt, that is, T = kΔt;

[0011] S2. Calculate and obtain the current robot state x0 through the state estimation algorithm:

[0012]

[0013] in, represents the angle at the current center of mass, p0 represents the current position, ω0 represents the current angular velocity, Indicates the current line speed;

[0014] S3. Construct a robot center of mass dynamics prediction model based on a single rigid body model, set the robot's ground sliding constraint and foot flip constraint conditions, and construct a quadratic programming problem (QP). The optimization goal is to minimize the error between the robot's center of mass state and the reference signal at the corresponding time in the future T time interval, while the bipedal support force and torque are small;

[0015] S4, according to the external reference signal X ref and the current robot state x0, use the QP solver to solve the quadratic programming problem and obtain the optimal input sequence U, where U includes the predicted future k-step bipedal support force and torque

[0016] S5. Retain the predicted future first step support in the optimal sequence U and torque Discard the remaining k-1 step predictions;

[0017] S6, calculate the force Jacobian matrix J of the robot at this time;

[0018] S7, according to the force Jacobian matrix J, the support force and torque Mapped to each joint, the calculation formula is: And send the results to each motor.

[0019] The beneficial effects brought about by the technical solution provided by the present invention include at least:

[0020] (1) In the present invention, the MPC method is used to directly calculate the ground reaction force required by the bipedal robot in the standing state, so that the control system can effectively distribute the robot's bipedal support force and torque, reduce the direct load demand on the ankle joint, and improve the accuracy and stability of the robot's balance control;

[0021] (2) In the present invention, the control modes of the robot in motion and static states are designed in a unified manner, and there is no need to frequently switch between force control and position control, thereby simplifying the control process, reducing system complexity and instability caused by state switching, and improving the overall control efficiency and response speed;

[0022] (3) In the present invention, taking into account the special situation that the non-flat feet of the robot touch the ground when standing, the present invention introduces ankle joint torque constraints to prevent the sole of the foot from flipping, thereby effectively avoiding hardware damage, extending the service life of the robot's moving parts, and improving the safety and reliability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0024] Figure 1 A schematic flow chart of a method for controlling a bipedal robot's standing balance based on MPC provided in an embodiment of the present invention;

[0025] Figure 2 A block diagram of a bipedal robot standing balance control method based on MPC provided by an embodiment of the present invention;

[0026] Figure 3 A schematic diagram of a single rigid body model of a bipedal robot in a standing state according to an MPC-based bipedal robot standing balance control method provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0027] The technical solution of the present invention is described below in conjunction with the accompanying drawings.

[0028] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "example" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of the word "example" is intended to present the concept in a specific way. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or it can be either of the two.

[0029] In the embodiments of the present invention, "image" and "picture" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same. "of", "corresponding, relevant" and "corresponding" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same.

[0030] In the embodiments of the present invention, sometimes a subscript such as W1 may be mistakenly written as a non-subscript such as W1. When the difference is not emphasized, the meanings to be expressed are consistent.

[0031] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0032] Reference Manual Attached Figure 1 , shows a flow chart of a MPC-based biped robot standing balance control method provided in an embodiment of the present invention.

[0033] The embodiment of the present invention provides a biped robot standing balance control method based on MPC, and the processing flow may include the following steps:

[0034] S1. The robot obtains an external reference signal from the current moment to the future T time interval:

[0035]

[0036] Among them, the external reference signal Includes the angle at the robot's center of mass Position p, angular velocity ω and linear velocity There are four parts, the T time interval includes the future k sampling intervals, and the sampling interval is Δt, that is, T = kΔt.

[0037] S2. Calculate and obtain the current robot state x0 through the state estimation algorithm:

[0038]

[0039] in, represents the angle at the current center of mass, p0 represents the current position, ω0 represents the current angular velocity, Indicates the current line speed.

[0040] S3. Construct a robot center of mass dynamics prediction model based on a single rigid body model, set the robot's ground sliding constraint and foot flipping constraint conditions, and construct a quadratic programming problem (QP). The optimization goal is to minimize the error between the robot's center of mass state and the reference signal at the corresponding time in the future T time interval, while keeping the bipedal support force and torque small. The specific steps are as follows:

[0041] S31. Construct rigid body dynamics model:

[0042]

[0043] in,

[0044]

[0045] R 3,3 represents the rotation matrix, I 3,3 represents the unit matrix, I represents the inertia matrix at the center of mass of the robot, g represents the acceleration of gravity, r1 and r2 represent the vectors from the support points of the left and right legs of the robot to the center of mass, respectively, r1] × represents the antisymmetric matrix formed by the vector r1, and m is the robot mass;

[0046] S32. Discretize the system by sampling to obtain the state space equation of the discretized system:

[0047] x(i+1)=A i x(i)+B i u i

[0048] Among them, the sampling interval is Δ t , A i ,B i ,u i , (i=0,...,k-1) represent the state coefficient matrix, input coefficient matrix and control input of the system at the i-th sampling time respectively;

[0049] S33. Constructing a center of mass dynamics prediction model:

[0050]

[0051] in, x k represents the predicted robot state in the kth step in the future, A and B are the coefficient matrix and input matrix of the prediction model, U represents the predicted input sequence in the kth step in the future, and the coefficient matrix is ​​calculated according to the S32 recursive formula;

[0052] S34. Construct foot contact sliding constraint and foot sole flipping constraint:

[0053]

[0054] Among them, I k,k is the identity matrix, and Represent the left and right foot sliding constraint coefficient matrices respectively, and Respectively represent the flipping constraint coefficient matrices of the left and right foot soles;

[0055] S35. Construct optimization goals:

[0056]

[0057] Among them, Q i and R i They are all parameter diagonal matrices, adjusting the tracking error and predictive power;

[0058] S36. The original problem is expressed as a typical QP problem through tight MPC, that is, a quadratic optimization problem (QP) is constructed:

[0059]

[0060] Where C is the constraint coefficient matrix, H = 2(B T QB+R), f=2B T Q(Ax0-X ref ), Q and R are Q i and R i is a block diagonal matrix.

[0061] S4, according to the external reference signal X ref and the current robot state x0, and use the QP solver to solve the quadratic programming problem to obtain the optimal input sequence U, where U includes the predicted future k-step bipedal support force and torque

[0062] In one possible implementation, the QP solver uses a QPOASES solver.

[0063] S5. Keep the predicted future first step support in the optimal sequence U and torque Discard the remaining k-1 step forward predictions.

[0064] S6. Calculate the force Jacobian matrix J of the robot at this time.

[0065] S7. According to the force Jacobian matrix J, the support force and torque Mapped to each joint, the calculation formula is: And send the results to each motor.

[0066] S8. Update the robot status information and return to S1.

[0067] It should be noted that model predictive control is a forward rolling optimization algorithm. Its core idea is to recursively obtain the state of the system in the future based on the prediction model according to the current state of the system, and use the system input as the optimization variable. After determining the optimization target and constraints, the optimal input sequence (also called the prediction sequence) of the system is solved at each moment, and then the first predicted value in the obtained optimal sequence is used as the current optimal input, and the remaining predicted input values ​​are discarded, and the system state is updated to enter the next round of optimization. The complete model predictive control should include six parts: simplified model construction, prediction model construction, optimization target construction, constraint construction, QP problem transformation, and optimal control solution. Since model predictive control needs to predict the state of the system in the future, it will inevitably lead to a rapid expansion of the system scale. In order to reduce the computational cost, the simplified model of the system is often used as the basis for the construction of the prediction model to increase the efficiency of solving the QP problem. For the control object with a high number of joints and degrees of freedom of the biped robot, the selection of a simplified model is particularly critical.

[0068] like Figure 2 As shown, the embodiment of the present invention provides a biped robot standing balance control method based on MPC, and a biped robot ground reaction force controller based on a single rigid body model. k At this moment, the robot first obtains the reference trajectory X of the center of mass in the future T time interval ref , the prediction step is k, and the sampling interval is Δ t , that is, T = kΔt; at the same time, the robot obtains the current latest state x0 using the state estimation algorithm; the force and torque input sequence required for the robot to maintain standing balance in the next k steps is obtained through the single rigid body model bipedal robot ground reaction controller The force Jacobian matrix J of the robot at this time is calculated based on the current robot joint angle and angular velocity information; the first step prediction value of the ground reaction force controller prediction sequence is transmitted to the simplified inverse dynamics model calculation unit as the optimal ground reaction force, and the ground reaction force is mapped to each joint in combination with the force Jacobian matrix J at this time to obtain the torque required to shut down the motor, and send it to each motor driver, and then complete this round of balance control, waiting for the next round of sampling and updating.

[0069] In order to solve the special situation that the robot is on uneven ground and the feet are not flat, the embodiment of the present invention uses a simplified single rigid body model with two foot soles to model the robot's standing state. Figure 3As shown in the figure, assuming that the robot's legs are relatively light and the overall mass is concentrated in the upper body, the linear momentum and angular momentum equations of the system are as follows:

[0070]

[0071] Among them, f l 、f r , τ l and τ r They represent the resultant force and moment on the left and right feet of a single rigid body model with two soles. Figure 3 After the world coordinate system is established as shown in the figure, each resultant force and resultant moment has component forces and component moments on the x, y, and z axes respectively:

[0072]

[0073] The force component in the z-axis direction supports the robot's combat power, and the force components in the x- and y-axis directions offset the static friction. Similarly, the torque along the x-, y-, and z-axes causes the foot to rotate. If the torque is too large and exceeds the upper limit that the sole of the foot can provide, it will cause the foot to flip over.

[0074] In the standing state, the robot body rotation angular velocity is assumed to be zero, and the system angular momentum equation can be approximately calculated as:

[0075]

[0076] The single rigid body model of the actual robot system is:

[0077]

[0078] Install an IMU (inertial measurement unit) at the origin of the robot's floating base coordinate system. By default, this is the center of mass of the single rigid body model. Obtain the body angle in the robot's coordinate system Where φ, θ, ψ represent the robot's roll, pitch, and yaw angles, respectively. The body angular velocity in the robot coordinate system is The conversion relationship of the angular velocity ω of the center of mass of a single rigid body to the world coordinate system is as follows:

[0079]

[0080] Where R = R z (ψ)R y (θ)R x (φ) represents the rotation matrix from the body coordinate system to the world coordinate system, R z , R y , R x are the rotation matrices around the z, y, and x axes respectively.

[0081] Based on the single rigid body model, the state space equation of the robot when standing is:

[0082]

[0083]

[0084] make Then the system state space equation can be simplified as:

[0085]

[0086] Where A and B are the system state matrix and input matrix respectively, and the expression is:

[0087]

[0088] With Δ t For the sampling interval, the simplified system state space equation is discretized to obtain the discretized system state space equation:

[0089] x(i+1)=A i x(i)+B i u i

[0090] Among them A i ,B i ,u i (i=0,…,k-1) represent the state coefficient matrix, input coefficient matrix and control input of the system at the i-th sampling time, respectively. The specific expression is:

[0091]

[0092] According to the discretized system state space equation, the system state equation for the next k steps is obtained recursively:

[0093]

[0094] Construct a center of mass dynamics prediction model based on the future k-step system state equation:

[0095]

[0096] Where X=[x1 x2 … x k ] T is the expansion of the robot state in the next k steps, where A, B, and U are specifically expressed as:

[0097]

[0098] Within the prediction time window, the next k steps of robot state evolution will follow the center of mass dynamics prediction model.

[0099] The reference state of the robot at the future moment is Since the embodiment of the present invention is directed to the standing state of the robot, the reference state may be designed as:

[0100]

[0101] Among them, h represents the desired standing height of the robot, which should actually be the height from the ground to the origin of the robot's floating base coordinate system in the world coordinate system. If the goal is to make the robot squat up and down, h can be designed as a function of time t h(t).

[0102] The system optimization goal is constructed based on the center of mass reference state information:

[0103]

[0104] Among them, Q i , R i They are all diagonal matrices of parameters to be adjusted, which control the tracking error and the predictive power respectively. The integrated optimization objective is in matrix form:

[0105]

[0106] Where R is Q i , R i The block diagonal matrix of is expressed as:

[0107]

[0108] Since the system must first satisfy the prediction model, according to the center of mass dynamics prediction model, the optimization goal can be further expanded as follows:

[0109]

[0110] At each prediction moment, all variables except variable U are known, and the actual optimization objective is equivalent to:

[0111]

[0112] To prevent the robot from sliding while standing, the following constraints are constructed:

[0113] |f x |≤μf z

[0114] |f y |≤μf z

[0115]

[0116] Where μ is the static friction coefficient, which is integrated into a matrix form. The above constraint expression is:

[0117]

[0118] Among them, I k,k is the identity matrix, Represents the left and right foot sliding constraint coefficient matrix, which has the same value. The specific expression is:

[0119]

[0120] In addition to the sliding constraint, in order to prevent the soles of the feet from flipping when standing on uneven ground or when non-flat feet touch the ground, the corresponding sole moment constraint is constructed:

[0121]

[0122] in, They are the left and right foot sole flip constraint coefficient matrices. Since the left and right feet are symmetrical when standing, the coefficient matrices have the same value. The specific expression is:

[0123]

[0124] Among them, a, b, and c are the distance from the projection point of the ankle on the ground to the left side of the foot (the same distance from the right side of the foot), the distance from the projection point to the front end of the foot, and the distance from the projection point to the rear end of the foot.

[0125] To ensure the safety of the system, the upper and lower limits of the prediction power are added as follows:

[0126]

[0127] Among them, U min , U max The upper and lower limits of the predicted force and moment are set.

[0128] In summary, solve the following quadratic programming problem to obtain the optimal control sequence:

[0129]

[0130] For the above QP problem, we can use the QPOASES open source solver to obtain the optimal prediction sequence U = [u0 u1 … u k-1 ] T .

[0131] According to the actual connecting rod parameters of the robot, the Jacobian function in Matlab is used to calculate the Jacobian matrix J of the system force based on the current robot joint angle and angular velocity information. The predicted first step support force and torque are mapped to each joint through the Jacobian matrix. The specific calculation formula is:

[0132]

[0133] Then each torque value is directly sent to each joint of the robot to complete this round of control.

[0134] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0135] (1) In the present invention, the MPC method is used to directly calculate the ground reaction force required by the bipedal robot in the standing state, so that the control system can effectively distribute the robot's bipedal support force and torque, reduce the direct load demand on the ankle joint, and improve the accuracy and stability of the robot's balance control;

[0136] (2) In the present invention, the control modes of the robot in motion and static states are designed in a unified manner, and there is no need to frequently switch between force control and position control, thereby simplifying the control process, reducing system complexity and instability caused by state switching, and improving the overall control efficiency and response speed;

[0137] (3) In the present invention, taking into account the special situation that the non-flat feet of the robot touch the ground when standing, the present invention introduces ankle joint torque constraints to prevent the sole of the foot from flipping, thereby effectively avoiding hardware damage, extending the service life of the robot's moving parts, and improving the safety and reliability of the system.

[0138] The above contents are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

[0139] There are a few points to note:

[0140] (1) The drawings of the embodiments of the present invention only relate to the structures related to the embodiments of the present invention, and other structures may refer to the general design.

[0141] (2) For the sake of clarity, in the drawings used to describe the embodiments of the present invention, the thickness of the layers or regions is exaggerated or reduced, that is, these drawings are not drawn according to the actual scale. It is understood that when an element such as a layer, film, region or substrate is referred to as being "on" or "under" another element, the element may be "directly" "on" or "under" the other element or there may be intermediate elements.

[0142] (3) In the absence of conflict, the embodiments of the present invention and the features therein may be combined with each other to obtain new embodiments.

[0143] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. The protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A standing balance control method for a bipedal robot based on MPC, characterized in that: include: S1. The robot obtains an external reference signal from the current moment to the future T time interval: Among them, the external reference signal Includes the angle at the robot's center of mass Position p, angular velocity ω and linear velocity Four parts, the time interval T includes the future k sampling intervals, and the sampling interval is Δt, that is, T = kΔt; S2. Calculate and obtain the current robot state x0 through the state estimation algorithm: in, represents the current center of mass angle, p0 represents the current position, ω0 represents the current angular velocity, Indicates the current line speed; S3. Construct a robot center of mass dynamics prediction model based on a single rigid body model, set the robot's ground sliding constraint and foot flip constraint conditions, and construct a quadratic programming problem (QP). The optimization goal is to minimize the error between the robot's center of mass state and the reference signal at the corresponding time in the future T time interval, while the bipedal support force and torque are small; S4, according to the external reference signal X ref and the current robot state x0, use the QP solver to solve the quadratic programming problem and obtain the optimal input sequence U, where U includes the predicted future k-step bipedal support force and torque i=0, ..., k-1; S5. Retain the predicted future first step support in the optimal sequence U and torque Discard the remaining k-1 step predictions; S6, calculate the force Jacobian matrix J of the robot at this time; S7, according to the force Jacobian matrix J, the support force and torque Mapped to each joint, the calculation formula is: And send the results to each motor.

2. The MPC-based biped robot standing balance control method according to claim 1, characterized in that: The center of mass dynamics prediction model in S3 specifically includes: S31. Construct rigid body dynamics model: in, R 3，3 represents the rotation matrix, I 3，3 represents the unit matrix, I represents the inertia matrix at the center of mass of the robot, g represents the acceleration due to gravity, r1 and r2 represent the vectors from the support points of the left and right legs of the robot to the center of mass, respectively, [r1] × represents the antisymmetric matrix composed of vector r1, m is the robot mass; S32. Construct the state space equation of the discretized system: x(i+1)=A i x(i)+B i u i Among them, the sampling interval is Δ t , A i , B i ,u i , (i=0, ..., k-1) represent the state coefficient matrix, input coefficient matrix and control input of the system at the i-th sampling time respectively; S33. Constructing a center of mass dynamics prediction model: in, x k represents the predicted robot state for the kth step in the future, A and B are the coefficient matrix and input matrix of the prediction model, and U represents the predicted input sequence for the next k steps.

3. The MPC-based biped robot standing balance control method according to claim 2, characterized in that: The constraints in S3 specifically include: S34. Construct foot contact sliding constraint and foot sole flipping constraint: Among them, I k，k is the identity matrix, and Respectively represent the left and right foot sliding constraint coefficient matrices, and Respectively represent the flipping constraint coefficient matrices of the left and right foot soles; S35. Construct optimization goals: Among them, Q i and R i They are all parameter diagonal matrices that adjust the tracking error and predictive power.

4. The MPC-based biped robot standing balance control method according to claim 3, characterized in that: The quadratic programming problem in S3 specifically includes: S36. Construct a quadratic optimization problem (QP): Where C is the constraint coefficient matrix, H = 2(B T QB+R), f=2B T Q(Ax0-X ref ), Q and R are Q i and R i is a block diagonal matrix.

5. The MPC-based biped robot standing balance control method according to claim 1, characterized in that: include: The QP solver uses the QPOASES solver.

6. The MPC-based biped robot standing balance control method according to claim 1, characterized in that: include: S8. After updating the robot status information, return to S1.

7. The MPC-based biped robot standing balance control method according to claim 2, characterized in that: The coefficient matrix specifically includes: The coefficient matrices are all calculated according to the S32 recursive formula.

8. The MPC-based biped robot standing balance control method according to claim 4, characterized in that: The S36 specifically includes: The quadratic optimization problem is to express the original problem as a typical QP problem through compact MPC.

Images