CN115256396B - Double-layer model predictive control-based omnidirectional walking centroid track planning method for bipedal robot - Google Patents
Double-layer model predictive control-based omnidirectional walking centroid track planning method for bipedal robot Download PDFInfo
- Publication number
- CN115256396B CN115256396B CN202211010867.6A CN202211010867A CN115256396B CN 115256396 B CN115256396 B CN 115256396B CN 202211010867 A CN202211010867 A CN 202211010867A CN 115256396 B CN115256396 B CN 115256396B
- Authority
- CN
- China
- Prior art keywords
- centroid
- track
- mpc
- sparse
- dense
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 239000013598 vector Substances 0.000 claims description 68
- 239000011159 matrix material Substances 0.000 claims description 38
- 230000008569 process Effects 0.000 claims description 16
- 125000004122 cyclic group Chemical group 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 abstract description 3
- 238000004364 calculation method Methods 0.000 description 14
- 206010034701 Peroneal nerve palsy Diseases 0.000 description 12
- 210000002683 foot Anatomy 0.000 description 12
- 238000005070 sampling Methods 0.000 description 7
- 230000014509 gene expression Effects 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000036544 posture Effects 0.000 description 3
- 210000000544 articulatio talocruralis Anatomy 0.000 description 2
- 238000012300 Sequence Analysis Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008602 contraction Effects 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 210000004744 fore-foot Anatomy 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a biped robot omnidirectional walking centroid track planning method based on double-layer model predictive control, wherein an upper MPC obtains a sparse centroid motion track and a ZMP track under the condition of meeting ZMP stability constraint and centroid motion feasibility domain constraint; the lower MPC takes the tracking upper sparse track as a target to obtain a dense centroid position motion track; after the dense track is judged to be free of divergence by the centroid track detector, the centroid movement position at the current moment is output for the robot to use. The invention adopts a variable-period double-layer model predictive control structure, combines the accuracy and the real-time performance of the generated track, and can prevent the divergent track from damaging the robot hardware through centroid track detection.
Description
Technical Field
The invention relates to the technical field of biped robots, in particular to a biped robot omnidirectional walking centroid track planning method based on double-layer model predictive control.
Background
The biped robot has a similar motion mode as a human, and adopts biped alternation to contact with the ground so as to realize walking motion. In application scenes such as home service, national defense and military, flexible omnidirectional walking is a necessary premise for improving environment exploration capacity and completing operation tasks of the bipedal robot. But the biped robot has the advantages of higher mass center, small supporting area, more whole body freedom degree and higher difficulty in whole body motion planning and stable control. The method for effectively and rapidly generating the walking centroid movement track which accords with the dynamic characteristics of the robot is an important basis for realizing the whole-body movement planning and stable control of the bipedal robot, and has important significance for realizing flexible omnidirectional walking of the bipedal robot.
The model predictive control (Model Predictive Control, MPC) places a heavy computational burden on predicting the state of future intervals. In practice, high performance computers are often relied upon to increase the computational speed of an MPC. In order to realize online planning of a walking centroid motion trail, in the prior art, a sparse sampling time interval (which is far greater than a control period) is generally adopted for an MPC prediction interval to reduce the dimension of a matrix and lighten the dependence on computer performance, but the processing can increase the interval of an optimal control sequence obtained by solving, so that the optimal control sequence is inaccurate, the control precision is reduced, and the generated trail is inaccurate.
The prior art lacks a check of the feasibility of performing a trajectory when generating a centroid motion trajectory. If the actual robot executes the divergent unreasonable centroid movement track, the robot joint is disabled, hardware faults are caused, and the electrical body of the robot is damaged.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a biped robot omnidirectional walking centroid track planning method based on double-layer model predictive control, which can give consideration to the accuracy and calculation speed of an optimal control sequence, thereby generating a relatively reliable centroid movement track on line; in addition, the invention also provides a centroid track feasibility judging criterion based on a centroid motion feasibility domain, which can capture the divergence phenomenon of the generated track, stop the generation process of the motion track when necessary and prevent huge damage to robot hardware.
The present invention achieves the above technical object by the following means.
The omnidirectional walking centroid track planning method of the biped robot based on the double-layer model predictive control comprises the following steps:
After receiving the input of the foothold planning and the centroid state estimation, the upper MPC minimizes the upper MPC objective function under the condition of meeting the zero moment point stability constraint and the feasible domain constraint, and obtains the sparse optimal control sequence by means of solution Obtaining the X-direction sparse centroid movement position track/>, in the prediction interval, through cyclic iteration of the state space equationY-direction sparse centroid motion position track/>X-direction sparse centroid movement speed track/>Y-direction sparse centroid movement velocity track/>X-direction sparse ZMP track/>And Y-direction sparse ZMP track/>
The lower MPC aims at tracking the upper sparse track and solves the sparse track to obtain a dense optimal control sequenceThrough the cyclic iteration of the state space equation, the X-direction dense centroid position track/> isobtainedAnd Y-direction dense centroid position track/>
The centroid track detector judges whether the dense centroid position track diverges according to the feasibility judging criterion, and if the dense centroid position track diverges, the centroid track detector judges the first term value of the dense centroid position trackAnd/>And sending the signals to the bipedal robot for execution.
In the above technical solution, the conditions of the feasible region constraint are:
Wherein the method comprises the steps of And/>Is a sparse coefficient matrix related to geometric parameters of centroid motion feasible domain,/>AndFor X, Y-direction sparse position vector of center point in feasible domain of predicting center of mass movement in interval,/>For X-direction centroid position vector in upper MPC prediction interval,/>And predicting a Y-direction centroid position vector in the interval for the upper layer MPC.
In the above technical solution, the feasible domain specifically includes: the motion direction of the mass center of the biped robot is taken as a gesture angle, two parallel lines are respectively made to pass through the center points of the two sole plates, the intersection point of the two parallel lines and the sole plates is taken as the boundary vertex of the mass center motion feasible region, and a closed rectangle is constructed, namely the mass center motion feasible region at the moment.
In the above technical solution, the optimal control sequenceThe method comprises the following steps:
Wherein: Is the control input vector of the X direction and the Y direction of the world coordinate system in the upper MPC prediction interval,/> And/>The vectors of the falling foot points in the X direction and the Y direction in the prediction interval output by the upper layer MPC;
From corrected state variables in X-and Y-directions And control input vector of upper layer MPCObtaining X-direction sparse centroid movement locus/>Y-direction sparse centroid motion position track/>X-direction sparse centroid movement speed track/>Y-direction sparse centroid movement velocity track/>X-direction sparse ZMP trackAnd Y-direction sparse ZMP track/>
Wherein: upper layer MPC optimal control input sequence Is a coefficient matrix of the upper-layer MPC.
In the above technical solution, the corrected state variables in the X direction and the Y directionThe method meets the following conditions:
Wherein: Is a state variable in the X direction,/> For the state variable in the Y-direction, α is the confidence level for the state estimation of the heartAnd/>A centroid state estimation vector is input for centroid state estimation.
In the above technical solution, the objective function of the lower MPC is:
Wherein: η is the number of times, Mu, lambda is the weight coefficient,/>A dense optimal control sequence solved for the lower MPC;
Tracking the locus/>, of the sparse centroid position of the upper MPC, in the lower MPC objective function And/>Is defined by:
Is used for tracking the sparse centroid speed track/>, of the upper MPC in the lower MPC objective function And/>Is defined by:
Tracking the upper MPC sparse ZMP track/>, in the lower MPC objective function And/>Is defined by:
Is the part of the underlying MPC objective function that directly minimizes the control input:
Where S is a matrix of mapping coefficients, For X-direction centroid position vector in lower MPC prediction interval,/>For Y-direction centroid position vector within lower MPC prediction interval,/>For X-direction centroid velocity vector in lower MPC prediction interval,/>For Y-direction centroid velocity vector within lower MPC prediction interval,/>For the X-direction ZMP position vector in the lower MPC prediction interval,/>For the Y-direction ZMP position vector in the lower MPC prediction interval,/>Is the control input vector of the X direction and Y direction of the world coordinate system in the prediction interval of the lower MPC.
In the above technical solution, the dense optimal control sequence is solved by the following analysis:
wherein: ψ is an analytic coefficient matrix, and ζ x,ξy is a coefficient matrix related to the upper-layer MPC sparse track; and:
Wherein: for the coefficient matrix of the lower MPC,/> Is the corrected state variable in the X-direction and Y-direction.
In the above technical solution, the X-direction dense centroid position trackAnd Y-direction dense centroid position trackThe method meets the following conditions:
in the above technical solution, the feasibility determining criterion is:
Wherein the method comprises the steps of And/>Is a dense coefficient matrix related to geometric parameters of a centroid motion feasible domain,/>AndDense position vectors for center points of centroid motion feasible regions within the prediction interval.
In the above technical scheme, when the current time is t k+1, updating the footdrop point plan and the centroid state estimation, and calculating to obtain the centroid dense position track at the time t k+1 And/>After judging that the track leader meets the feasibility judgment criterion, the track leader/>, and the track leader/>, wherein the track leader/>, and the track leader are the same as the track leaderAnd/>And executing the process for the bipedal robot, and repeating the process until the robot stops moving.
The beneficial effects of the invention are as follows:
(1) According to the omnidirectional walking centroid track planning method of the biped robot, the upper MPC adopts sparse time intervals, the lower MPC adopts dense time intervals, the discrete period is sparse and dense, the accuracy and the calculation speed of an optimal control sequence can be considered, the instantaneity and the accuracy of the walking centroid track planning of the biped robot are ensured, and therefore the accurate centroid movement track of the robot is generated on line;
(2) The invention designs the omnidirectional walking centroid movement feasible region of the biped robot, and also designs the criterion of centroid track feasibility based on the feasible region, thereby capturing the divergence phenomenon of the centroid track of the robot, stopping the generation process of the movement track when necessary and preventing huge damage to the robot hardware.
Drawings
FIG. 1 is a flow chart of planning a movement track of a walking centroid of a bipedal robot;
FIG. 2 is a schematic view of a single-foot and double-foot support period support domain of the bipedal robot according to the present invention;
FIG. 3 is a schematic diagram illustrating security domain constraints according to the present invention;
Fig. 4 is a schematic diagram illustrating the construction of a centroid motion feasible region according to the present invention.
Detailed Description
The invention will be further described with reference to the drawings and the specific embodiments, but the scope of the invention is not limited thereto.
The model predictive control is a rolling time domain control algorithm, and the core idea is to solve the open loop optimal control problem in the future limited interval on line at each moment by using a predictive model based on the current state of a dynamic system, and act the leader of the optimal control sequence obtained by solving on the system. Model predictive control often includes three parts, a predictive model, an objective function, and constraints. The predictive model is a mathematical model describing the process variation of the system, and the objective function and the constraint condition are basic components of the optimal control problem, wherein the objective function is used for describing the control cost in a future limited interval, also called cost function, and the constraint condition is used for describing the constraint condition to be met by the control system in the process of variation.
As shown in fig. 1, the invention provides a biped robot omnidirectional walking centroid track planning method based on double-layer model predictive control, wherein biped foot drop point planning and centroid state estimation are input into a centroid track planner, and the biped foot drop point planning and centroid state estimation method is the prior art; at time t k, the bipedal foot drop plan provides the centroid planning planner with full-dimensional information of the desired foot drop plane within the MPC prediction interval [ t k,tk+Tp](Tp is time interval), including the X-direction plane position vector of the desired foot dropY-direction planar position vector/>, of desired landing pointPose vector of desired foothold/>And a walking cycle vector/>, corresponding to the planar position vector and the posture vectorThe centroid state estimation part accurately estimates an X-direction state estimation vector/>, of the centroid of the robot in the walking motion processY-direction State estimation vector/>For correcting state variables at time t k of the predictive model of the updated MPC; the centroid track planner comprises an upper model predictive controller (upper MPC), a lower model predictive controller (lower MPC) and a centroid track detector; after receiving the input of the foothold planning and the centroid state estimation, the upper MPC minimizes the upper MPC objective function under the condition of meeting the stability constraint and the feasible domain constraint of the zero moment point (Zero Moment Point, ZMP) to obtain a sparse optimal control sequence/>Obtaining the X-direction sparse centroid movement position track/>, in the prediction interval, through cyclic iteration of the state space equationY-direction sparse centroid motion position track/>X-direction sparse centroid movement speed track/>Y-direction sparse centroid movement speed trackX-direction sparse ZMP track/>And Y-direction sparse ZMP track/>The lower MPC aims at tracking an upper sparse track (comprising a sparse centroid movement position track, a sparse centroid movement speed track and a sparse ZMP track) to obtain a dense optimal control sequence/>Through the cyclic iteration of the state space equation, the X-direction dense centroid position track/> isobtainedAnd Y-direction dense centroid position track/>The centroid track detector judges whether the dense centroid position track diverges according to the feasibility judging criterion, and if the dense centroid position track diverges, the centroid track's leader value/>, if not diverges, the centroid track's leader value/>, and if the centroid track's leader value/>, and the centroid track's leader value/>, respectively, and the centroid track's leader value/>, if the centroid's leader value is the leaderAnd/>And sending the signals to the bipedal robot for execution.
A three-dimensional linear inverted pendulum model (3D-LIPM) is used as a prediction model of an upper-layer MPC and a lower-layer MPC, the model is used for decomposing the centroid motion of a robot into a linear inverted pendulum model which is completely decoupled and similar in two directions of an X axis and a Y axis under a world coordinate system, and a discrete state space expression is as follows:
wherein: a is a system matrix, B is an input matrix, C is an output matrix, and expressions of the system matrix, the input matrix and the output matrix are respectively:
Wherein T is a discrete period of the state, and the upper MPC and the lower MPC use different discrete period values; g is a gravity acceleration constant value, h c is a world coordinate system Z-axis direction centroid height constant value;
For a state variable in the direction of the time c (representing the direction of the X axis or the Y axis under the world coordinate system) at the time t k, u k is a control input in the direction of the time c at the time t k, z k is a linear inverted pendulum model output after decoupling at the time t k, and the expressions are as follows:
Where c k is the centroid position in the c direction at time t k, Centroid speed in the direction of time c, t k,/>Centroid acceleration in the direction of c at time t k,/>Barycenter jerk in direction c at time t k,/>The ZMP position in the direction of time c at time t k.
At time t k, based on the current state variableBy loop iteration (formula (1)), state variables at any time can be predicted in a prediction interval [ t k,tk+Tp ], and the calculation formula is as follows:
Wherein: a i and A j respectively represent multiplication of i and j system matrixes, and u k+i-1-j is a direction control input at a time c of t k+i-1-j; n is the number of discrete sampling points in a prediction interval, and the calculation formula is as follows:
Based on the formula (4), all state variables in the prediction interval [ t k,tk+Tp ] are disassembled and combined, and the current state variable can be used And control input vector/>, within prediction intervalCentroid position vector for respectively representing c direction in prediction intervalCentroid velocity vector/>Centroid acceleration vector/>ZMP position vector/>Namely:
Wherein phi is ps,Φvs,Φas Coefficient matrix corresponding to current state variable phi pu,Φvu,Φau And (3) obtaining a coefficient matrix corresponding to the centroid acceleration vector by iterative calculation of the formula (4), wherein the coefficient matrix is related to the discrete period. /(I)Representing an N x 3-dimensional linear space in the real number domain, the rest of the representation and so on.
The method comprises the steps of (1) adopting a sparse sampling strategy for an upper MPC to improve the calculation speed of the part; taking discrete cycles of an upper MPCGreater than the control period T c, there are:
Wherein the method comprises the steps of N up is the number of sampling points in the upper MPC prediction interval, which is an integer greater than 1.
When using discrete periods of upper MPCWhen the coefficient matrix of the formulas (6) - (9) isAnd/>
The quadratic objective function of the upper MPC is designed as follows:
Wherein alpha, beta, gamma, omega are weight coefficients, The expression of the optimal control sequence to be solved by the upper MPC is as follows:
Wherein the method comprises the steps of Is the control input vector of the X direction and the Y direction of the world coordinate system in the upper MPC prediction interval,/>And/>The vectors of the falling foot points in the X direction and the Y direction in the prediction interval output by the upper layer MPC;
is the part of the upper layer MPC objective function that directly minimizes the control input:
is the part of the upper layer MPC objective function that tracks the desired centroid average speed input:
is the part of the upper layer MPC objective function that tracks the desired ZMP:
is the part of the upper layer MPC objective function that tracks the position of the desired foothold:
Wherein: representing the euclidean norms of the number of norms, For the X-direction centroid velocity vector within the upper MPC prediction interval,For Y-direction centroid velocity vector in upper MPC prediction interval,/>For the X-direction ZMP position vector in the upper MPC prediction interval,/>A Y-direction ZMP position vector in the prediction interval of the upper MPC; /(I)For the desired centroid motion velocity vector in the X direction,/>A centroid motion velocity vector that is desired for the Y-direction; /(I)For the X-direction desired ZMP trajectory,/>The ZMP track expected in the Y direction can be obtained by calculation through the foot drop plane full-dimensional information input by the foot drop planning part.
For the upper MPC, stability constraints are designed based on ZMP stability criteria. Because the sole of the biped robot can only bear the supporting force of the ground, ZMP stability criterion requires that the ZMP is always in a supporting polygon to ensure the stability of the motion in the motion process of the robot, wherein the supporting polygon is the minimum convex polygon area of all contact points between the soles of the biped robot and the ground, when the robot is in a monopod supporting period, the supporting polygon area (simply called supporting area) is the shape of the sole of the biped robot, and the supporting area is the minimum convex polygon comprising the two sole plates in the biped supporting period; see fig. 2.
In order to keep a certain stability margin, the supporting domain is contracted inwards for a certain distance and is converted into a safety domain, the mass center movement track generated by constraint is required to meet the constraint condition that the corresponding ZMP track is always in the range of the safety domain in the walking process, and the supporting domain in the single-foot supporting period is considered, and the constraint condition can be expressed as follows:
Wherein the method comprises the steps of And/>Is a coefficient matrix related to the geometrical parameters of the sole plate, the inward contraction distance of the supporting domain and the Z-axis attitude angle of the foot drop point under the world coordinate system. Taking the j-th footfall point in the prediction interval as an example, the security domain constraint diagram is shown in FIG. 3, wherein X w and Y w are the X-axis direction and the Y-axis direction of the world coordinate system, and X foot and Y foot are the X-axis direction and the Y-axis direction of the footfall point coordinate system,/>Is the pose vector/>, of the desired footholdThe Z-axis attitude angle of the jth foothold in the middle is the area where the support area is contracted inwards by a certain distance.
For the upper layer MPC, in order to avoid the divergence of the centroid movement track, the centroid plane movement range constraint condition is designed based on the full-dimensional information of the falling foot point plane, and the centroid movement track required to be planned on line is required to be always in the allowed movement range. During walking of the bipedal robot, the centroid position is located between the two foot positions. In the left-right direction, when the centroid is positioned at the left side of the left foot drop point or the right side of the right foot drop point, the centroid locus divergence can be determined. When the centroid is located on the forefoot side or the rearfoot side, the centroid locus divergence can be determined. Therefore, the invention designs a centroid movement feasible region based on the full-dimensional information of the foothold, as shown in fig. 4. At tk moment, the biped robot is in an omnidirectional walking state, the postures of the two feet are different, at the moment, the position and the movement direction of the mass center are shown in figure 4, two parallel lines are respectively made to pass through the center points of the two sole plates by taking the movement direction of the mass center as a posture angle, the intersection point of the two parallel lines and the sole plates is taken as the boundary vertex of the mass center movement feasible region, and a closed rectangle is constructed, wherein the rectangle is the movement feasible region of the mass center at the moment. In the omnidirectional walking process, the ground projection of the robot supporting leg and foot bottom plate is the supporting leg and foot bottom plate, and the ground projection of the swinging leg and foot bottom plate can be obtained by vertically mapping the swinging leg and foot bottom plate to the ground. In the omnidirectional walking process of the robot, the geometric parameters of the centroid movement feasible region can be changed along with the movement of the legs of the robot, but the geometric parameters of the movement feasible region (including the position of the center point of the rectangle under the world coordinate system and the length and width of the rectangle) can be calculated by the construction method for determining the centroid movement feasible region.
The centroid movement feasible region is a plane position constraint on the forward direction and the transverse direction of the centroid of the robot, and the centroid position is required to be in the centroid movement feasible region in the omnidirectional walking process, and the expression of the feasible region constraint which can be satisfied by the centroid position vector in the prediction interval can be obtained by combining the above feasible region construction description is as follows:
Wherein the method comprises the steps of And/>Is a sparse coefficient matrix related to geometric parameters of centroid motion feasible domain,/>And/>And a X, Y-direction sparse position vector of a center point in a feasible domain of center of mass movement in a prediction interval.
For state variable in robot walking processThe initial value is:
at time t k, a centroid state estimation vector is estimated using the centroid state estimation input Correcting and updating the current state variable of the centroid, namely:
Wherein the method comprises the steps of And/>For the corrected state variables in the X-direction and Y-direction,/>Is a state variable in the X direction,/>Alpha is the confidence coefficient of the state estimation of the heart, and alpha is more than or equal to 0 and less than or equal to 1.
Integrating the objective function, stability constraint and feasible region constraint of the upper MPC, and solving the optimal control sequence by a quadratic programming solver (which is the prior art) based on the corrected and updated state variablesThe control input vector/>, of the upper MPC can be obtained as shown in the formula (13)And the output foot drop position vector/>The output foot drop point position vector can be used for online planning of the ankle joint movement track of the bipedal robot (in the prior art). Will/>In/>Merging to obtain the optimal control input sequence/>, of the upper MPCThe method comprises the following steps:
From corrected state variables in X-and Y-directions And upper MPCBased on formulas (6), (7) and (9), a sparse centroid position track/>, of the upper-layer MPC is obtainedAnd/>Sparse centroid velocity traceAnd/>Sparse ZMP track/>And/>The calculation formula is as follows:
Because of the complexity of objective function and constraint, the upper MPC adopts sparse sampling interval to accelerate the speed of the upper MPC optimizing solution, thus obtaining the optimal control sequence And the tracks are sparse, the time interval (the discrete period of the upper MPC) is larger than the control period, and the optimal control sequence leader represents/>The control input in the interval, such as directly using the optimal control sequence leader to replace the control input in the interval [ t k,tk+Tc ] to calculate the centroid track of the next control period, will cause larger error and/>The larger the error is, so the invention designs the lower MPC to calculate the intensive optimal control sequence, thereby obtaining a more accurate centroid track.
The lower MPC aims at tracking a sparse track obtained by solving the upper MPC and solves a dense optimal control sequenceI.e. discrete period/>, of the underlying MPCShould be the same as control period T c, then:
Wherein N down is the number of sampling points in the lower MPC prediction interval.
When using discrete periods of an underlying MPCWhen the coefficient matrix of the formulas (6) - (9) isAnd/>
The objective function of the underlying MPC is:
wherein eta is defined as the number of times, Mu, lambda is the weight coefficient,/>Dense optimal control sequences solved for the underlying MPC, namely:
the control input vectors of the world coordinate system X direction and Y direction in the lower MPC prediction interval are respectively;
Tracking the locus/>, of the sparse centroid position of the upper MPC, in the lower MPC objective function And/>Is defined by:
Is used for tracking the sparse centroid speed track/>, of the upper MPC in the lower MPC objective function And/>Is defined by:
Tracking the upper MPC sparse ZMP track/>, in the lower MPC objective function Is defined by:
Is the part of the underlying MPC objective function that directly minimizes the control input:
In the formulae (30) - (33) For mapping coefficient matrix, sampling points needing to track the sparse track of the upper MPC in the lower MPC in the prediction interval can be extracted; /(I)For the X-direction centroid position vector within the lower MPC prediction interval,A Y-direction centroid position vector in a prediction interval for the lower MPC; /(I)For X-direction centroid velocity vector in lower MPC prediction interval,/>A Y-direction mass center speed vector in a prediction interval of the lower MPC; /(I)For the X-direction ZMP position vector in the lower MPC prediction interval,/>The Y-direction ZMP position vector in the interval is predicted for the lower MPC.
The lower MPC aims at tracking the upper sparse track, no additional constraint is set, and the lower MPC is unconstrained MPC, so that a dense optimal control sequence analysis solution can be directly obtained, and the analysis solution expression is as follows:
Wherein the method comprises the steps of For the analysis coefficient matrix, the calculation formula is as follows:
Wherein the method comprises the steps of Is a unitary matrix,/>The coefficient matrix is related to the upper MPC sparse track, and the calculation formulas are respectively as follows:
when the weight parameter of the objective function is a constant value, ψ is a constant value matrix, and the matrix and an inverse matrix thereof can be calculated in advance for standby before the centroid locus planner designed by the invention is operated on line, thereby improving the calculation speed of the on-line centroid movement locus planning; the zeta x,ξy is a dynamic matrix which is changed by the omnidirectional walking of the upper MPC sparse track and the corrected state variable, and the matrix except the sparse track and the state variable in the calculation formula can be calculated in advance.
From corrected state variables in X-and Y-directionsDense optimal control with lower MPCControl input vector in sequence/>Based on equation (6), a dense centroid position trace of the underlying MPC is obtained, the calculation of which can be integrated as:
Because of more external inputs and more complex structure of the double-layer MPC, the invention also designs a centroid track detection criterion and a protection mechanism for detecting whether the centroid track planner outputs due to calculation errors, unreasonable external inputs and the like And/>The divergence phenomenon occurs, and the huge damage to the robot hardware caused by the overrun of joints and the like is prevented.
Centroid trajectory detector utilizes centroid motion feasible domain constraint pairs of upper layer MPCAnd/>Detection is performed if/>And/>If any centroid movement position does not meet the centroid movement feasible region constraint, judging that the centroid movement locus has a divergence sign, and stopping the walking movement of the robot if the centroid movement feasible region constraint is met, and ending the leader/>, of the dense centroid movement position locus at the time t k And/>And issuing to the robot for execution. As described above, according to the formula (19), the feasibility determination criterion is:
/>
Wherein the method comprises the steps of And/>Is a dense coefficient matrix related to geometric parameters of a centroid motion feasible domain,/>And/>Dense position vectors for center points of centroid motion feasible regions within the prediction interval. When dense centroid position track/>And/>And (3) satisfying the formula (39), judging that the track has no divergence sign, outputting the track leader to the robot for execution, and if the formula (39) is not satisfied, judging that the track diverges and ending the robot walking process.
When the current time is t k+1, updating the input provided by the foothold planning and the centroid state estimation to the centroid track planner, and repeating the process to calculate to obtain the centroid dense position track at the time t k+1 And/>After judging that the method meets the feasibility judgment criterion, issuing the track leader/>And/>To be performed for a bipedal robot. The above process is repeated until the robot stops moving, with progressive progress over time.
The omnidirectional walking centroid movement track planning of the biped robot can be matched with ankle joint track planning (which is the prior art) to obtain the omnidirectional walking whole body movement track of the biped robot through inverse kinematics, so that the on-line walking whole body movement planning of the biped robot is realized.
The examples are preferred embodiments of the present invention, but the present invention is not limited to the above-described embodiments, and any obvious modifications, substitutions or variations that can be made by one skilled in the art without departing from the spirit of the present invention are within the scope of the present invention.
Claims (10)
1. The omnidirectional walking centroid track planning method of the biped robot based on the double-layer model predictive control is characterized by comprising the following steps of:
After receiving the input of the foothold planning and the centroid state estimation, the upper MPC minimizes the upper MPC objective function under the condition of meeting the zero moment point stability constraint and the feasible domain constraint, and obtains the sparse optimal control sequence by means of solution Obtaining the X-direction sparse centroid movement position track/>, in the prediction interval, through cyclic iteration of the state space equationY-direction sparse centroid motion position track/>X-direction sparse centroid movement speed track/>Y-direction sparse centroid movement speed trackX-direction sparse ZMP track/>And Y-direction sparse ZMP track/>
The lower MPC aims at tracking the upper sparse track and solves the sparse track to obtain a dense optimal control sequenceThrough the cyclic iteration of the state space equation, the X-direction dense centroid position track/> isobtainedAnd Y-direction dense centroid position track/>
The centroid track detector judges whether the dense centroid position track diverges according to the feasibility judging criterion, and if the dense centroid position track diverges, the centroid track detector judges the first term value of the dense centroid position trackAnd/>And sending the signals to the bipedal robot for execution.
2. The biped robot omnidirectional walking centroid trajectory planning method of claim 1, wherein the feasible region constraint conditions are:
Wherein the method comprises the steps of And/>Is a sparse coefficient matrix related to geometric parameters of centroid motion feasible domain,/>And/>For X, Y-direction sparse position vector of center point in feasible domain of predicting center of mass movement in interval,/>For X-direction centroid position vector in upper MPC prediction interval,/>And predicting a Y-direction centroid position vector in the interval for the upper layer MPC.
3. The method for planning the omnidirectional walking centroid trajectory of the biped robot according to claim 2, wherein the feasible region is specifically: the motion direction of the mass center of the biped robot is taken as a gesture angle, two parallel lines are respectively made to pass through the center points of the two sole plates, the intersection point of the two parallel lines and the sole plates is taken as the boundary vertex of the mass center motion feasible region, and a closed rectangle is constructed, namely the mass center motion feasible region at the moment.
4. The bipedal robot omnidirectional walking centroid trace programming method of claim 1 wherein said optimal control sequenceThe method comprises the following steps:
Wherein: Is the control input vector of the X direction and the Y direction of the world coordinate system in the upper MPC prediction interval,/> And/>The vectors of the falling foot points in the X direction and the Y direction in the prediction interval output by the upper layer MPC;
From corrected state variables in X-and Y-directions And control input vector/>, of upper layer MPCObtaining X-direction sparse centroid movement locus/>Y-direction sparse centroid motion position track/>X-direction sparse centroid movement speed track/>Y-direction sparse centroid movement velocity track/>X-direction sparse ZMP track/>And Y-direction sparse ZMP track/>
Wherein: upper layer MPC optimal control input sequence Is a coefficient matrix of the upper-layer MPC.
5. The method for planning an omnidirectional walking center of mass trajectory of a bipedal robot of claim 4, wherein said corrected X-and Y-direction state variables areThe method meets the following conditions:
Wherein: Is a state variable in the X direction,/> For the state variable in the Y-direction, alpha is the confidence level for the state estimation of the heart,And/>A centroid state estimation vector is input for centroid state estimation.
6. The biped robot omnidirectional walking centroid trajectory planning method of claim 1, wherein the objective function of the lower MPC is:
Wherein: η is the number of times, Mu, lambda is the weight coefficient,/>A dense optimal control sequence solved for the lower MPC;
Tracking the locus/>, of the sparse centroid position of the upper MPC, in the lower MPC objective function And/>Is defined by:
Is used for tracking the sparse centroid speed track/>, of the upper MPC in the lower MPC objective function And/>Is defined by:
Tracking the upper MPC sparse ZMP track/>, in the lower MPC objective function And/>Is defined by:
Is the part of the underlying MPC objective function that directly minimizes the control input:
Where S is a matrix of mapping coefficients, For X-direction centroid position vector in lower MPC prediction interval,/>For Y-direction centroid position vector within lower MPC prediction interval,/>For X-direction centroid velocity vector in lower MPC prediction interval,/>For Y-direction centroid velocity vector within lower MPC prediction interval,/>For the X-direction ZMP position vector in the lower MPC prediction interval,/>For the Y-direction ZMP position vector in the lower MPC prediction interval,/>Is the control input vector of the X direction and Y direction of the world coordinate system in the prediction interval of the lower MPC.
7. The biped robot omnidirectional walking centroid trajectory planning method of claim 6 wherein said dense optimal control sequence is solved by the following resolution:
wherein: ψ is an analytic coefficient matrix, and ζ x,ξy is a coefficient matrix related to the upper-layer MPC sparse track; and:
Wherein: for the coefficient matrix of the lower MPC,/> Is the corrected state variable in the X-direction and Y-direction.
8. The method for planning omni-directional travel centroid trajectories of bipedal robots according to claim 7, wherein the X-direction dense centroid position trajectoriesAnd Y-direction dense centroid position track/>The method meets the following conditions:
9. The biped robot omnidirectional walking centroid trajectory planning method of claim 8, wherein the feasibility determining criteria is:
Wherein the method comprises the steps of And/>Is a dense coefficient matrix related to geometric parameters of a centroid motion feasible domain,/>And/>Dense position vectors for center points of centroid motion feasible regions within the prediction interval.
10. The method for planning omnidirectional walking centroid trajectories of bipedal robots according to claim 9, wherein when the current time is t k+1, updating the landing point plan and centroid state estimation, and calculating to obtain centroid dense position trajectories at time t k+1 And/>After judging that the track leader meets the feasibility judgment criterion, the track leader/>, and the track leader/>, wherein the track leader/>, and the track leader are the same as the track leaderAnd/>And executing the process for the bipedal robot, and repeating the process until the robot stops moving.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211010867.6A CN115256396B (en) | 2022-08-23 | 2022-08-23 | Double-layer model predictive control-based omnidirectional walking centroid track planning method for bipedal robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211010867.6A CN115256396B (en) | 2022-08-23 | 2022-08-23 | Double-layer model predictive control-based omnidirectional walking centroid track planning method for bipedal robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115256396A CN115256396A (en) | 2022-11-01 |
| CN115256396B true CN115256396B (en) | 2024-05-28 |
Family
ID=83752648
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211010867.6A Active CN115256396B (en) | 2022-08-23 | 2022-08-23 | Double-layer model predictive control-based omnidirectional walking centroid track planning method for bipedal robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115256396B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116224892B (en) * | 2023-05-08 | 2023-07-21 | 之江实验室 | Control method and device of robot, storage medium and electronic equipment |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104793622A (en) * | 2014-01-20 | 2015-07-22 | 丰田自动车株式会社 | Biped walking robot control method and biped walking robot control system |
| CN110371213A (en) * | 2019-07-12 | 2019-10-25 | 沈阳城市学院 | A kind of biped robot's walking planning and control method |
| CN110696793A (en) * | 2019-09-19 | 2020-01-17 | 江苏理工学院 | Intelligent vehicle layered control method combining four-wheel steering and differential braking |
| CN111674486A (en) * | 2020-05-25 | 2020-09-18 | 浙江大学 | Method, device and equipment for controlling stable walking of biped robot and readable medium |
| CN112051741A (en) * | 2020-09-16 | 2020-12-08 | 北京理工大学 | Dynamic motion generation and control method for biped robot |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6450279B2 (en) * | 2015-08-05 | 2019-01-09 | 本田技研工業株式会社 | Device for generating target ZMP trajectory of mobile robot |
| CN105511465B (en) * | 2015-12-02 | 2017-08-04 | 歌尔股份有限公司 | A kind of gait control method and apparatus of biped robot |
-
2022
- 2022-08-23 CN CN202211010867.6A patent/CN115256396B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104793622A (en) * | 2014-01-20 | 2015-07-22 | 丰田自动车株式会社 | Biped walking robot control method and biped walking robot control system |
| CN110371213A (en) * | 2019-07-12 | 2019-10-25 | 沈阳城市学院 | A kind of biped robot's walking planning and control method |
| CN110696793A (en) * | 2019-09-19 | 2020-01-17 | 江苏理工学院 | Intelligent vehicle layered control method combining four-wheel steering and differential braking |
| CN111674486A (en) * | 2020-05-25 | 2020-09-18 | 浙江大学 | Method, device and equipment for controlling stable walking of biped robot and readable medium |
| CN112051741A (en) * | 2020-09-16 | 2020-12-08 | 北京理工大学 | Dynamic motion generation and control method for biped robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115256396A (en) | 2022-11-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Baudouin et al. | Real-time replanning using 3D environment for humanoid robot | |
| EP2233387A1 (en) | Systems and methods for tracking and balancing robots for imitating motion capture data | |
| Luo et al. | Robust bipedal locomotion based on a hierarchical control structure | |
| Kajita et al. | Biped walking pattern generation based on spatially quantized dynamics | |
| Li et al. | Contact force/torque control based on viscoelastic model for stable bipedal walking on indefinite uneven terrain | |
| Yamane et al. | Control-aware mapping of human motion data with stepping for humanoid robots | |
| JP6781101B2 (en) | Non-linear system control method, biped robot control device, biped robot control method and its program | |
| CN115256396B (en) | Double-layer model predictive control-based omnidirectional walking centroid track planning method for bipedal robot | |
| JP2015145049A (en) | robot | |
| CN114248855A (en) | Biped robot space domain gait planning and control method | |
| US20230278214A1 (en) | Robot localization using variance sampling | |
| Xi et al. | Walking control of a biped robot on static and rotating platforms based on hybrid reinforcement learning | |
| CN116619382A (en) | Robot motion control method and system and electronic equipment | |
| Ding et al. | Walking stabilization control for humanoid robots on unknown slope based on walking sequences adjustment | |
| Khadiv et al. | A low friction demanding approach in gait planning for humanoid robots during 3D manoeuvres | |
| Stasse et al. | Integrating walking and vision to increase humanoid autonomy | |
| Kwon et al. | Estimation of the center of mass of humanoid robot | |
| Coelho et al. | Development and implementation of a new approach for posture control of a hexapod robot to walk in irregular terrains | |
| Thompson et al. | Localisation for autonomous humanoid navigation | |
| Raj et al. | Humanoid gait pattern generation with orbital energy | |
| Seekircher et al. | A closed-loop gait for humanoid robots combining LIPM with parameter optimization | |
| Li et al. | A nonlinear state estimation framework for humanoid robots | |
| Ficht et al. | Online balanced motion generation for humanoid robots | |
| Kim et al. | An optimization-based methodology to predict digital human gait motion | |
| Han | Action Planning and Design of Humanoid Robot Based on Sports Analysis in Digital Economy Era |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |