Cascade control method and device for bipedal robot based on task hierarchical optimization

Abstract

The present application relates to a biped robot cascade control method based on task hierarchical optimization, including: obtaining the desired speed and desired angular velocity of the biped robot, and calculating the desired base state according to the desired speed and desired angular velocity; obtaining the motion state of the biped robot, and estimating the base state and the foot-end contact force according to the motion state; constructing a single rigid body model according to the desired base state and the base state estimation, and calculating the control force screw of the biped robot's supporting leg; calculating the landing point of the biped robot's swinging leg according to the desired speed, and obtaining the trajectory planning; making the foot-end contact force track the control force screw according to the control force screw and the trajectory planning, and calculating the relaxation variable; calculating the joint control torque of the biped robot according to the current foot-end contact force and the relaxation variable; and controlling the biped robot's motion in real time according to the joint control torque. The method can realize high-speed motion cascade control of the biped robot.

Description

Technical Field

[0001] The present application relates to the technical field of biped robot cascade control, and in particular to a biped robot cascade control method and device based on task hierarchical optimization. Background Art

[0002] Bipedal robots have similar movement patterns to humans and can move freely in various complex environments where humans are active. At the same time, humans have always hoped to create robots similar to humans for use in entertainment education, health care, military, industrial manufacturing, operations in harsh and dangerous environments, and rescue. Bipedal robots have great development potential and have become a research hotspot in the field of robotics, representing the comprehensive development level of robotics technology.

[0003] In recent years, the focus of legged robot research has shifted to the model predictive control framework, which has been applied mainly to quadruped robots with good results. This framework can be used in legged robots using various models, such as inverted pendulum model, single rigid body model and overall model. The complexity of the model will directly affect the type of MPC method and the speed of solution. The method types of the MPC framework are mainly divided into linear MPC, nonlinear MPC, robust MPC and learning MPC. Since most of the current control objects are nonlinear time-varying systems, the use of robust MPC and learning MPC can improve the robustness and tracking accuracy of the system.

[0004] However, the core of the overall control framework is single-step PD control, which has weak disturbance resistance and high-speed performance. The computational efficiency of the optimization algorithm of model predictive control still limits the real-time application of these methods, and the use of linear MPC with simplified models will lead to the loss of dynamic characteristics. Summary of the invention

[0005] Based on this, it is necessary to provide a bipedal robot cascade control method based on task hierarchical optimization to address the above-mentioned technical problems, which can more accurately control the posture and height of the floating base, better coordinate the stability of the robot, improve the robot's anti-interference ability and movement speed, and realize high-speed motion cascade control of the bipedal robot.

[0006] The cascade control method of bipedal robot based on task hierarchical optimization includes:

[0007] Obtaining an expected speed and an expected angular velocity of the biped robot, and calculating an expected base state according to the expected speed and the expected angular velocity;

[0008] Acquire the motion state of the biped robot, and estimate the base state and the foot end contact force according to the motion state;

[0009] According to the desired base state and the base state estimation, a single rigid body model is constructed to calculate the control force rotation of the supporting leg of the biped robot;

[0010] According to the expected speed, the landing point of the swinging leg of the biped robot is calculated, and the trajectory planning is obtained;

[0011] According to the control force screw and the trajectory planning, the foot end contact force is made to track the control force screw and a relaxation variable is calculated; and the joint control torque of the biped robot is calculated according to the current foot end contact force and the relaxation variable;

[0012] The movement of the biped robot is controlled in real time according to the joint control torque.

[0013] In one embodiment, according to the control force screw and the trajectory planning, the foot end contact force is made to track the control force screw, and a relaxation variable is calculated; and the joint control torque of the biped robot is calculated according to the current foot end contact force and the relaxation variable, including:

[0014] According to the control force screw and the trajectory planning, different priority tasks are constructed; according to the priority tasks, the foot end contact force is made to track the control force screw, and a hierarchical optimization algorithm is constructed;

[0015] Solving the hierarchical optimization algorithm in sequence according to the order of priority tasks to obtain slack variables;

[0016] According to the current foot end contact force and the relaxation variable, the joint control torque of the biped robot is calculated using a dynamic decomposition model.

[0017] In one embodiment, different priority tasks are constructed according to the control force screw and the trajectory planning; according to the priority tasks, the foot end contact force is made to track the control force screw, and the hierarchical optimization algorithm is constructed, including:

[0018] Constructing different priority tasks according to the control force screw and the trajectory planning;

[0019] The first priority task is dynamic constraints:

[0020]

[0021]

[0022]

[0023] Where M is the inertia matrix of the robot's overall dynamics, represents the generalized vector of the robot, including the inertial position of the floating base and the joint angle, h represents the centrifugal force, Coriolis force term, and gravity term, is the selection matrix of the active joint, T is the transpose of the matrix, is the active drive joint torque, is the foot contact Jacobian matrix, is the foot contact force torque exerted by the ground on the robot, f c_mpc represents the foot end force rotation calculated by model predictive control, Δf c The foot contact force compensation to be calculated is the three dynamic matrices decomposed into M = [M b , M a ] T , h=[h b ,h a ] T , I J c =[ I J cb I J ca ],M b is the inertia matrix of the floating base, h b are the centrifugal force, Coriolis force term and gravity term of the floating base. I J cb is the Jacobian matrix of the foot contact of the floating base part, M a is the inertia matrix of the leg, h a are the centrifugal force, Coriolis force term, and gravity term of the leg, I J ca is the contact Jacobian matrix of the foot end of the leg;

[0024] The second priority task is the foot end contact non-slip constraint:

[0025]

[0026]

[0027] The third priority task is floating base attitude and altitude:

[0028]

[0029]

[0030] In the formula, x d_b , is the position, velocity, and acceleration of the desired trajectory of the floating base attitude and height, is the control quantity calculated by feedback control, x b and is the actual state, k p_b and k d_b is the controller diagonal gain matrix; is the Jacobian matrix J IF_task The derivative of

[0031] The fourth priority task is the end position of the swing leg:

[0032]

[0033]

[0034] In the formula, x d_f , and is the position, velocity, and acceleration of the desired trajectory of the end position of the swing leg, is the control quantity calculated by feedback control, x f and is the actual state, k p_f and k d_f is the controller diagonal gain matrix; is the Jacobian matrix J IF_task The derivative of

[0035] The fifth priority task is to minimize the joint torque:

[0036]

[0037]

[0038] Where M a is the inertia matrix of the leg, h a are the centrifugal force, Coriolis force term, and gravity term of the leg, I J ca is the contact Jacobian matrix of the foot end of the leg;

[0039] The sixth priority task is to minimize the foot end contact force compensation:

[0040]

[0041] Where I is the base inertia matrix;

[0042] According to the priority tasks, the foot end contact force is made to track the control force rotation quantity, and a hierarchical optimization algorithm is constructed:

[0043]

[0044] In the formula, A i refers to the left-hand matrix of the first to sixth priorities, refers to the optimization variable matrix, b i Refers to the right-hand side matrix of the first to sixth priority levels, i=1,2,3…6.

[0045] In one embodiment, when solving the hierarchical optimization algorithm in sequence according to the order of priority tasks, equality constraints and inequality constraints also need to be satisfied;

[0046] Equality constraints:

[0047]

[0048] The physical meaning of the above formula is that the optimization solution of the i-th priority task corresponds to To compare with the previous (i-1) are equal, where A j Refers to the left-hand matrix of the first to sixth priority levels, which is the same as A during the calculation process. i Synchronous corresponding, Refers to the optimization variable matrix, which is related to Synchronous corresponding j = 1, 2, 3...6;

[0049] Inequality constraints:

[0050] τ min ≤τ≤τ max

[0051] U. I J c ≤0

[0052] In the formula, τ, τ min , τ max Refers to joint torque, minimum torque, maximum torque; U refers to the constraint matrix, J c Refers to the foot contact matrix.

[0053] In one embodiment, constructing a single rigid body model according to the desired base state and the base state estimation, and calculating the control force rotation of the supporting leg of the biped robot includes:

[0054] According to the desired base state and the base state estimation, a cost function for trajectory tracking is constructed, and under the conditions of satisfying the dynamic constraints, inequality constraints and equality constraints of a single rigid body model, the cost function is solved to obtain the control force spinor of the supporting leg of the biped robot.

[0055] In one embodiment, the conditions for satisfying the dynamic constraints of the single rigid body model include:

[0056] x(k+1)=A k ·x(k)+B k u(k)

[0057]

[0058]

[0059] x(k)=[ I q b,r , I q b,p , I ω b , I υ b , g] T

[0060]

[0061]

[0062]

[0063]

[0064] k g =[0 0 -1] T

[0065] k τ =[0 0 1] T

[0066] In the formula, x(k+1) represents the next state of the control system, x(k) represents the current state of the control system, that is, the state vector of the robot, u(k) represents the control quantity of the control system, and A k represents the system matrix of the control system, B k represents the control matrix of the control system; ΔT is the discrete time step, generally the control time step; is the inertia matrix of the base in the inertial system; I p Bc (q) is the position of the foot of the supporting leg relative to the origin of the base system in the inertial system, and the direction is from the origin of the base system to the foot. The subscripts L and R represent the left and right legs, respectively; Indicates the amount of rotation at the foot end of the left leg. It indicates the right leg foot end force rotation; I q b,r Base attitude angle (roll, pitch, yaw), I q b,p The position of the base in the inertial system; I ω b and I υ b are the angular velocity and linear velocity in the inertial system respectively; g is the acceleration due to gravity; q b,yaw is the yaw angle of the base, q b,p it c h Pedestal pitch angle.

[0067] In one embodiment, the conditions for satisfying the inequality constraint include:

[0068]

[0069] f z ≤f z,max

[0070] -μf z ≤f x ≤μf z

[0071] -μf z ≤f y ≤μf z

[0072]

[0073]

[0074] In the formula, Indicates the foot end force rotation, The normal vector representing the contact position between the sole of the foot and the ground, f z refers to the foot end force in the vertical direction, μ refers to the friction coefficient, f x Refers to the foot end force in the horizontal direction, f y Refers to the foot end force in the lateral direction, τ yaw Refers to the moment in the yaw direction, L toe Indicates the distance from the ankle joint projection point on the ground to the front contact point, L heel It represents the distance from the projection point of the ankle joint on the ground to the rear contact point.

[0075] In one embodiment, the conditions for satisfying the equality constraint include:

[0076]

[0077]

[0078] Where k refers to the kth moment and i refers to the ith step.

[0079] In one embodiment, according to the expected speed, calculating the landing point of the swinging leg of the biped robot and obtaining the trajectory planning includes:

[0080] According to the expected speed, the landing point of the bipedal robot's swinging leg is calculated:

[0081]

[0082] I v b =[ I v bxI v by 0] T

[0083] I v b,des =[ I v bx,des I v by,des 0] T

[0084] In the formula, p f,des is the foothold, the first term p com is the real-time position of the floating base in the inertial system, and the second term R IB B p B_hip is the position from the base system to the hip in the inertial system. The third and fourth items are the heuristic foot point calculation formulas proposed by Raibert. I v b It represents the actual speed of the robot in the inertial system. I v b,des It represents the expected speed of the robot in the inertial system. The fifth item is the foot compensation item during yaw steering. I q b,pz Indicates the height of the robot, I ω bz,des represents the expected yaw rate;

[0085] Based on the foothold, calculate the trajectory plan:

[0086] x d_f =(1-m) 3 p0+3m(1-m) 2 p1+3m 2 (1-m)p2+m 3 p d

[0087] p d =p f,des

[0088] Where m∈

[01] represents the normalized time, p0 represents the foot lift point, p1 represents the intermediate control point, p2 represents the intermediate control point, p1≠p2, p d Indicates the landing point.

[0089] A biped robot cascade control device based on task hierarchical optimization, comprising:

[0090] An acquisition module, used to acquire an expected speed and an expected angular velocity of the biped robot, and calculate an expected base state according to the expected speed and the expected angular velocity;

[0091] An estimation module, used for acquiring the motion state of the biped robot, and performing base state estimation and foot end contact force estimation according to the motion state;

[0092] A model prediction module, used to construct a single rigid body model according to the expected base state and the base state estimation, and calculate the control force rotation of the supporting leg of the biped robot;

[0093] A planning module, used to calculate the landing point of the swinging leg of the biped robot according to the expected speed and obtain trajectory planning;

[0094] A hierarchical optimization module is used to make the foot-end contact force track the control force screw quantity and calculate the relaxation variable according to the control force screw quantity and the trajectory planning; and calculate the joint control torque of the biped robot according to the current foot-end control force and the relaxation variable;

[0095] The control module is used to control the movement of the biped robot in real time according to the joint control torque.

[0096] The above-mentioned biped robot cascade control method based on task hierarchical optimization proposes a high-speed motion trajectory tracking method that combines model prediction considering leg dynamics with task hierarchical optimization overall control. Based on the time-sharing control framework of the underactuated robot, a single rigid body model MPC based on ignoring the legs is designed at the upper level to calculate the posture and height force rotation in the support period, and the foot point principle is used to control the robot's forward and lateral speed; the task hierarchical optimization overall controller considering leg dynamics is used at the lower level to track the force rotation; the foot end contact force relaxation variable is designed in the optimization variable, and the variable is used to compensate for the difference between the two foot end contact forces in the single rigid body model and the complete dynamic model, thereby building a link between the upper model prediction controller and the lower overall controller. The heavy weight of the hydraulically driven legs and the external impact environment are more challenging for the high-speed movement of the biped robot that lacks driving freedom. The robustness of this method under high-speed movement and disturbance is verified through the high-speed forward experiment of walking gait and the external impact stability simulation experiment, and the maximum walking speed of the robot is increased from 1.9m / s of the model prediction method to 2.5m / s. BRIEF DESCRIPTION OF THE DRAWINGS

[0097] Figure 1 A flow chart of a biped robot cascade control method based on task hierarchical optimization in one embodiment;

[0098] Figure 2 is a schematic diagram of a cascade control framework in one embodiment;

[0099] Figure 3Schematic diagram of the high-speed dynamic motion process of the robot in one embodiment; a is 0.2s, 0m / s, b is 3.3s, 0.3m / s, c is 5s, 0.5m / s, d is 8.5s, 1.2m / s, e is 11.4s, 2m / s, f is 12.8s, 2.2m / s, g is 14.5s, 2.4m / s, h is 19s, 2.5m / s;

[0100] Figure 4 Schematic diagram of the foot end contact force and moment in the z-axis direction in one embodiment; a is the left leg Z-direction contact force, b is the left leg yaw moment, c is the right leg Z-direction contact force, and d is the right leg yaw moment;

[0101] Figure 5 Schematic diagram of the Euler angle of a floating base in one embodiment; a is the roll angle, b is the pitch angle, and c is the yaw angle;

[0102] Figure 6 Schematic diagram of the position of the floating base in one embodiment; a is the X-axis, b is the Y-axis, and c is the Z-axis height;

[0103] Figure 7 Schematic diagram of the speed in the x-axis and y-axis directions in one embodiment; a is the X-axis, and b is the Y-axis;

[0104] Figure 8 Schematic diagram of the Euler angle of the floating base in another embodiment; a is the roll angle, b is the pitch angle, and c is the yaw angle;

[0105] Fig. 9 Schematic diagram of speed and height trajectory in one embodiment; a is the speed in the X-axis direction, b is the speed in the Y-axis direction, and c is the height in the Z-axis direction;

[0106] Fig.10 It is a structural block diagram of a biped robot cascade control device based on task hierarchical optimization in one embodiment. DETAILED DESCRIPTION

[0107] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0108] like Figure 1 As shown, the present application provides a biped robot cascade control method based on task hierarchical optimization, in one embodiment, comprising the following steps:

[0109] Step 102: Obtain the expected speed and expected angular velocity of the biped robot, and calculate the expected base state according to the expected speed and the expected angular velocity.

[0110] The desired speed includes the desired forward speed and lateral speed.

[0111] Calculating the desired base state based on the desired velocity and the desired angular velocity belongs to the prior art.

[0112] Step 104, obtaining the motion state of the biped robot, and performing base state estimation and foot end contact force estimation according to the motion state.

[0113] The specific estimation process belongs to the prior art and will not be described in detail here.

[0114] Step 106: construct a single rigid body model based on the expected base state and the base state estimation, and calculate the control force rotation of the supporting leg of the biped robot. Specifically:

[0115] According to the desired base state and the base state estimation, a cost function for trajectory tracking is constructed, and under the conditions of satisfying the dynamic constraints, inequality constraints and equality constraints of a single rigid body model, the cost function is solved to obtain the control force spinor of the supporting leg of the biped robot.

[0116] More specific:

[0117] The conditions that satisfy the dynamic constraints of the single rigid body model include the discrete state equation of the floating base single rigid body model:

[0118] x(k+1)=A k ·x(k)+B k ·u(k) (1)

[0119]

[0120]

[0121] x(k)=[ I q b,r , I q b,p , I ω b , I υ b , g] T

[0122]

[0123]

[0124]

[0125]

[0126] k g=[0 0 -1] T

[0127] k τ =[0 0 1] T

[0128] In the formula, x(k+1) represents the next state of the control system, x(k) represents the current state of the control system, that is, the state vector of the robot, u(k) represents the control quantity of the control system, and A k represents the system matrix of the control system, B k represents the control matrix of the control system; ΔT is the discrete time step, generally the control time step; is the inertia matrix of the base in the inertial system; I p Bc (q) is the position of the foot of the supporting leg relative to the origin of the base system in the inertial system, and the direction is from the origin of the base system to the foot. The subscripts L and R represent the left and right legs, respectively; Indicates the amount of rotation at the foot end of the left leg. It indicates the right leg foot end force rotation; I q b,r Base attitude angle (roll, pitch, yaw), I q b,p The position of the base in the inertial system; I ω b and I υ b are the angular velocity and linear velocity in the inertial system respectively; g is the acceleration due to gravity; q b,yaw is the yaw angle of the base, q b,pitch Pedestal pitch angle.

[0129] In order to track the desired trajectory, a cost function is designed to reflect the tracking performance of the controlled system. The cost function contains two items. The first item is that the weighted state trajectory tracking error is as small as possible, and the second item is that the weighted control sequence is as small as possible. The optimization variable is the foot end contact force screw, u = f c_mpc ; Then, dynamic constraints and physical constraints are added to construct the trajectory tracking optimization problem as follows:

[0130]

[0131] Satisfy the dynamic constraints:

[0132] stx(k+i|t)=A k,t ·x(k+i-1|t)+B k,t u(k+i-1|t),i=1...N p (2b)

[0133] Satisfy the inequality constraints:

[0134]

[0135] Satisfy the equality constraints:

[0136] D i u(k+i-1)=0, i=1...N p (2d)

[0137] In the formula, x(k+i|t) is the actual state trajectory, x ref (k+i|t) is the expected trajectory of the state.

[0138] N p is the number of prediction steps, N c is the number of control steps, i is the i-th step, k is the k-th moment, Q and R are weight matrices respectively.

[0139] Among them, the conditions that satisfy the inequality constraints mainly include the friction cone constraints, including:

[0140] 1) During the contact between the foot and the ground, the ground only exerts a ground reaction force on the foot, but cannot form a pulling force on the foot. Therefore, the vertical force on the foot is f z Must be greater than or equal to zero. Considering the undulation of the ground, the component of the foot contact force in the direction of the ground normal is greater than or equal to zero. In addition, due to the maximum physical driving capacity of the joint driver, the foot force f z There is a maximum value.

[0141]

[0142] f z ≤f z,max

[0143] 2) To prevent the foot from slipping on the ground and causing an uncontrollable state, the forward and lateral contact forces are limited to the friction cone.

[0144] -μf z ≤f x ≤μf z

[0145] -μf z ≤f y ≤μf z

[0146] 3) In addition, the yaw moment of the foot contact will form a lateral force at the front and rear contact points. This lateral force will be superimposed on the lateral contact force, so it also needs to be limited within the friction cone.

[0147]

[0148]

[0149] In the formula, Indicates the foot end force rotation, The normal vector representing the contact position between the sole of the foot and the ground, f z refers to the foot end force in the vertical direction, μ refers to the friction coefficient, f x Refers to the foot end force in the horizontal direction, f y Refers to the foot end force in the lateral direction, τ yaw Refers to the moment in the yaw direction, L toe Indicates the distance from the ankle joint projection point on the ground to the front contact point, L heel It represents the distance from the projection point of the ankle joint on the ground to the rear contact point.

[0150] The conditions that satisfy the equality constraints are mainly gait constraints. The gait sequence is integrated into the optimization solution by selecting the foot end contact force screw, including:

[0151] 1) If the leg is in the swing phase, use the equality constraint to force the foot end contact force to be zero; if the leg is in the support phase, the foot end contact force is not constrained here, and its value is calculated by other constraints. After unifying the left and right legs into one frame, an 8×8 dimensional matrix will be obtained at a certain moment. When the left leg is in support and the right leg is swinging:

[0152]

[0153] 2) When the right leg is supporting and the left leg is swinging:

[0154]

[0155] Where k refers to the kth moment and i refers to the ith step.

[0156] Based on the dynamic model of formula (1), the model predictive control optimization problem (2a) is transformed into the standard form of linear quadratic programming problem (QP). The foot end contact force rotation f can be obtained by optimizing the solution within the control cycle. c_mpc .

[0157] The floating base single rigid body model is derived from the overall dynamics by ignoring the dynamic part of the leg, so the upper model predicts the foot contact force f planned by the controller c_mpc The desired foot contact force f required by the task-layered optimization controller is c_wbc are not equal. In general, f c_wbc is greater than f c_mpc , so this paper designs a foot end contact force relaxation variable Δf c , using this variable to compensate for the difference in contact force between the two foot ends, we get the relationship f c_wbc =f c_mpc +Δfc , thus building a link between the upper-level model predictive controller and the lower-level overall controller. Since the upper-level controller has provided a stable expected median foot contact force, the relaxation variable Δf c Under the overall dynamics constraint, c_mpc Close, the overall performance is f c_wbc In tracking f c_mpc , thus forming a cascade controller of the model prediction task hierarchical optimization controller.

[0158] For the biped robot of this application, each leg has only four active joints. In this unified framework, we only need to set the gain values ​​corresponding to the x and y degrees of freedom in the weight matrix Q to 0 to abandon the control of these two degrees of freedom during the support period. If this is not done, using four active degrees of freedom to control six degrees of freedom will result in poor control effects due to under-actuation.

[0159] Step 108, according to the expected speed, calculate the landing point of the swinging leg of the biped robot and obtain the trajectory planning. Specifically:

[0160] According to the expected speed, the landing point of the swinging leg of the biped robot in the inertial coordinate system is calculated:

[0161]

[0162] I v b =[ I v bx I v by 0] T

[0163] I v b,des =[ I v bx,des I v by,des 0] T

[0164] In the formula, p f,des is the foothold, the first term p com is the real-time position of the floating base in the inertial system, and the second term R IB B p B_hip is the position from the base system to the hip in the inertial system. The third and fourth items are the heuristic foot point calculation formulas proposed by Raibert. I v b It represents the actual speed of the robot in the inertial system. I v b,desIt represents the expected speed of the robot in the inertial system. The fifth item is the foot compensation item during yaw steering. I q b,pz Indicates the height of the robot, I ω bz,des represents the expected yaw rate;

[0165] The x and y positions of the foot landing point in the inertial system can be calculated through the formula, while the z position should be based on the robot's standing plane and probed 10mm down to ensure that the robot's foot touches the ground slightly in advance.

[0166] Considering the requirement of trajectory acceleration smoothness, the third-order Bezier curve is used for the trajectory of the swing leg foot end, and its form is shown in the following formula. Within a fixed time period, its curve trajectory is determined by four control points, namely, the foot lift point p 0 , intermediate control point p 1 , intermediate control point p 2 and the landing point p d , where the four control points are required to be within the reachable space of the swinging leg.

[0167] According to the footfall point, in order to complete the leg lift height and smooth transition process, the swing leg trajectory planning is calculated in real time:

[0168] x d_f =(1-m) 3 p0+3m(1-m) 2 p1+3m 2 (1-m)p2+m 3 p d

[0169] p d =p f,des

[0170] Where m∈

[01] represents the normalized time, p0 represents the foot lift point, p1 represents the intermediate control point, p2 represents the intermediate control point, p1≠p2, p d Indicates the landing point.

[0171] For the planned swing leg position trajectory x d _f After differentiation, we get the expected speed and acceleration

[0172] Step 110, according to the control force screw and the trajectory planning, the foot end contact force is made to track the control force screw and the relaxation variable is calculated; the joint control torque of the biped robot is calculated according to the current foot end contact force and the relaxation variable.

[0173] It should be noted that the current foot contact force refers to the foot contact force after the tracking control force is rotated.

[0174] Specific:

[0175] According to the control force screw and the trajectory planning, different priority tasks are constructed; according to the priority tasks, the foot end contact force is made to track the control force screw, and a hierarchical optimization algorithm is constructed;

[0176] Solving the hierarchical optimization algorithm in sequence according to the order of priority tasks to obtain slack variables;

[0177] According to the current foot end contact force and the relaxation variable, the joint control torque of the biped robot is calculated using a dynamic decomposition model.

[0178] More specific:

[0179] The basic idea of ​​the task-layered optimization controller is to prioritize the full-body dynamics model of the robot, sort all tasks by priority, and then optimize and solve them in order. The solutions of low-priority tasks must satisfy high-priority tasks. In the optimization solution of each task, the controller of the corresponding space (task space / joint space) is designed at the outer loop level, and the trajectory tracking optimization controller under equality and inequality constraints is designed at the inner loop level. Generalized acceleration and contact force compensation are used as optimization variables. The controller is described in detail as follows.

[0180] Constructing different priority tasks according to the control force screw and the trajectory planning;

[0181] 1) The first priority task: dynamic constraints. The overall dynamic model of the robot is:

[0182]

[0183] In order to correspond to the optimization variables, the undriven part model of the overall dynamics is taken,

[0184]

[0185] Constructed as a function containing optimization variables,

[0186]

[0187] Where M is the inertia matrix of the robot's overall dynamics, represents the generalized vector of the robot, including the inertial position of the floating base and the joint angle, h represents the centrifugal force, Coriolis force term, and gravity term, is the selection matrix of the active joint, T is the transpose of the matrix, is the active drive joint torque, is the foot contact Jacobian matrix, is the foot contact force torque exerted by the ground on the robot, f c_mpc represents the foot end force rotation calculated by model predictive control, Δf c The foot contact force compensation to be calculated is the three dynamic matrices decomposed into M = [M b , M a ] T , h=[h b ,h a ] T , I J c =[ I J cb I J ca ],M b is the inertia matrix of the floating base, h b are the centrifugal force, Coriolis force term and gravity term of the floating base. I J cb is the Jacobian matrix of the foot contact of the floating base part, M a is the inertia matrix of the leg, h a are the centrifugal force, Coriolis force term, and gravity term of the leg, I J ca is the contact Jacobian matrix of the foot end of the leg; the subscripts A1 and b1 will be used in the design of the cost function in the optimization algorithm.

[0188] 2) Second priority task: foot end contact non-slip constraint,

[0189]

[0190]

[0191] 3) The third priority task: floating base attitude and height. The expected values ​​of the floating base attitude and height are consistent with the expected values ​​in the upper model predictive controller, so the outer loop controller is,

[0192]

[0193] Constructed as a function containing optimization variables,

[0194]

[0195] The priority of attitude and altitude tasks is that attitude priority is greater than altitude;

[0196] In the formula, x d_b , is the position, velocity, and acceleration of the desired trajectory of the floating base attitude and height, is the control quantity calculated by feedback control, xb and is the actual state, k p_b and k d_b is the controller diagonal gain matrix; is the Jacobian matrix J IF_task The derivative of .

[0197] 4) Fourth priority task: swing leg end pose, outer loop controller,

[0198]

[0199] Constructed as a function containing optimization variables,

[0200]

[0201] In the formula, x d_f , and is the position, velocity, and acceleration of the desired trajectory of the end position of the swing leg, is the control quantity calculated by feedback control, x f and is the actual state, k p_f and k d_f is the controller diagonal gain matrix; is the Jacobian matrix J IF_task The derivative of .

[0202] 5) Fifth priority task: Minimize joint torque, based on the active drive part of the overall dynamics,

[0203]

[0204] It is expected that the joint torque τ approaches 0, and a function containing optimization variables is constructed.

[0205]

[0206] Where M a is the inertia matrix of the leg, h a are the centrifugal force, Coriolis force term, and gravity term of the leg, I J ca is the contact Jacobian matrix of the foot end of the leg.

[0207] 6) The sixth priority task: Minimum foot contact force compensation, expected foot contact force compensation Δf c Approaching 0, constructing a function containing optimized variables,

[0208]

[0209] Where I is the base inertia matrix.

[0210] After determining the priority tasks, the foot end contact force is made to track the control force screw according to the priority tasks, and a hierarchical optimization algorithm is constructed:

[0211]

[0212] In the formula, A i refers to the left-hand matrix of the first to sixth priorities, refers to the optimization variable matrix, b i Refers to the right-hand side matrix of the first to sixth priority levels, i=1,2,3…6.

[0213] The optimization algorithm performs optimization and solution in order from the first to the sixth tasks in order of task priority. When the hierarchical optimization algorithm performs optimization and solution in order of priority tasks, equality constraints and inequality constraints need to be satisfied.

[0214] The physical meaning of the equality constraint is that the optimal solution of the i-th priority task corresponds to To compare with the previous (i-1) The task is to ensure that the solution of the i-th priority task does not affect the control objectives corresponding to the solutions of the previous (i-1) high-priority tasks, thus achieving strict priority stratification:

[0215]

[0216] The physical meaning of the above formula is that the optimization solution of the i-th priority task corresponds to To compare with the previous (i-1) are equal, where A j Refers to the left-hand matrix of the first to sixth priority levels, which is the same as A during the calculation process. i Synchronous corresponding, Refers to the optimization variable matrix, which is related to Synchronous corresponding j = 1, 2, 3...6;

[0217] Inequality constraints include joint torque constraints and friction cone constraints:

[0218] τ min ≤τ≤τ max

[0219] U. I J c ≤0

[0220] In the formula, τ, τ min , τ max Refers to joint torque, minimum torque, maximum torque; U refers to the constraint matrix, J c Refers to the foot contact matrix.

[0221] After the optimization problem is solved and the optimization solution is obtained, the active drive joint torque, that is, the joint control torque, can be calculated using the dynamic decomposition model:

[0222]

[0223] Step 112: Control the movement of the biped robot in real time according to the joint control torque.

[0224] It should be noted that in this application, variables are marked with and They are uniformly expressed as the first-order derivative and second-order derivative of the variables; the x, y, and z axes point to the robot's forward direction, left direction, and vertical upward direction, respectively.

[0225] In this embodiment, if Figure 2 As shown, the operator gives the robot forward and lateral speed v through the remote control handle b,x ,v b,y and yaw angular velocity ω b,z , the desired state x of the floating base is obtained by velocity integration and polynomial trajectory planning d The finite state machine determines the contact event based on the estimated contact force value, obtains the controller mode (control_mode) and the leg working mode (leg_mode), and introduces them into the model predictive controller, the swing leg trajectory planner, and the task hierarchical optimization controller. The desired state of the floating base x d and the actual state estimate The contact force of the supporting leg is obtained through MPC calculation. In addition, the foothold and trajectory planning of the swing leg in the x, y and z directions are calculated synchronously and output to the task hierarchical optimization controller in real time. After the joint control torque is generated, the robot is driven to move. The floating base state estimation is obtained by fusing the IMU acceleration information with the supporting leg kinematic information. Since it is not the focus of this article, it will not be repeated here.

[0226] The core of the cascade control framework is the model prediction task hierarchical optimization overall controller, which is mainly divided into two parts: the first part is the upper-layer model prediction controller based on the floating base single rigid body model, and the second part is the lower-layer task hierarchical optimization controller based on the overall dynamics model.

[0227] (1) In the upper layer, this paper adopts the underactuated time-sharing control principle to divide a gait cycle into two parts: the support period and the swing period. For the bipedal robot under study, when one leg is in the support period, the MPC based on the single rigid body model is used to control the robot's posture and height. When the other leg is in the swing period, the robot's forward and lateral speed are controlled by the foot landing point. Then the calculated control force rotation and the expected value of the foot end trajectory of the swing leg are sent to the lower-level controller.

[0228] (2) In the lower layer, the tasks include full-body dynamic constraints, foot-end non-slip constraints, posture control tasks, and torque minimization tasks. Based on the task hierarchical optimization controller, the robot's active drive joint torque is optimally calculated. This ensures that the target quantity of each part is controllable in the time-sharing control, and then the left and right legs alternately reciprocate, thereby realizing the three-dimensional omnidirectional motion and anti-disturbance control of the high-speed dynamic motion of the large-inertia leg biped robot.

[0229] The above-mentioned biped robot cascade control method based on task hierarchical optimization proposes a high-speed motion trajectory tracking method that combines model prediction considering leg dynamics with task hierarchical optimization overall control. Based on the time-sharing control framework of the underactuated robot, a single rigid body model MPC based on ignoring the legs is designed at the upper level to calculate the posture and height force rotation in the support period, and the foot point principle is used to control the robot's forward and lateral speed; the task hierarchical optimization overall controller considering leg dynamics is used at the lower level to track the force rotation; the foot end contact force relaxation variable is designed in the optimization variable, and the variable is used to compensate for the difference between the two foot end contact forces in the single rigid body model and the complete dynamic model, thereby building a link between the upper model prediction controller and the lower overall controller. The heavy weight of the hydraulically driven legs and the external impact environment are more challenging for the high-speed movement of the biped robot that lacks driving freedom. The robustness of this method under high-speed movement and disturbance is verified through the high-speed forward experiment of walking gait and the external impact stability simulation experiment, and the maximum walking speed of the robot is increased from 1.9m / s of the model prediction method to 2.5m / s.

[0230] In order to verify the high dynamic anti-disturbance performance of the control framework algorithm proposed in this application, a biped robot multi-body dynamics simulation platform based on the multi-body dynamics virtual prototype analysis software RecurDyn and the mathematical calculation software MATLAB was built. The mechanical structure and component weight of the simulated robot were directly generated and imported from the three-dimensional model of the physical robot.

[0231] The control algorithm parameters are set as follows: the control frequency of the model predictive controller is 100 Hz, and the weight is set to Q = diag ([3e 6 ,3e 6 ,1e 7 ,0,0,1e 9 ,1e 3 ,1e 3 ,1e 4 ,0,0,1e 6 ,0]), R = diag([0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1]), prediction step number N p =3, control step number N c=1, setting a smaller number of steps is mainly to reduce the matrix dimension and improve the calculation speed of single optimization. In the finite state machine based on event and time hybrid, the maximum swing period is set to 0.25s; the step height in the swing leg planning is 0.15m. The maximum foot end force in the z-axis direction in the friction cone constraint is 600N, and the friction coefficient is 0.6. The robot model parameters in the model predictive control scheme are shown in Table 1.

[0232] Table 1 Robot model parameters

[0233]

[0234]

[0235] The task sequence optimization control frequency is 500Hz, and the attitude PD gain of the floating base task is set to K p_pos =20·diag([3,3,1]), K d_pos =2·diag([3,3,1]), the corresponding order is roll angle, pitch angle and yaw angle. The PD gain of height is K p_h =300, K d_body = 30. The PD gain of the swing leg task is K p_swing =5·diag([1,1,1,7]), K d_swing =0.5·diag([1,1,1,7]). Since the goal of the algorithm design is to improve the robot's movement speed and anti-interference ability, two sets of simulation experiments were conducted, namely (A) high-speed forward walking gait experiment and (B) external impact stability experiment.

[0236] Simulation experiment (A): walking gait with high-speed forward motion

[0237] A polynomial fitting method is used to formulate a smooth transition forward speed trajectory. The transition time from zero to maximum speed is 15s. The maximum expected speed is set to 2.5m / s. The expected forward position is obtained by integrating the speed. The floating base height is set to 0.75m, and other expected states are set to 0. A high-speed motion simulation experiment is carried out on flat ground, and the high-dynamic motion process of the robot is obtained. See Figure 2. Figure 3 , the foot end contact force and moment curves in the z-axis direction are shown in Figure 4 , the robot floating base Euler angle and the position tracking curve in the inertial system are shown in Figure 5 Figure 6 And the speed tracking curves in the x and y axis directions are shown in Figure 7 .

[0238] from Figure 4It can be seen from the foot contact force in the z-axis direction that from 10s to 15s, the robot speed increased from 1.8m / s to 2.5m / s, and the foot contact force in the support phase became larger and larger, indicating that the controller provides a force corresponding to the increasingly faster forward speed to control the robot's posture and height.

[0239] exist Figure 5 In the Euler angle trajectory curve of the floating base, due to the forward motion, the roll angle can be evenly controlled within the range of ±0.05rad. The pitch angle and yaw angle show a uniform increase in the acceleration stage from 0-2.5m / s. After the speed stabilizes at 2.5m / s, the two attitude angles also fluctuate periodically and stably within a limited range between -0.08~0.05rad and -0.11~0.1rad respectively as the gait switches.

[0240] exist Figure 6 and Figure 7 In the figure, compared with the maximum speed of 1.4m / s for VMC and 1.9m / s for MPC, the position and forward speed trajectory of the robot in the series control framework tracked the expected trajectory well, and the maximum forward speed reached 2.5m / s, which was 0.6m / s higher than the maximum forward speed of 1.9m / s for the model predictive controller used alone, with an improvement of 31.5%. This shows that the stable control of attitude and height by task-layered optimization control and the use of the overall dynamics model are of great help to the improvement of the dynamic motion speed of the robot. In terms of lateral position and speed control, due to the high-speed movement of the robot, the lateral speed control has a certain offset, and the lateral displacement slowly offsets by 0.25m from the 10th second to the 20th second. This offset is acceptable in such high dynamic motion. The height of the robot has been controlled at the expected value of 0.75m, indicating that the overall control based on the overall model has a high dynamic compensation accuracy, and the 0.73m height in the model predictive control method is increased by 0.02m.

[0241] Simulation experiment (B): External shock stability

[0242] In order to verify the stability of the controller in dealing with external impacts, this experiment applies impact forces to the floating base for a certain period of time from the backward, sideways and vertically downward directions during the robot's forward movement. After multiple tests, the maximum impulses that satisfy the robot's dynamic balance are: 60N impact force is applied along the positive direction of the x-axis in the 3.5-4s interval, and the impulse is 30N.s; 115N impact force is applied along the positive direction of the y-axis in the 6.5-7s interval, and the impulse is 57.5Ns; 150N impact force is applied along the negative direction of the z-axis in the 8.5-9s interval, and the impulse is 75N.s. Figure 8 Euler angle tracking curve for the floating base, Fig. 9The velocity tracking curves and height curves in the x and y axis directions are shown.

[0243] In terms of attitude control: when subjected to a backward impact in the 3.5-4s interval, as the impulse accumulates, the first thing that is affected is the pitch angle, which gradually increases in this interval until it reaches a maximum peak of -0.096rad at the 4th second. Due to the rotational torque formed by the impulse on the supporting leg, the yaw angle also deflects greatly, with the positive and negative peaks reaching 0.173rad and -0.16rad respectively, and the maximum peak of the roll angle is -0.145 (rad). In the lateral impact in the 6.5-7s interval, the roll angle is the most affected, reaching a maximum peak of 0.17rad at the 7th second. Subsequently, through the foot-end contact force spinor generated by the model predictive control and the priority and force tracking control of the sequence optimized WBC, after 1 second of supporting leg control, the above attitudes are quickly controlled within a small amplitude steady-state fluctuation range.

[0244] In terms of speed and height control: From the 3.5s at the start of the backward impact to the 4s at the end, the forward speed increased from 1m / s to 1.6m / s. During the lateral impact, the lateral speed increased from 0 to 0.7m / s. The robot suddenly deviated from the expected speed.

[0245] The controller recalculates the foothold and re-establishes a new balance point through multiple steps. The height drops slightly after the impact and then quickly returns to the 0.75m position.

[0246] In summary, the cascade control framework inherits the anti-disturbance performance of the model prediction method, and improves the stability of the robot's floating base posture and height control through task hierarchical optimization control, greatly improving the forward speed of the robot's walking gait dynamic motion. The simulation experiment in this paper uses a physical three-dimensional model of a bipedal robot, and the physical quantity values ​​are close to the real thing, so the research on the robot trajectory tracking high-speed motion anti-disturbance control method completed in this paper has a good theoretical guiding significance for realizing dynamic control on the real object.

[0247] By combining task hierarchical optimization control with model predictive control, under the hierarchical optimization based on the overall model, the attitude and height of the floating base are more accurately controlled, the stability of the robot is better coordinated, the robot's anti-disturbance ability and movement speed are improved, and a high-speed motion cascade control framework for bipedal robots considering leg dynamics is proposed. Through simulation experiments on high-speed forward motion of walking gait and external impact stability, while maintaining the anti-disturbance performance of the model prediction method, the maximum forward speed of the robot's walking gait dynamics is greatly increased from 1.9m / s of model predictive control to 2.5m / s.

[0248] It should be understood that although Figure 1The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0249] like Fig.10 As shown, the present application also provides a vehicle-mounted multi-band stereoscopic vision perception device, which, in one embodiment, includes: an acquisition module 1002, an estimation module 1004, a model prediction module 1006, a planning module 1008, a hierarchical optimization module 1010 and a control module 1012, specifically:

[0250] An acquisition module 1002 is used to acquire an expected speed and an expected angular velocity of the biped robot, and calculate an expected base state according to the expected speed and the expected angular velocity;

[0251] An estimation module 1004 is used to obtain the motion state of the biped robot, and perform base state estimation and foot end contact force estimation according to the motion state;

[0252] A model prediction module 1006 is used to construct a single rigid body model according to the expected base state and the base state estimation, and calculate the control force rotation of the supporting leg of the biped robot;

[0253] A planning module 1008, for calculating the landing point of the swinging leg of the biped robot according to the expected speed, and obtaining a trajectory plan;

[0254] The hierarchical optimization module 1010 is used to make the foot-end contact force track the control force screw according to the control force screw and the trajectory planning, and calculate the relaxation variable; calculate the joint control torque of the biped robot according to the current foot-end contact force and the relaxation variable;

[0255] The control module 1012 is used to control the movement of the biped robot in real time according to the joint control torque.

[0256] The specific definition of a biped robot cascade control device based on task hierarchical optimization can be found in the above definition of a biped robot cascade control method based on task hierarchical optimization, which will not be repeated here. Each module in the above device can be implemented in whole or in part by software, hardware and a combination thereof. Each of the above modules can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0257] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0258] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. Cascade control method of bipedal robot based on task hierarchical optimization, It is characterized in that include: Obtaining an expected speed and an expected angular velocity of the biped robot, and calculating an expected base state according to the expected speed and the expected angular velocity; Acquire the motion state of the biped robot, and estimate the base state and the foot end contact force according to the motion state; According to the desired base state and the base state estimation, a single rigid body model is constructed to calculate the control force rotation of the supporting leg of the biped robot; According to the expected speed, the landing point of the swinging leg of the biped robot is calculated, and the trajectory planning is obtained; According to the control force screw and the trajectory planning, the foot end contact force is made to track the control force screw, and a relaxation variable is calculated; Calculate the joint control torque of the biped robot according to the current foot end contact force and the relaxation variable; According to the control force screw and the trajectory planning, the foot end contact force is made to track the control force screw, and a relaxation variable is calculated; Calculating the joint control torque of the biped robot according to the current foot-end contact force and the relaxation variable includes: constructing different priority tasks according to the control force screw and the trajectory planning; according to the priority tasks, making the foot-end contact force track the control force screw, and constructing a hierarchical optimization algorithm; solving the hierarchical optimization algorithm in sequence according to the order of priority tasks to obtain relaxation variables; calculating the joint control torque of the biped robot according to the current foot-end contact force and the relaxation variable using a dynamic decomposition model; constructing different priority tasks according to the control force screw and the trajectory planning; according to the priority tasks, making the foot-end contact force track the control force screw, and constructing a hierarchical optimization algorithm includes: Constructing different priority tasks according to the control force screw and the trajectory planning; The first priority task is dynamic constraints: ; ; ; In the formula, is the inertia matrix of the robot's overall dynamics, represents the robot generalized vector, including the floating base inertial pose and joint angles, represents centrifugal force, Coriolis force term, and gravity term, is the selection matrix of the active joint, T is the transpose of the matrix, is the active drive joint torque, is the foot contact Jacobian matrix, is the foot contact force torque exerted by the ground on the robot, represents the foot end force rotation calculated by model predictive control, To calculate the foot contact force compensation, the three dynamic matrices are decomposed into , , , is the inertia matrix of the floating base part, are the centrifugal force, Coriolis force term and gravity term of the floating base. is the Jacobian matrix of the foot contact of the floating base part, is the inertia matrix of the leg, are the centrifugal force, Coriolis force term, and gravity term of the leg, is the contact Jacobian matrix of the foot end of the leg; The second priority task is the foot end contact non-slip constraint: ; ; The third priority task is floating base attitude and altitude: ; ; In the formula, , , is the position, velocity, and acceleration of the desired trajectory of the floating base attitude and height, is the control quantity calculated for feedback control, and For the actual state, and is the controller diagonal gain matrix; is the Jacobian matrix The derivative of The fourth priority task is the end position of the swing leg: ; ; In the formula, , and is the position, velocity, and acceleration of the desired trajectory of the end position of the swing leg, is the control quantity calculated for feedback control, and For the actual state, and is the controller diagonal gain matrix; is the Jacobian matrix The derivative of The fifth priority task is to minimize the joint torque: ; ; In the formula, is the inertia matrix of the leg, are the centrifugal force, Coriolis force term, and gravity term of the leg, is the contact Jacobian matrix of the foot end of the leg; The sixth priority task is to minimize the foot end contact force compensation: ; In the formula, is the base inertia matrix; According to the priority tasks, the foot end contact force is made to track the control force rotation quantity, and a hierarchical optimization algorithm is constructed: ; In the formula, refers to the left-hand matrix of the first to sixth priorities, refers to the optimization variable matrix, refers to the right-hand side matrix of the first to sixth priorities, i =1,2,3…6; The movement of the biped robot is controlled in real time according to the joint control torque.

2. The method according to claim 1, It is characterized in that When solving the hierarchical optimization algorithm in sequence according to the order of priority tasks, equality constraints and inequality constraints also need to be satisfied; Equality constraints: ; The physical meaning of the above formula is The optimal solution of the priority task corresponds to To be with before indivual are equal, respectively, where Refers to the left-hand matrix of the first to sixth priority levels, which is the same as Synchronous corresponding, Refers to the optimization variable matrix, which is related to Synchronous corresponding ; Inequality constraints: ; ; In the formula, Finger joint torque, minimum torque, maximum torque; refers to the constraint matrix, Refers to the foot contact matrix.

3. The method according to claim 1 or 2, It is characterized in that According to the desired base state and the base state estimation, a single rigid body model is constructed, and the control force rotation quantity of the supporting leg of the biped robot is calculated, including: According to the desired base state and the base state estimation, a cost function for trajectory tracking is constructed, and under the conditions of satisfying the dynamic constraints, inequality constraints and equality constraints of a single rigid body model, the cost function is solved to obtain the control force spinor of the supporting leg of the biped robot.

4. The method according to claim 3, It is characterized in that The conditions for satisfying the dynamic constraints of a single rigid body model include: ; ; ; ; ; ; ; ; ; ; In the formula, represents the future state of the control system, represents the current state of the control system, that is, the state vector of the robot, represents the control quantity of the control system, represents the system matrix of the control system, represents the control matrix of the control system; is the discrete time step, generally the control time step; is the inertia matrix of the base in the inertial system; is the position of the foot of the supporting leg relative to the origin of the base system in the inertial system, and the direction is from the origin of the base system to the foot. L and R Represents left and right legs respectively; Indicates the rotation force at the left leg foot end. It indicates the right leg foot end force rotation; The base attitude angle is roll, pitch, and yaw. The position of the base in the inertial system; and are the angular velocity and linear velocity in the inertial system respectively; is the acceleration due to gravity; is the yaw angle of the base, Pedestal pitch angle.

5. The method according to claim 4, It is characterized in that The conditions for satisfying the inequality constraints include: ; ; ; In the formula, Indicates the foot end force rotation, The normal vector representing the contact position between the sole of the foot and the ground, Refers to the foot end force in the vertical direction, The friction coefficient, Refers to the foot end force in the horizontal direction. Refers to the foot end force in the lateral direction. refers to the moment in the yaw direction, It represents the distance from the projection point of the ankle joint on the ground to the front contact point. It represents the distance from the projection point of the ankle joint on the ground to the rear contact point.

6. The method according to claim 5, It is characterized in that The conditions for satisfying the equality constraints include: ; ; In the formula, k refer to k time, i refer to i step.

7. The method according to claim 1 or 2, It is characterized in that According to the expected speed, the landing point of the swinging leg of the biped robot is calculated, and the trajectory planning is obtained including: According to the expected speed, the landing point of the bipedal robot's swinging leg is calculated: ; ; ; In the formula, For the foothold, the first item is the real-time position of the floating base in the inertial system, and the second term is the position from the base system to the hip in the inertial system. The third and fourth items are the heuristic foot point calculation formulas proposed by Raibert. It represents the actual speed of the robot in the inertial system. It represents the expected speed of the robot in the inertial system. The fifth item is the foot compensation item during yaw steering. Indicates the height of the robot, represents the expected yaw rate; Based on the foothold, calculate the trajectory plan: ; ; In the formula, represents the normalized amount of time, Indicates to lift your feet. represents the first intermediate control point, represents the second intermediate control point, , Indicates the landing point.

8. A bipedal robot cascade control device based on task hierarchical optimization. It is characterized in that include: An acquisition module, used to acquire an expected speed and an expected angular velocity of the biped robot, and calculate an expected base state according to the expected speed and the expected angular velocity; An estimation module, used for acquiring the motion state of the biped robot, and performing base state estimation and foot end contact force estimation according to the motion state; A model prediction module, used to construct a single rigid body model according to the expected base state and the base state estimation, and calculate the control force rotation of the supporting leg of the biped robot; A planning module, used to calculate the landing point of the swinging leg of the biped robot according to the expected speed and obtain trajectory planning; A hierarchical optimization module, configured to make the foot end contact force track the control force screw and calculate relaxation variables according to the control force screw and the trajectory planning; Calculate the joint control torque of the biped robot according to the current foot-end control force and the relaxation variable; According to the control force screw and the trajectory planning, the foot end contact force is made to track the control force screw, and a relaxation variable is calculated; Calculating the joint control torque of the biped robot according to the current foot-end contact force and the relaxation variable includes: constructing different priority tasks according to the control force screw and the trajectory planning; according to the priority tasks, making the foot-end contact force track the control force screw, and constructing a hierarchical optimization algorithm; solving the hierarchical optimization algorithm in sequence according to the order of priority tasks to obtain relaxation variables; calculating the joint control torque of the biped robot according to the current foot-end contact force and the relaxation variable using a dynamic decomposition model; constructing different priority tasks according to the control force screw and the trajectory planning; according to the priority tasks, making the foot-end contact force track the control force screw, and constructing a hierarchical optimization algorithm includes: Constructing different priority tasks according to the control force screw and the trajectory planning; The first priority task is dynamic constraints: ; ; ; In the formula, is the inertia matrix of the robot's overall dynamics, represents the robot generalized vector, including the floating base inertial pose and joint angles, represents centrifugal force, Coriolis force term, and gravity term, is the selection matrix of the active joint, T is the transpose of the matrix, is the active drive joint torque, is the foot contact Jacobian matrix, is the foot contact force torque exerted by the ground on the robot, represents the foot end force rotation calculated by model predictive control, To calculate the foot contact force compensation, the three dynamic matrices are decomposed into , , , is the inertia matrix of the floating base part, are the centrifugal force, Coriolis force term and gravity term of the floating base. is the Jacobian matrix of the foot contact of the floating base part, is the inertia matrix of the leg, are the centrifugal force, Coriolis force term, and gravity term of the leg, is the contact Jacobian matrix of the foot end of the leg; The second priority task is the foot end contact non-slip constraint: ; ; The third priority task is floating base attitude and altitude: ; ; In the formula, , , is the position, velocity, and acceleration of the desired trajectory of the floating base attitude and height, is the control quantity calculated for feedback control, and For the actual state, and is the controller diagonal gain matrix; is the Jacobian matrix The derivative of The fourth priority task is the end position of the swing leg: ; ; In the formula, , and is the position, velocity, and acceleration of the desired trajectory of the end position of the swing leg, is the control quantity calculated for feedback control, and For the actual state, and is the controller diagonal gain matrix; is the Jacobian matrix The derivative of The fifth priority task is to minimize the joint torque: ; ; In the formula, is the inertia matrix of the leg, are the centrifugal force, Coriolis force term, and gravity term of the leg, is the contact Jacobian matrix of the foot end of the leg; The sixth priority task is to minimize the foot end contact force compensation: ; In the formula, is the base inertia matrix; According to the priority tasks, the foot end contact force is made to track the control force rotation quantity, and a hierarchical optimization algorithm is constructed: ; In the formula, refers to the left-hand matrix of the first to sixth priorities, refers to the optimization variable matrix, refers to the right-hand side matrix of the first to sixth priorities, ; The control module is used to control the movement of the biped robot in real time according to the joint control torque.

Images