CN115328186A - Task layering optimization-based biped robot cascade control method and device - Google Patents
Task layering optimization-based biped robot cascade control method and device Download PDFInfo
- Publication number
- CN115328186A CN115328186A CN202211129303.4A CN202211129303A CN115328186A CN 115328186 A CN115328186 A CN 115328186A CN 202211129303 A CN202211129303 A CN 202211129303A CN 115328186 A CN115328186 A CN 115328186A
- Authority
- CN
- China
- Prior art keywords
- control
- force
- foot
- biped robot
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
- B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
- B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
Abstract
The application relates to a biped robot cascade control method based on task hierarchical optimization, which comprises the following steps: acquiring the expected speed and the expected angular speed of the biped robot, and calculating the expected base state according to the expected speed and the expected angular speed; acquiring the motion state of the biped robot, and estimating the base state and the foot end contact force according to the motion state; according to the expected base state and the base state estimation, a single rigid body model is constructed, and the control force rotation amount of the supporting legs of the biped robot is calculated; according to the expected speed, calculating the foot falling point of the swing leg of the biped robot and obtaining a track plan; according to the control force rotation and the trajectory planning, enabling the foot end contact force to track the control force rotation, and calculating a relaxation variable; calculating joint control moment of the biped robot according to the current foot end contact force and the relaxation variable; and controlling the motion of the biped robot in real time according to the joint control moment. By adopting the method, the high-speed motion cascade control of the biped robot can be realized.
Description
Technical Field
The application relates to the technical field of biped robot cascade control, in particular to a biped robot cascade control method and device based on task layering optimization.
Background
The biped robot has a motion pattern similar to that of a human being and can freely move in various complex environments in which the human being moves. Meanwhile, people always hope to create robots similar to the human beings, and the robots are used in the fields of entertainment education, medical care, military, industrial manufacturing, operation and rescue in severe dangerous environments and the like. The biped robot has great development potential, has become a research hotspot in the robot field, and represents the comprehensive development level of the robot technology.
In recent years, in the prior art, a foot type robot researches that the gravity center is transferred to a model prediction control frame, and the application effect of the foot type robot is better mainly on a four-foot robot. The frame can be applied to a legged robot by using various models, such as an inverted pendulum model, a single rigid body model and an integral model, and the complexity of the models can directly influence the MPC method type and the solution speed. The method types of MPC frameworks are mainly classified as linear MPC, nonlinear MPC, robust MPC and learning MPC. Since most of the control objects are nonlinear time-varying systems at present, the robustness and the tracking accuracy of the system can be improved by using the robust MPC and the learning MPC.
However, the kernel of the overall control framework is single-step PD control, and this kind of method is weak in 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 linear MPC using a simplified model leads to the loss of dynamic characteristics.
Disclosure of Invention
Therefore, in order to solve the above technical problems, it is necessary to provide a task-based hierarchical optimization biped robot cascade control method, which can more accurately control the attitude and height of the floating base, better coordinate the stability of the robot, improve the disturbance resistance and the movement speed of the robot, and realize the high-speed movement cascade control of the biped robot.
A biped robot cascade control method based on task hierarchical optimization comprises the following steps:
acquiring an expected speed and an expected angular speed of the biped robot, and calculating an expected base state according to the expected speed and the expected angular speed;
acquiring the motion state of the biped robot, and estimating the base state and the foot end contact force according to the motion state;
according to the expected base state and the base state estimation, a single rigid body model is constructed, and the control force rotation quantity of the support leg of the biped robot is calculated;
according to the expected speed, calculating a foot falling point of a swing leg of the biped robot, and obtaining a track plan;
according to the control force rotation and the trajectory plan, enabling the foot end contact force to track the control force rotation, and calculating a relaxation variable; calculating joint control moment of the biped robot according to the current foot end contact force and the relaxation variable;
and controlling the motion of the biped robot in real time according to the joint control moment.
In one embodiment, the relaxation variable is calculated by enabling the foot end contact force to track the control force rotation according to the control force rotation and the trajectory plan; calculating the joint control moment of the biped robot according to the current foot end contact force and the relaxation variable comprises the following steps:
constructing different priority tasks according to the control force rotation and the trajectory plan; according to a priority task, enabling the foot-end contact force to track the control force rotation amount, and constructing a hierarchical optimization algorithm;
sequentially solving the hierarchical optimization algorithm according to the sequence of the priority tasks to obtain a relaxation variable;
and calculating the joint control moment of the biped robot by using a dynamic decomposition model according to the current foot end contact force and the relaxation variable.
In one embodiment, different priority tasks are constructed according to the control force rotation and the trajectory plan; according to the priority task, enabling the foot-end contact force to track the control force rotation quantity, and constructing a hierarchical optimization algorithm comprises the following steps:
constructing different priority tasks according to the control force rotation and the trajectory plan;
the first priority task is a dynamics constraint:
in the formula, M is an inertia matrix of the overall dynamics of the robot,
the generalized vector of the robot is represented, the generalized vector comprises the inertial pose and the joint angle of the floating base, h represents the centrifugal force, the Coriolis force term and the gravity term,
is a selection matrix for the active joint, T is the transpose of the matrix,
in order to actively drive the joint torque,
is a foot-end contact jacobian matrix,
amount of rotation of foot-end contact force applied to the robot by the ground, f c_mpc Represents the calculated foot end force rotation quantity delta f of model predictive control c For the foot end requiring calculationThe contact force compensation quantity and three dynamic matrixes are respectively decomposed into M = [ M = [ [ M ] b ，M a ] T ，h＝[h b ，h a ] T ， I J c ＝[ I J cb I J ca ]，M b Is an inertia matrix of the floating base portion, h b The centrifugal force, the Coriolis force term and the gravity term of the floating base part, I J cb is the foot-end contact Jacobian matrix of the floating base portion, M a Is the inertia matrix of the leg, h a The centrifugal force, the Coriolis force term and the gravity term of the leg, I J ca contacting the foot end of the leg with a Jacobian matrix;
the second priority task is a foot-end contact no-slip constraint:
the third priority task is floating base attitude and height:
in the formula, x d_b ，
The position, velocity, acceleration of the desired trajectory for the attitude and altitude of the floating base,
calculated control quantity, x, for feedback control b And
is in an actual state, k p_b And k d_b A diagonal gain matrix for the controller;
is a Jacobian matrix J IF_task A derivative of (a);
the fourth priority task is the swing leg end pose:
in the formula, x d_f ，
And
the position, the speed and the acceleration of the expected track of the tail end pose of the swing leg,
calculated control quantity, x, for feedback control f And
is in an actual state, k p_f And k d_f A diagonal gain matrix for the controller;
is a Jacobian matrix J IF_task A derivative of (a);
the fifth priority task is that the joint moment is minimum:
in the formula, M a Is the inertia matrix of the leg, h a The centrifugal force, the Coriolis force term and the gravity term of the leg, I J ca contacting the Jacobian matrix for the foot end of the leg;
the sixth priority task is that the compensation quantity of the foot-end contact force is minimum:
in the formula, I is a base inertia matrix;
according to the priority task, enabling the foot end contact force to track the control force rotation amount, and constructing a hierarchical optimization algorithm:
in the formula, A i A left entry matrix referring to first to sixth priorities,
finger optimization variable matrix, b i The right item matrix of the first to sixth priorities is referred to, i =1,2,3 \82306.
In one embodiment, when the hierarchical optimization algorithm is sequentially solved according to the order of the priority tasks, equality constraints and inequality constraints also need to be satisfied;
the equation constrains:
the physical meaning of the above formula is that the optimal solution of the ith priority task corresponds to
Is to be compared with the previous (i-1)
Are respectively equal to each other, wherein A j The left entry matrix referring to the first to sixth priorities, which is the same as A in the calculation process i The corresponding ones of the synchronization signals are synchronized,
refers to the optimized variable matrix, which is AND in the calculation process
Corresponding j =1,2,3 \ 8230h 6;
the inequality constrains:
τ min ≤τ≤τ max
U· I J c ≤0
in the formula, tau min ，τ max Finger joint moment, minimum moment, maximum moment; u refers to the constraint matrix, J c The finger tips contact the matrix.
In one embodiment, constructing a single rigid body model based on the desired base state and the base state estimate, calculating a control force curl of a bipedal robotic support leg comprises:
and according to the expected base state and the base state estimation, constructing a cost function of trajectory tracking, and solving the cost function under the condition of meeting single rigid body model dynamics constraint, inequality constraint and equality constraint to obtain the control force momentum of the supporting leg of the biped robot.
In one embodiment, the conditions for satisfying the single rigid body model dynamics constraints include:
x(k+1)＝A k ·x(k)+B k ·u(k)
x(k)＝[ I q b,r ， I q b,p ， I ω b ， I υ b ，g] T
k g ＝[0 0 -1] T
k τ ＝[0 0 1] T
wherein x (k + 1) represents the future one-step state of the control system, x (k) represents the current state of the control system, i.e. the state vector of the robot, u (k) represents the control quantity of the control system, A k System matrix representing the control system, B k A control matrix representing a control system; Δ T is a discrete time step, typically a control time step;
is an inertia matrix of the base under an inertia system; I p Bc (q) is the position of the foot end of the support leg relative to the origin of the base system under the inertial system, the direction is from the origin of the base system to the foot end, and subscripts L and R respectively represent the left and right legs;
showing the force rotation of the foot end of the left leg,
representing the force rotation amount of the right leg and foot; I q b,r attitude angles (roll, pitch, yaw) of the base, I q b,p the position of the base under the inertial system; I ω b and I υ b angular velocity and linear velocity under the inertial system respectively; g is gravity acceleration; q. q of b,yaw Is the base yaw angle, q b,p it c h Base pitch angle.
In one embodiment, the condition that satisfies the inequality constraint includes:
f z ≤f z,max
-μf z ≤f x ≤μf z
-μf z ≤f y ≤μf z
in the formula (I), the compound is shown in the specification,
the force rotation quantity of the foot end is shown,
normal vector representing the contact position of the sole of the foot with the ground, f z Refers to the end force of the foot in the vertical direction, mu refers to the coefficient of friction, f x Refers to the end force of the foot in the horizontal direction, f y Foot end force in the lateral direction, τ yaw In the yaw directionMoment of (L) toe Indicating the distance of the ankle joint from the ground projection point to the anterior contact point, L heel Indicating the distance of the ankle joint from the ground projection point to the posterior contact point.
In one embodiment, the conditions that satisfy the constraints of an equation include:
in the formula, k denotes the time k, and i denotes the step i.
In one embodiment, calculating a foot-falling point of a swing leg of the biped robot according to the desired velocity, and obtaining a trajectory plan comprises:
and calculating a foot falling point of a swing leg of the biped robot according to the expected speed:
I v b ＝[ I v bx I v by 0] T
I v b,des ＝[ I v bx,des I v by,des 0] T
in the formula, p f,des To the foot-drop point, the first term p com The second term R being the real-time position of the floating base under the inertial system IB B p B_hip The third term and the fourth term are heuristic drop point calculation formulas proposed by Raibert for the position of the hip from the base frame under the inertial frame, I v b represents the actual speed of the robot under the inertial system, I v b,des representing the expected speed of the robot under an inertial system, the fifth term is a drop-foot compensation term during yaw steering, I q b,pz which is indicative of the height of the robot, I ω bz,des representing a desired yaw rate;
according to the foot falling point, calculating a trajectory plan:
x d_f ＝(1-m) 3 p 0 +3m(1-m) 2 p 1 +3m 2 (1-m)p 2 +m 3 p d
p d ＝p f,des
wherein m is [01 ]]Representing the normalized amount of time, p 0 Indicating the point of lifting the foot, p 1 Denotes the intermediate control point, p 2 Denotes the intermediate control point, p 1 ≠p 2 ，p d Indicating a foot drop point.
Biped robot cascade control device based on task layering optimization includes:
the acquisition module is used for acquiring the expected speed and the expected angular speed of the biped robot and calculating the expected base state according to the expected speed and the expected angular speed;
the estimation module is used for acquiring the motion state of the biped robot and estimating the base state and the foot end contact force according to the motion state;
the model prediction module is used for constructing a single rigid body model according to the expected base state and the base state estimation and calculating the control force momentum of the supporting leg of the biped robot;
the planning module is used for calculating the foot falling point of the swing leg of the biped robot according to the expected speed and obtaining a track plan;
the hierarchical optimization module is used for enabling the foot end contact force to track the control force rotation amount according to the control force rotation amount and the track plan and calculating a relaxation variable; calculating joint control moment of the biped robot according to the current foot end control force and the relaxation variable;
and the control module is used for controlling the motion of the biped robot in real time according to the joint control moment.
The biped robot cascade control method based on task hierarchical optimization provides a high-speed motion trajectory tracking method which combines model prediction considering leg dynamics and task hierarchical optimization overall control. On the basis of a time-sharing control frame of the under-actuated robot, a single rigid body model MPC (model-based control) based on neglecting legs is designed on the upper layer, the attitude and the force momentum in the height direction in the supporting period are calculated, and meanwhile, the forward and lateral speeds of the robot are controlled by adopting a foot drop point principle; a task layering optimization integrated controller considering leg dynamics is used at the lower layer to track the force rotation amount; a foot end contact force relaxation variable is designed in an optimization variable, and the variable is used for compensating the difference of two foot end contact forces in a single rigid body model and a complete dynamic model, so that the link relation between an upper layer model prediction controller and a lower layer integral controller is built. The hydraulically-driven heavy leg and external impact environment are more challenging for the high-speed motion of the biped robot lacking in driving freedom degree, the robustness of the method under high-speed motion and disturbance is verified through a walking gait high-speed forward experiment and an external impact stability simulation experiment, and the maximum walking speed of the robot is increased to 2.5m/s from 1.9m/s of a model prediction method.
Drawings
FIG. 1 is a flowchart of a task-based hierarchical optimization biped robot cascade control method in one embodiment;
FIG. 2 is a schematic diagram of a cascaded control framework in one embodiment;
FIG. 3 is a schematic diagram of a high-speed dynamic movement process of a robot in one embodiment; a is 0.2s and 0m/s, b is 3.3s and 0.3m/s, c is 5s and 0.5m/s, d is 8.5s and 1.2m/s, e is 11.4s and 2m/s, f is 12.8s and 2.2m/s, g is 14.5s and 2.4m/s, and h is 19s and 2.5m/s;
FIG. 4 is a schematic representation of the z-axis foot end contact force and moment in one embodiment; a is the contact force of the left leg in the Z direction, b is the yaw moment of the left leg, c is the contact force of the right leg in the Z direction, and d is the yaw moment of the right leg;
FIG. 5 is a schematic diagram of the Euler angle of the floating base in one embodiment; a is a rolling angle, b is a pitch angle, and c is a yaw angle;
FIG. 6 is a schematic illustration of the position of a floating base member in one embodiment; a is an X axis, b is a Y axis, and c is a Z axis height;
FIG. 7 is a graphical illustration of x and y speed in one embodiment; a is an X axis and b is a Y axis;
FIG. 8 is a schematic view of another embodiment of the Euler angle of the floating base; a is a rolling angle, b is a pitch angle, and c is a yaw angle;
FIG. 9 is a schematic illustration of velocity and altitude trajectories 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;
fig. 10 is a block diagram showing the structure of a tandem control device for a biped robot based on task-based hierarchical optimization in one embodiment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of and not restrictive on the broad application.
As shown in fig. 1, the present application provides a biped robot cascade control method based on task-based hierarchical optimization, which in one embodiment includes the following steps:
step 102, acquiring a desired speed and a desired angular speed of the biped robot, and calculating a desired base state according to the desired speed and the desired angular speed.
The desired speeds include a desired forward speed and a lateral speed.
It is state of the art to calculate the desired base state from the desired velocity and the desired angular velocity.
And step 104, acquiring the motion state of the biped robot, and estimating the base state and the foot end contact force according to the motion state.
The specific estimation process belongs to the prior art, and is not described herein again.
And 106, constructing a single rigid body model according to the expected base state and the base state estimation, and calculating the control force rotation amount of the supporting leg of the biped robot. Specifically, the method comprises the following steps:
and according to the expected base state and the base state estimation, constructing a cost function of trajectory tracking, and solving the cost function under the condition of meeting single rigid body model dynamics constraint, inequality constraint and equality constraint to obtain the control force momentum of the supporting leg of the biped robot.
More specifically:
the conditions for satisfying the single rigid body model dynamics constraints include a floating base single rigid body model discrete state equation:
x(k+1)＝A k ·x(k)+B k ·u(k) (1)
x(k)＝[ I q b,r ， I q b,p ， I ω b ， I υ b ，g] T
k g ＝[0 0 -1] T
k τ ＝[0 0 1] T
where x (k + 1) represents the future one-step state of the control system, x (k) represents the current state of the control system, i.e. the state vector of the robot, and u (k) tableIndicating the control quantity of the control system, A k System matrix representing a control system, B k A control matrix representing a control system; Δ T is a discrete time step, typically a control time step;
an inertia matrix of the base under an inertia system; I p Bc (q) is the position of the foot end of the support leg relative to the origin of the base system under the inertial system, the direction is from the origin of the base system to the foot end, and subscripts L and R respectively represent the left and right legs;
showing the force rotation amount of the left leg and the foot,
representing the force rotation of the foot end of the right leg; I q b,r attitude angles (roll, pitch, yaw) of the base, I q b,p the position of the base under the inertial system; I ω b and I υ b angular velocity and linear velocity under the inertial system respectively; g is gravity acceleration; q. q of b,yaw Is the base yaw angle, q b,pitch Base pitch angle.
In order to track the expected track, a cost function is designed to reflect the tracking performance of the controlled system. The cost function comprises two terms, the first term is that the state track tracking error weighting is as small as possible, the second term is that the control sequence weighting is as small as possible, the optimization variable is the foot end contact force momentum, u = f c_mpc (ii) a Then adding dynamic constraints and physical constraints, and constructing an optimization problem of trajectory tracking as follows:
satisfying the kinetic constraints:
s.t.x(k+i|t)＝A k,t ·x(k+i-1|t)+B k,t ·u(k+i-1|t),i＝1...N p (2b)
the inequality constraint is satisfied:
satisfying the equality constraint:
D i u(k+i-1)＝0, i＝1...N p (2d)
wherein x (k + i | t) is the actual trajectory of the state, x ref (k + i | t) is the state desired trajectory.
N p To predict the number of steps, N c For controlling the number of steps, i is i step, k is k time, and Q and R are weight matrixes respectively.
Wherein, satisfy the condition of inequality constraint, mainly contain friction cone constraint, include:
1) During contact of the foot end with the ground, the ground exerts a ground reaction force only on the foot end and cannot form a pulling force of the ground on the foot end, so the foot end force f in the vertical direction z Must be equal to or greater than zero. The component of the foot end contact force in the direction of the ground normal is greater than or equal to zero considering the undulation of the ground. In addition, the maximum physical driving capability of the joint driver is limited, and the foot end force f z There is a maximum value.
f z ≤f z,max
2) To prevent the foot end from slipping on the ground causing uncontrollable conditions, the forward and lateral direction contact forces are limited to within the friction cone.
-μf z ≤f x ≤μf z
-μf z ≤f y ≤μf z
3) In addition, the foot end contact yaw moment can generate lateral force at the front and back contact points. This lateral force is superimposed on the lateral contact force and therefore also needs to be limited within the friction cone.
In the formula (I), the compound is shown in the specification,
the force rotation quantity of the foot end is shown,
normal vector representing the contact position of the sole of the foot with the ground, f z Refers to the end force of the foot in the vertical direction, mu refers to the coefficient of friction, f x Refers to the end force of the foot in the horizontal direction, f y Foot end force in the lateral direction, τ yaw Refers to the moment in yaw, L toe Indicating the distance of the projected point of the ankle joint on the ground to the front contact point, L heel Representing the distance of the ankle joint from the ground projection point to the posterior contact point.
The method meets the conditions of equality constraint, mainly gait constraint, and combines a gait sequence into an optimization solution by selecting the contact force momentum of the foot end, and comprises the following steps:
1) If the leg is in the swing period, using equation constraint to force the foot end contact force to be zero; if the leg is in the support period, where the foot end contact force curl is unconstrained, its value is calculated from the other constraints. Unifying the left and right legs into one frame results in an 8 x 8 dimensional matrix at one time. When the left leg is supported and the right leg is in a swinging state:
2) When the left leg is in a swinging state with the right leg supported:
in the formula, k denotes the time k, and i denotes the step i.
Based on a kinetic model of a formula (1), converting a model prediction control optimization problem (2 a) into a linear quadratic programming problem (QP) standard form, and obtaining a foot end contact force momentum f through optimization solution in a control period c_mpc 。
The floating base single rigid body model is derived from the overall dynamics by ignoring the dynamic part of the leg, and the model of the upper layers predicts the controller projected foot end contact force f c_mpc Desired foot end contact force f required by task-layered optimization controller with lower layer c_wbc Are not equal, in general, f c_wbc Is greater than f c_mpc Therefore, the text designs a foot end contact force relaxation variable delta f c Using the variable to compensate for the difference in contact force between the two foot ends to obtain the relationship f c_wbc ＝f c_mpc +Δf c Therefore, a link relation between the upper model predictive controller and the lower integrated controller is established. The relaxation variable Δ f is due to the fact that the upper level controller has provided a stable desired median foot end contact force c Down f under global kinetic constraints c_mpc Close, overall appearance is f c_wbc In tracking f c_mpc Thus forming a cascade controller of the model prediction task hierarchical optimization controller.
For the biped robot, only four active joints are arranged on a single leg, and under the unified framework, the gain values corresponding to the two degrees of freedom x and y in the weight matrix Q are only required to be set to be 0, so that the control on the two degrees of freedom in the support period can be abandoned. Without such processing, using four active degrees of freedom to control six degrees of freedom would result in poor control due to underactuation.
And step 108, calculating the foot falling point of the swing leg of the biped robot according to the expected speed, and obtaining a track plan. Specifically, the method comprises the following steps:
and calculating the foot falling point of the swing leg of the biped robot under an inertial coordinate system according to the expected speed:
I v b ＝[ I v bx I v by 0] T
I v b,des ＝[ I v bx,des I v by,des 0] T
in the formula, p f,des To the foot-drop point, the first term p com The second term R being the real-time position of the floating base under the inertial system IB B p B_hip The third term and the fourth term are heuristic drop point calculation formulas proposed by Raibert for the position of the hip from the base frame under the inertial frame, I v b represents the actual speed of the robot under the inertial system, I v b,des the expected speed of the robot under the inertial system is shown, the fifth term is a foot drop compensation term during yaw steering, I q b,pz which represents the height of the robot and, I ω bz,des representing a desired yaw rate;
the x-direction position and the y-direction position of a foot falling point under an inertial system can be obtained through formula calculation, and the z-direction position is detected by 10mm under a standing plane of the robot so as to ensure that the foot end of the robot slightly contacts the ground in advance.
Considering the requirement of track acceleration smoothing, the track of the foot end of the swing leg adopts a third-order Bezier curve, and the form of the curve is shown in the following formula. In a fixed time period, the curve track is determined by four control points, namely a foot lifting point p 0 Intermediate control point p 1 Intermediate control point p 2 And the foot drop point p d Wherein the four control points are all required to be within the reach of the swing leg.
According to the foot falling point, in order to complete the leg lifting height and the smooth transition of the process, calculating the swing leg track plan in real time:
x d_f ＝(1-m) 3 p 0 +3m(1-m) 2 p 1 +3m 2 (1-m)p 2 +m 3 p d
p d ＝p f,des
wherein m is ∈ [01 ]]Representing the normalized amount of time, p 0 Indicating the point of lifting the foot, p 1 Denotes the intermediate control point, p 2 Denotes an intermediate control point, p 1 ≠p 2 ，p d Indicating a foot drop point.
For the planned swing leg position track x d _f Differentiating to obtain desired speed
And acceleration
The following description is required: the current foot end contact force refers to the foot end contact force after tracking and controlling the force rotation.
Specifically, the method comprises the following steps:
constructing different priority tasks according to the control force rotation and the trajectory plan; according to a priority task, enabling the foot-end contact force to track the control force rotation amount, and constructing a hierarchical optimization algorithm;
solving the hierarchical optimization algorithm in sequence according to the sequence of the priority tasks to obtain a relaxation variable;
and calculating the joint control moment of the biped robot by using a dynamic decomposition model according to the current foot end contact force and the relaxation variable.
More specifically:
the basic idea of the task layering optimization controller is to preferentially constrain a whole body dynamics model of the robot, sort all tasks according to priority, and then perform optimization solution according to the sequence, wherein the solution of a low-priority task needs to satisfy a high-priority task. In the optimization solution of each task, a controller of a corresponding space (task space/joint space) is designed on the outer ring layer, and an equality and an inequality are designed on the inner ring layerAnd (4) a trajectory tracking optimization controller under constraint. Using generalized acceleration and contact force compensation as optimization variables
The controller will now be described in detail.
Constructing different priority tasks according to the control force rotation and the trajectory plan;
1) The first priority task: the dynamic constraint and the overall dynamic model of the robot are as follows,
in order to correspond to the optimization variables, a driving-part-free model of the whole dynamics is taken,
is constructed as a function containing the optimization variables,
in the formula, M is an inertia matrix of the overall dynamics of the robot,
the generalized vector of the robot is represented, the generalized vector comprises the inertial pose and the joint angle of the floating base, h represents the centrifugal force, the Coriolis force term and the gravity term,
is a selection matrix of the active joint, T is the transpose of the matrix,
in order to actively drive the joint torque,
is the foot-end contact jacobian matrix,
amount of foot end contact force spin, f, applied to the robot by the ground c_mpc Represents the calculated foot end force rotation quantity delta f of model predictive control c For the foot-end contact force compensation quantity to be calculated, three dynamic matrixes are respectively 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 an inertia matrix of the floating base portion, h b The centrifugal force, the Coriolis force term and the gravity term of the floating base part, I J cb is the foot-end contact Jacobian matrix of the floating base portion, M a Is the inertia matrix of the leg, h a The centrifugal force, the Coriolis force term and the gravity term of the leg, I J ca contacting the foot end of the leg with a Jacobian matrix; subscript A 1 And b 1 Will be used to optimize the design of the cost function in the algorithm.
2) Second priority tasks: the foot end is contacted with the non-sliding constraint,
3) The third priority task: the attitude and height of the floating base, the expected value of the attitude and height of the floating base is consistent with the expected value in the upper layer model predictive controller, the outer ring controller is,
a function is constructed that contains the optimization variables,
the priority of the posture and height tasks is that the posture priority is greater than the height;
in the formula, x d_b ，
The position, velocity, acceleration of the desired trajectory for the attitude and altitude of the floating base,
calculated control quantity, x, for feedback control b And
is an actual state, k p_b And k d_b A diagonal gain matrix for the controller;
is Jacobi matrix J IF_task The derivative of (c).
4) The fourth priority task: the pose of the tail end of the swing leg, an outer ring controller,
is constructed as a function containing the optimization variables,
in the formula, x d_f ，
And
the position, speed and expected trajectory of the end pose of the swing leg,The acceleration of the vehicle is measured by the acceleration sensor,
calculated control quantity, x, for feedback control f And
is an actual state, k p_f And k d_f A diagonal gain matrix for the controller;
is a Jacobian matrix J IF_task The derivative of (c).
5) The fifth priority task: the joint moment is minimum, and according to the active driving part of the whole dynamics,
the expected joint moment tau approaches 0, a function containing an optimization variable is constructed,
in the formula, M a Is the inertia matrix of the leg, h a The centrifugal force, the Coriolis force term and the gravity term of the leg, I J ca the foot end of the leg is contacted with the Jacobian matrix.
6) The sixth priority task: the foot end contact force compensation is minimal, the desired foot end contact force compensation Δ f c Approaching 0, constructing a function containing optimization variables,
wherein I is a base inertia matrix.
After the priority task is determined, enabling the foot end contact force to track the control force rotation amount according to the priority task, and constructing a hierarchical optimization algorithm:
in the formula, A i A left entry matrix referring to first to sixth priorities,
finger optimization variable matrix, b i The right item matrix of the first to sixth priorities is referred to, i =1,2,3 \82306.
The optimization algorithm sequentially carries out optimization solution from the first task to the sixth task according to the sequence of task priorities, and when the hierarchical optimization algorithm is sequentially carried out each time of optimization solution according to the sequence of the priority tasks, equality constraint and inequality constraint are required to be met;
the physical meaning of the equality constraint is that the optimal solution for the ith priority task corresponds
Is to be compared with the previous (i-1)
Respectively equal, the task is to ensure that the solution of the ith priority task does not influence the control target corresponding to the solution of the previous (i-1) high priority task, and strict priority layering is realized:
the physical meaning of the above formula is that the optimal solution of the ith priority task corresponds to
Is to be compared with the previous (i-1)
Are respectively equal to each other, wherein A j The left entry matrix referring to the first to sixth priorities, which is the same as A in the calculation process i Synchronous pairIn the light of the above, the method should be,
means to optimize the variable matrix, which is the AND in the calculation process
Corresponding j =1,2,3 \ 8230h 6;
the inequality constraint comprises joint moment constraint and friction cone constraint:
τ min ≤τ≤τ max
U· I J c ≤0
in the formula, tau min ，τ max Knuckle moment, minimum moment, maximum moment; u refers to a constraint matrix, J c The finger tips contact the matrix.
After the optimization problem is solved to obtain an optimization solution, the dynamic decomposition model is used to calculate the active driving joint torque, namely the joint control torque:
and step 112, controlling the motion of the biped robot in real time according to the joint control moment.
It is required to state that in the present application, the variables are
And
uniformly expressed as the first and second derivatives of the variables; the x, y and z axes respectively point to the advancing direction, the left direction and the vertical upward direction of the robot.
In this embodiment, the operator gives the robot forward and lateral velocity v by means of the remote control handle, as shown in fig. 2 b,x ,v b,y And yaw rate omega b,z Obtaining the desired state x of the floating base by velocity integration and polynomial trajectory planning d . Finite shapeAnd the state machine judges the contact event according to the estimated value of the contact force to obtain a controller mode (control _ mode) and a leg working mode (leg _ mode), and the controller mode and the leg working mode are introduced into a model prediction controller, a swing leg trajectory planner and a task hierarchical optimization controller. Desired state x of floating base d And an actual state estimate
Contact force of the supporting leg is obtained after calculation of the MPC, and in addition, foot falling points and trajectory planning in x, y and z directions of the swing leg are synchronously calculated and output to the task layering optimization controller in real time, and the robot is driven to move after joint control moment is generated. The floating base state estimate is derived from the fusion of IMU acceleration information with support leg kinematics information, and as this is not a focus of attention herein, it will not be described again.
The core of the cascade control framework is a model prediction task hierarchical optimization integrated controller which is mainly divided into two parts: the first part is an upper layer model prediction controller based on a floating base single rigid body model, and the second part is a lower layer task layering optimization controller based on an overall dynamic model.
(1) In the upper layer, the lower-actuated time-sharing control principle is adopted, and one gait cycle is divided into two parts, namely a support period and a swing period. For the studied biped robot, when one leg is in a support period, the posture and the height of the robot are controlled by adopting the mpc based on the single rigid body model, and when the other leg is in a swing period, the advancing and lateral speeds of the robot are controlled by a foot drop point. And then the calculated control force rotation amount and the expected foot end track value of the swing leg are sent to a lower layer controller.
(2) In the lower layer, tasks comprise whole body dynamics constraint, foot end non-slip constraint, pose control task and moment minimum task, and the robot active driving joint moment is optimally calculated based on a task layering optimization controller. Therefore, the target quantity of each part is controllable in time-sharing control, and then the left leg and the right leg alternately reciprocate, so that three-dimensional omnidirectional motion and anti-interference control of high-speed dynamic motion of the biped robot with the large inertia legs are realized.
The biped robot cascade control method based on task hierarchical optimization provides a high-speed motion trajectory tracking method which combines model prediction considering leg dynamics with task hierarchical optimization overall control. On the basis of a time-sharing control frame of the under-actuated robot, a single rigid body model MPC (dynamic control computer) based on neglecting legs is designed on the upper layer, the force momentum of the posture and the height direction in the support period is calculated, and meanwhile, the forward and lateral speeds of the robot are controlled by adopting a foot-falling point principle; a task layering optimization integrated controller considering leg dynamics is used at the lower layer to track the force rotation amount; a foot end contact force relaxation variable is designed in an optimization variable, and the variable is used for compensating the difference of two foot end contact forces in a single rigid body model and a complete dynamic model, so that the link relation between an upper layer model prediction controller and a lower layer integral controller is built. The hydraulically driven heavy legs and the external impact environment are more challenging for the high-speed motion of the biped robot lacking the driving freedom degree, the robustness of the method under the high-speed motion and disturbance is verified through a walking gait high-speed forward experiment and an external impact stability simulation experiment, and the maximum walking speed of the robot is increased to 2.5m/s from 1.9m/s of a model prediction method.
In order to verify the high dynamic disturbance rejection performance of the control frame algorithm, a double-foot robot multi-body dynamics simulation platform based on multi-body dynamics virtual prototype analysis software RecurDyn and mathematical computation software MATLAB is set up, and the mechanical structure and the component weight of the simulation robot are directly generated and introduced by a three-dimensional model of a physical robot.
The control algorithm parameter settings are as follows: the control frequency of the model predictive controller was 100Hz and the weight was 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]) Predicting the step number N p =3, number of control steps N c =1, the smaller number of steps is set mainly to reduce the matrix dimension and increase the computation speed of single optimization. Setting the maximum time of the swing period to be 0.25s in a finite state machine based on the mixture of events and time; in the swing legThe step height in the 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.
TABLE 1 robot model parameters
The optimized control frequency of the task sequence is 500Hz, and the PD gain of the attitude of the floating base task is set to be K p_pos ＝20·diag([3,3,1])，K d_pos ＝2·diag([3,3,1]) The corresponding sequences are roll angle, pitch angle and yaw angle, respectively. High PD gain of K p_h ＝300，K d_body =30. PD gain for the swing leg task is K p_swing ＝5·diag([1,1,1,7])，K d_swing ＝0.5·diag([1,1,1,7]). Because the aim of the algorithm design is to improve the movement speed and the anti-interference capability of the robot, two groups of simulation experiments are carried out, namely (A) a walking gait high-speed advancing experiment and (B) an external impact stability experiment.
Simulation experiment (a): high-speed forward movement of walking gait
A forward speed track of smooth transition is drawn by using a polynomial fitting method, the transition time from zero to the maximum speed is 15s, the maximum expected speed is set to be 2.5m/s, and the expected forward position is obtained by using the integral of the speed. The floating base height is set to 0.75m and the other desired states are all set to 0. A high-speed motion simulation experiment is carried out on flat ground, a high-dynamic motion process diagram of the robot is shown in figure 3, a foot end contact force and moment curve in the z-axis direction is shown in figure 4, a Euler angle and position tracking curve of a floating base of the robot under an inertial system is shown in figure 5, and a speed tracking curve in the x-axis direction and the y-axis direction is shown in figure 7.
As can be seen from the contact force of the foot end in the z-axis direction in FIG. 4, the speed of the robot is increased from 1.8m/s to 2.5m/s from 10s to 15s, and the contact force of the foot end in the supporting stage is increased, which means that the controller provides a force corresponding to the faster and faster advancing speed to control the attitude and height of the robot.
In the Euler angle trace curve of the floating base in FIG. 5, the roll angle can be uniformly controlled within a range of + -0.05 rad due to the forward movement, the pitch angle and the yaw angle exhibit a phenomenon of constant velocity increase in the acceleration stage from 0 to 2.5m/s, and after the velocity is stabilized at 2.5m/s, the two attitude angles also fluctuate periodically and stably with the gait switch in limited amplitudes between-0.08 to 0.05rad and-0.11 to 0.1rad, respectively.
In fig. 6 and 7, relative to the maximum speed of 1.4m/s of VMC and the maximum speed of MPC is 1.9m/s, the position and the forward speed of the robot of the series control frame can track the expected track well, the maximum forward speed reaches 2.5m/s, which is 0.6m/s higher than the maximum forward speed of 1.9m/s of the model predictive controller used alone, and the lifting ratio is 31.5%, which shows that the stable control of the task hierarchical optimization control in the posture and height and the use of the overall dynamic model have great help to the lifting of the dynamic movement speed of the robot. In the lateral position and speed control, since the robot moves in a high speed state, the lateral speed control is deviated to a certain extent, and the lateral displacement is slowly deviated from the 10 th to the 20 th s by a distance of 0.25 m. This offset is acceptable in such high dynamic motions. The height of the robot is always controlled to be at the expected value of 0.75m, which shows that the overall control is higher in dynamic compensation accuracy based on the overall model, and the height of 0.73m in the model prediction control method is improved by 0.02m.
Simulation experiment (B): stability to external impact
In order to verify the stability of the controller against external impact, the experiment applies impact force which lasts for a certain time on the floating base from three directions of backward direction, lateral direction and vertical downward direction respectively in the advancing process of the robot. The maximum impulse satisfying the dynamic balance of the robot after a plurality of tests is respectively as follows: applying impact force 60N along the positive direction of the x axis in the interval of 3.5-4s, wherein the impulse is 30N.s; applying impact force 115N along the positive direction of the y axis in the interval of 6.5-7s, wherein the impulse is 57.5N.s; and applying 150N impact force along the negative direction of the z axis in the interval of 8.5-9s, wherein the impulse is 75N.s. Fig. 8 is a plot of floating base euler angle tracking and fig. 9 shows a plot of velocity tracking and height in the x and y directions.
In terms of attitude control: when the rear impact is applied in the 3.5-4s interval, the pitch angle is gradually increased in the interval until the maximum peak value of-0.096 rad is reached at the 4 th time along with the accumulation of the impulse, the yaw angle is greatly deflected due to the rotation moment formed by the impulse on the supporting leg, the positive and negative peak values reach 0.173rad and-0.16 rad respectively, and the maximum peak value of the rolling angle is-0.145 (rad). In a lateral impact in the interval 6.5-7s, the roll angle is affected much, reaching a maximum peak of 0.17rad at 7 seconds. And then optimizing the priority and force tracking control of WBC (work group) through the foot end contact force momentum and sequence generated by model prediction control, and rapidly controlling the postures in a steady state fluctuation range with a small amplitude through support leg control for 1 second.
In terms of speed and altitude control: the forward velocity increased from 1m/s to 1.6m/s from the 3.5 th to the 4 th of the end time of the backward impact. At the time of the side impact, the side velocity increased from 0 to 0.7m/s. The robot suddenly deviates from the desired speed.
The controller is caused to recalculate the foot drop point and reestablish a new balance point through a multi-step stride. The height dropped slightly after the impact and then quickly returned to the 0.75m position.
In a word, the cascade control framework inherits the disturbance resistance of the model prediction method, improves the stability of the attitude and height control of the floating base of the robot through task layering optimization control, and greatly improves the advancing speed of the dynamic movement of the walking gait of the robot. The simulation experiment in the text uses a biped robot real object three-dimensional model, and the physical quantity numerical value is close to the real object, so the robot track tracking high-speed motion anti-interference control method study completed in the text has better theoretical guiding significance for realizing dynamic control on the real object.
By combining task layered optimization control and model prediction control, the stability of the robot is better coordinated and the disturbance resistance and the movement speed of the robot are improved by more accurately controlling the posture and the height of the floating base under the layered optimization action based on an integral model, so that the high-speed movement cascade control framework of the biped robot considering leg dynamics is provided. Through simulation experiments of high-speed forward movement of walking gait and external impact stability, the maximum dynamic forward speed of walking gait of the robot is greatly increased from 1.9m/s of model prediction control to 2.5m/s while the anti-interference performance of the model prediction method is kept.
It should be understood that, although the steps in the flowchart of fig. 1 are shown in order as indicated by the arrows, the steps are not necessarily performed in order as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least a portion of the steps in fig. 1 may include multiple sub-steps or multiple stages that are not necessarily performed at the same time, but may be performed at different times, and the order of performance of the sub-steps or stages is not necessarily sequential, but may be performed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.
As shown in fig. 10, the present application further provides an apparatus for vehicle-mounted multiband stereoscopic vision perception, which in one embodiment comprises: an obtaining module 1002, an estimating module 1004, a model predicting module 1006, a planning module 1008, a hierarchical optimizing module 1010 and a control module 1012, specifically:
an obtaining module 1002, configured to obtain a desired velocity and a desired angular velocity of the biped robot, and calculate a desired base state according to the desired velocity and the desired angular velocity;
an estimation module 1004, configured to obtain a motion state of the biped robot, and perform base state estimation and foot end contact force estimation according to the motion state;
the model prediction module 1006 is used for constructing a single rigid body model according to the expected base state and the base state estimation, and calculating the control force rotation amount of the biped robot supporting leg;
the planning module 1008 is used for calculating foot falling points of the swing legs of the biped robot according to the expected speed and obtaining a track plan;
a hierarchical optimization module 1010, configured to make the foot end contact force track the control force rotation according to the control force rotation and the trajectory plan, and calculate a relaxation variable; calculating joint control moment of the biped robot according to the current foot end contact force and the relaxation variable;
and the control module 1012 is used for controlling the motion of the biped robot in real time according to the joint control moment.
The specific definition of the biped robot cascade control device based on task hierarchical optimization can be referred to the above definition of the biped robot cascade control method based on task hierarchical optimization, and is not described herein again. The various modules in the above-described apparatus may be implemented in whole or in part by software, hardware, and combinations thereof. The modules can be embedded in a hardware form or independent of a processor in the computer device, and can also be stored in a memory in the computer device in a software form, so that the processor can call and execute operations corresponding to the modules.
The technical features of the above embodiments can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the above embodiments are not described, but should be considered as the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (10)
1. The biped robot cascade control method based on task layering optimization is characterized by comprising the following steps:
acquiring an expected speed and an expected angular speed of the biped robot, and calculating an expected base state according to the expected speed and the expected angular speed;
acquiring the motion state of the biped robot, and estimating the base state and the foot end contact force according to the motion state;
according to the expected base state and the base state estimation, a single rigid body model is constructed, and the control force rotation quantity of the support leg of the biped robot is calculated;
according to the expected speed, calculating the foot falling point of the swing leg of the biped robot and obtaining a track plan;
according to the control force rotation and the trajectory plan, enabling the foot end contact force to track the control force rotation, and calculating a relaxation variable; calculating joint control moment of the biped robot according to the current foot end contact force and the relaxation variable;
and controlling the motion of the biped robot in real time according to the joint control moment.
2. The method of claim 1, wherein calculating a relaxation variable by tracking the foot-end contact force to the control force curl based on the control force curl and the trajectory plan; calculating the joint control moment of the biped robot according to the current foot end contact force and the relaxation variable comprises the following steps:
constructing different priority tasks according to the control force rotation and the trajectory plan; according to a priority task, enabling the foot-end contact force to track the control force rotation amount, and constructing a hierarchical optimization algorithm;
sequentially solving the hierarchical optimization algorithm according to the sequence of the priority tasks to obtain a relaxation variable;
and calculating the joint control moment of the biped robot by using a dynamic decomposition model according to the current foot end contact force and the relaxation variable.
3. The method of claim 2, wherein different priority tasks are constructed according to the control force curl and the trajectory plan; according to the priority task, enabling the foot-end contact force to track the control force rotation quantity, and constructing a hierarchical optimization algorithm comprises the following steps:
constructing different priority tasks according to the control force rotation and the trajectory plan;
the first priority task is a dynamics constraint:
in the formula, M is an inertia matrix of the overall dynamics of the robot,
the generalized vector of the robot is represented, the generalized vector comprises the inertial pose and the joint angle of the floating base, h represents the centrifugal force, the Coriolis force term and the gravity term,
is a selection matrix of the active joint, T is the transpose of the matrix,
in order to actively drive the joint torque,
is the foot-end contact jacobian matrix,
amount of foot end contact force spin, f, applied to the robot by the ground c_mpc Represents the calculated foot end force spin, Δ f, of the model predictive control c For the foot end contact force compensation quantity needing to be calculated, three matrixes of dynamics are respectively 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 an inertia matrix of the floating base portion, h b The centrifugal force, the Coriolis force term and the gravity term of the floating base part, I J cb is the foot-end contact Jacobian matrix of the floating base portion, M a Is the inertia matrix of the leg, h a The centrifugal force, the Coriolis force term and the gravity term of the leg, I J ca contacting the Jacobian matrix for the foot end of the leg;
the second priority task is a foot-end contact no-slip constraint:
the third priority task is floating base attitude and height:
in the formula, x d_b ，
For floating the base attitude andthe position, velocity, acceleration of the highly desired trajectory,
calculated control quantity, x, for feedback control b And
is in an actual state, k p_b And k d_b A diagonal gain matrix for the controller;
is a Jacobian matrix J IF_task A derivative of (d);
the fourth priority task is swing leg end pose:
in the formula, x d_f ，
And
the position, the speed and the acceleration of the expected track of the end pose of the swing leg,
calculated control quantity, x, for feedback control f And
is in an actual state, k p_f And k d_f A diagonal gain matrix for the controller;
is a Jacobian matrix J IF_task A derivative of (a);
the fifth priority task is that the joint moment is minimum:
in the formula, M a Is the inertia matrix of the leg, h a The centrifugal force, the Coriolis force term and the gravity term of the leg, I J ca contacting the Jacobian matrix for the foot end of the leg;
the sixth priority task is that the compensation quantity of the foot end contact force is minimum:
in the formula, I is a base inertia matrix;
according to the priority task, enabling the foot end contact force to track the control force rotation amount, and constructing a hierarchical optimization algorithm:
in the formula, A i A left entry matrix referring to first to sixth priorities,
finger optimization variable matrix, b i The right entry matrix referring to the first to sixth priorities, i =1,2,3 \ 82306.
4. The method of claim 3, wherein equality constraints and inequality constraints are also satisfied when solving the hierarchical optimization algorithm in turn in order of priority tasks;
and (3) constraint of an equation:
the physical meaning of the above formula is that the optimal solution of the ith priority task corresponds to
Is to be compared with the previous (i-1)
Are respectively equal, wherein A j The left entry matrix referring to the first to sixth priorities, which is the matrix of A's in the calculation process i The corresponding ones of the synchronization signals are synchronized,
refers to the optimized variable matrix, which is AND in the calculation process
Corresponding j =1,2,3 \ 8230h 6;
the inequality constrains:
τ min ≤τ≤τ max
U· I J c ≤0
in the formula, tau min ，τ max Finger joint moment, minimum moment, maximum moment; u refers to a constraint matrix, J c The finger tips contact the matrix.
5. The method of any of claims 1 to 4, wherein a single rigid body model is constructed from the desired base state and the base state estimates, and wherein calculating the control force momentum of the bipedal robotic support leg comprises:
and constructing a cost function of track tracking according to the expected base state and the base state estimation, and solving the cost function under the condition of satisfying single rigid body model dynamics constraint, inequality constraint and equality constraint to obtain the control force momentum of the supporting leg of the biped robot.
6. The method of claim 5, wherein satisfying the conditions for the single rigid body model dynamics constraints comprises:
x(k+1)＝A k ·x(k)+B k ·u(k)
x(k)＝[ I q b,r ， I q b,p ， I ω b ， I υ b ，g] T
k g ＝[0 0 -1] T
k τ ＝[0 0 1] T
wherein x (k + 1) represents the future one-step state of the control system, x (k) represents the current state of the control system, i.e. the state vector of the robot, u (k) represents the control quantity of the control system, A k System matrix representing the control system, B k A control matrix representing a control system; Δ T is a discrete time step, typically a control time step;
is an inertia matrix of the base under an inertia system; I p Bc (q) is the position of the foot end of the support leg relative to the origin of the base system under the inertial system, the direction is from the origin of the base system to the foot end, and subscripts L and R respectively represent the left and right legs;
showing the force rotation amount of the left leg and the foot,
representing the force rotation of the foot end of the right leg; I q b,r attitude angles (roll, pitch, yaw) of the base, I q b,p the position of the base under the inertial system; I ω b and I υ b angular velocity and linear velocity under the inertial system respectively; g is the acceleration of gravity; q. q of b,yaw Is the base yaw angle, q b,pitch Base pitch angle.
7. The method of claim 6, wherein satisfying the inequality constraint comprises:
f z ≤f z,max
-μf z ≤f x ≤μf z
-μf z ≤f y ≤μf z
in the formula (I), the compound is shown in the specification,
the force rotation amount of the foot end is shown,
normal vector representing the contact position of the sole of the foot with the ground, f z Refers to the end force of the foot in the vertical direction, mu refers to the coefficient of friction, f x Refers to the end force of the foot in the horizontal direction, f y Refers to the foot end force in the lateral direction, tau yaw Refers to the moment in yaw, L toe Indicating the distance of the projected point of the ankle joint on the ground to the front contact point, L heel Indicating the distance of the ankle joint from the ground projection point to the posterior contact point.
8. The method of claim 7, wherein satisfying the constraint of an equation comprises:
in the formula, k denotes the time k, and i denotes the step i.
9. The method of any one of claims 1 to 4, wherein calculating a drop point of a swing leg of the biped robot based on the desired velocity and deriving a trajectory plan comprises:
and calculating the foot falling point of the swing leg of the biped robot according to the expected speed:
I v b ＝[ I v bx I v by 0] T
I v b,des ＝[ I v bx,des I v by,des 0] T
in the formula, p f,des As a foothold, the first term p com The second term R being the real-time position of the floating base under the inertial system IB B p B_hip The third term and the fourth term are heuristic drop point calculation formulas proposed by Raibert for the position from the base frame to the hip under the inertial frame, I v b represents the actual speed of the robot under the inertial system, I v b,des representing the expected speed of the robot under an inertial system, the fifth term is a drop-foot compensation term during yaw steering, I q b,pz which represents the height of the robot and, I ω bz,des representing a desired yaw rate;
according to the foot falling point, calculating a trajectory plan:
x d_f ＝(1-m) 3 p 0 +3m(1-m) 2 p 1 +3m 2 (1-m)p 2 +m 3 p d
p d ＝p f,des
wherein m is ∈ [01 ]]Representing the normalized amount of time, p 0 Indicating the point of lifting the foot, p 1 Denotes a first intermediate control point, p 2 Denotes a second intermediate control point, p 1 ≠p 2 ，p d Indicating a foot drop point.
10. Biped robot cascade control device based on task layering optimization, its characterized in that includes:
the acquisition module is used for acquiring the expected speed and the expected angular speed of the biped robot and calculating the expected base state according to the expected speed and the expected angular speed;
the estimation module is used for acquiring the motion state of the biped robot and estimating the base state and the foot end contact force according to the motion state;
the model prediction module is used for constructing a single rigid body model according to the expected base state and the base state estimation and calculating the control force momentum of the supporting leg of the biped robot;
the planning module is used for calculating the foot falling point of the swing leg of the biped robot according to the expected speed and obtaining a track plan;
the hierarchical optimization module is used for enabling the foot end contact force to track the control force rotation amount according to the control force rotation amount and the track plan, and calculating a relaxation variable; calculating joint control moment of the biped robot according to the current foot end control force and the relaxation variable;
and the control module is used for controlling the motion of the biped robot in real time according to the joint control moment.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211129303.4A CN115328186A (en) | 2022-09-14 | 2022-09-14 | Task layering optimization-based biped robot cascade control method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211129303.4A CN115328186A (en) | 2022-09-14 | 2022-09-14 | Task layering optimization-based biped robot cascade control method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115328186A true CN115328186A (en) | 2022-11-11 |
Family
ID=83929872
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211129303.4A Pending CN115328186A (en) | 2022-09-14 | 2022-09-14 | Task layering optimization-based biped robot cascade control method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115328186A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115709471A (en) * | 2023-01-10 | 2023-02-24 | 深圳鹏行智能研究有限公司 | Robot state value adjusting method and robot |
-
2022
- 2022-09-14 CN CN202211129303.4A patent/CN115328186A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115709471A (en) * | 2023-01-10 | 2023-02-24 | 深圳鹏行智能研究有限公司 | Robot state value adjusting method and robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3972854B2 (en) | Robot motion control device | |
| US7400939B2 (en) | Robot device, motion control device for robot device and motion control method | |
| KR102044437B1 (en) | Balancing control apparatus of robot and method for controlling the same | |
| US7053577B2 (en) | Robot and motion control method of robot | |
| US7805218B2 (en) | Robot device and control method of robot device | |
| US8825213B2 (en) | Gait generating device for legged mobile robot and operational target generating device for robot | |
| US8682488B2 (en) | Humanoid robot and walking control method thereof | |
| US20120303162A1 (en) | Mobile object controller and floor surface estimator | |
| JP2001157973A (en) | Walk control device, and walk control method for robot | |
| US7765030B2 (en) | Gait generator for mobile robot | |
| JP2011093025A (en) | Control device for legged mobile robot | |
| WO2006067904A1 (en) | Legged mobile robot and gait generating device | |
| Buschmann et al. | Biped walking control based on hybrid position/force control | |
| Buschmann et al. | Experiments in fast biped walking | |
| CN112643679A (en) | Robot motion control method, device, controller and storage medium | |
| Li et al. | Force-and-moment-based model predictive control for achieving highly dynamic locomotion on bipedal robots | |
| JP6781101B2 (en) | Non-linear system control method, biped robot control device, biped robot control method and its program | |
| CN115328186A (en) | Task layering optimization-based biped robot cascade control method and device | |
| Seiwald et al. | Quintic spline collocation for real-time biped walking-pattern generation with variable torso height | |
| JP2004142095A (en) | Robot system and control method therefor | |
| Lorch et al. | ViGVVaM—An Emulation Environment for a Vision Guided Virtual Walking Machine | |
| Nagarajan et al. | Integrated motion planning and control for graceful balancing mobile robots | |
| Shahrokhshahi et al. | Optimal stair climbing pattern generation for humanoids using virtual slope and distributed mass model | |
| CN113618740B (en) | Dance balance control method and device for biped robot and biped robot | |
| JP2002210681A (en) | Motion control device and method for leg-type moving robot, and robot device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |