PLANNING AND CONTROL METHOD FOR LEGGED ROBOT, APPARATUS, ROBOT, AND STORAGE MEDIUM

Abstract

Provided are a planning and control method for a legged robot, an apparatus, a robot, and a storage medium. The method includes: determining, based on the movement state and the swing parameter of each leg at the next moment, a foot-end reference state of each leg, and planning, based on a foot-end dynamic model and a discrete collision model, an impact-aware swing leg trajectory for a leg that starts to swing at the next moment; and performing, based on the movement state, the reference state sequence, and the foot-end reference state, whole-body control on the legged robot, and performing, based on a whole-body dynamic model and the discrete collision model, impact-aware whole-body control for a leg that is a supporting leg in planning but does not touch the ground.

Claims

1. A planning and control method for a legged robot, comprising: obtaining a movement instruction and a movement state of the legged robot; generating, based on the movement instruction and the movement state, a reference state sequence of the legged robot and a swing parameter of each leg at a next moment; determining, based on the movement state and the swing parameter of each leg at the next moment, a foot-end reference state of each leg, and planning, based on a foot-end dynamic model and a discrete collision model, an impact-aware swing leg trajectory for a leg that starts to swing at the next moment, to enable an influence caused by an early collision to be within a target constraint range; and performing, based on the movement state, the reference state sequence, and the foot-end reference state, whole-body control on the legged robot, and performing, based on a whole-body dynamic model and the discrete collision model, impact-aware whole-body control for a leg that is a supporting leg in planning but does not touch the ground, to enable an influence caused by a delayed collision to be within the target constraint range.

2. The planning and control method for the legged robot according to claim 1, wherein the foot-end dynamic model is: ${}_{c,l}{\stackrel{.Math.}{x}}_{c,l}+h_{c,l}=F_j,$ where x.sub.c,l is a linear acceleration of a foot end of an l-th leg in an inertial system I, .sub.c,l is a mass matrix in an operation space, h.sub.c,l is a term comprising a Coriolis force, a centripetal force, and a gravitational force in the operation space, and F.sub.j is a foot-end driving force obtained by mapping a torque .sub.j of a driving joint to the operation space; and the discrete collision model is: ${\stackrel{}{x}}_{c,l}=P_n\left({\stackrel{}{x}}_1^{+}-{\stackrel{}{x}}_1^{-}\right)=-\left(1+c_r\right)P_n{\stackrel{}{x}}_{c,l}^{-},$ where P.sub.n=nn.sup.T is a projection operator on a collision normal vector, n is a unit normal vector of a collision surface, ${\stackrel{}{x}}_1^{+},{\stackrel{}{x}}_1^{-}$ are a velocity of a first object before collision and a velocity of the first object after the collision respectively, ${\stackrel{}{x}}_2^{+},{\stackrel{}{x}}_2^{-}$ are a velocity of a second object before collision and a velocity of the second object after the collision respectively, c.sub.r is a restitution coefficient, ${\stackrel{}{x}}_{c,l}^{-}$ is a velocity of the foot-end of the l-th leg before collision in the inertial system, and {dot over (x)}.sub.c,l is a velocity change amount between the velocity of the foot-end of the l-th leg before collision in the inertial system and a velocity of the foot-end of the l-th leg after collision in the inertial system.

3. The planning and control method for the legged robot according to claim 1, wherein said planning, based on the foot-end dynamic model and the discrete collision model, the impact-aware swing leg trajectory comprises: obtaining a pre-defined state variable and a target constraint condition; constructing, based on the foot-end dynamic model, the pre-defined state variable, and the target constraint condition, a continuous-time trajectory optimization problem for the leg that starts to swing at the next moment; and discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for a foot end of a swing leg, and solving the discrete-time trajectory optimization problem to obtain the impact-aware swing leg trajectory.

4. The planning and control method for the legged robot according to claim 3, wherein: the pre-defined state variable comprises one or more of: an initial state of the foot end of the swing leg, a touchdown state of the foot end of the swing leg, a swing duration, and a desired maximum height of the foot end of the swing leg from the ground; and the target constraint condition comprises one or more of: a state equation equality constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equality constraint, a terminal state equality constraint, an impact-aware inequality constraint, and a terminal velocity direction inequality constraint.

5. The planning and control method for the legged robot according to claim 3, wherein: an objective function of the continuous-time trajectory optimization problem is: ${\min }_{x\left(t\right),u\left(t\right)}{f}_{TO}={.Math.x\left(t_F\right)-\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right].Math.}_{Q_{x_F}}+{}_{t_0}^{t_F}{.Math.u\left(\right).Math.}_{Q_u}d+{}_{t_{h_1}}^{t_{h_2}}{.Math.z_c\left(\right)-z_h^{\mathrm{ref}}.Math.}_{Q_z}d,$ where x(t) is a state vector about time t, u(t) is a control vector about time t, Q.sub.x.sub.F, Q.sub.u, and Q.sub.z are weight matrices (mostly diagonal matrices) with different dimensions, a.sub.Q=a.sup.TQa represents a weighted norm of a vector a under a weight matrix Q, ${.Math.x\left(t_F\right)-\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right].Math.}_{Q_{x_F}}$ reflects a task of a touchdown state of the foot end of the swing leg, $x_{c,F}^{\mathrm{ref}}\mathrm{and}{\stackrel{}{x}}_{c,F}^{\mathrm{ref}}$ are a known terminal position of the foot end of the swing leg and a known linear velocity of the foot end of the swing leg respectively, ${}_{t_0}^{t_F}{.Math.u\left(\right).Math.}_{Q_u}d$ is a minimum control variable (i.e., a linear acceleration) in an entire swing process, and t.sub.0=0, t.sub.F=T.sub.sw, where T.sub.sw is a swing duration, ${}_{t_{h_1}}^{t_{h_2}}{.Math.z_c\left(\right)-z_h^{\mathrm{ref}}.Math.}_{Q_z}d$ reflects a task of a maximum height of the swing leg from the ground, t.sub.h1 and t.sub.h2 are parameters specifying a time period to reach the maximum height, satisfying t.sub.0t.sub.h1th.sub.2t.sub.F, and z.sub.c is a third component of the state variable, i.e., a component representing a height of the foot end from the ground in the state variable; and the discrete-time trajectory optimization problem is: $\underset{U}{\min }{f}_{c,\mathrm{TO}}={.Math.x_N-x_N^{\mathrm{ref}}.Math.}_{Q_{x_N}}+\underset{k=0}{\overset{N-1}{.Math.}}{.Math.u_k.Math.}_{Q_{u_K}}+\underset{k=k_{h_1}}{\overset{k_{h_2}}{.Math.}}{.Math.S_z{x}_k-z_h^{\mathrm{ref}}.Math.}_{Q_{z_k}},$ $s.t.\underset{\underset{x_{k+1}}{}}{\left[\begin{array}{c}{x}_{c,k+1}\\ {\stackrel{.}{x}}_{c,k+1}\end{array}\right]}=\underset{\underset{A}{}}{\left[\begin{array}{cc}{I}_{33}& {\mathrm{tI}}_{33}\\ 0_{33}& I_{33}\end{array}\right]}\underset{\underset{x_k}{}}{\left[\begin{array}{c}{x}_{c,k}\\ {\stackrel{.}{x}}_{c,k}\end{array}\right]}+\underset{\underset{B}{}}{\left[\begin{array}{c}\frac{1}{2}{t}^2{I}_{33}\\ {\mathrm{tI}}_{33}\end{array}\right]}\underset{\underset{u_k}{}}{{\stackrel{.Math.}{x}}_{c,k}},k=0,1,.Math.,N-1,$ $f_{\min }{}_c{u}_k+h_c{f}_{\max },k=0,1,.Math.,N-1,$ $z_{c,\min }{S}_z{x}_k{z}_{c,\max },k=1,2,.Math.,N-1,$ $x_0=x_0^{\mathrm{fb}},$ $S_{z,\stackrel{.}{z}}{x}_N=S_{z,\stackrel{.}{z}}{x}_N^{\mathrm{ref}},$ ${\mathrm{.Math.}}_{c,\min }{}_c\left[-\left(1+c_r\right)P_n{S}_{}{x}_k\right]{\mathrm{.Math.}}_{c,\max },k=k_{\mathrm{imp}},k_{\mathrm{imp}}+1,.Math.,N,$ $\left(C_{{}_1}+\frac{1}{\cos {}_{c,\max }}{D}_n\right)S_v{\stackrel{.}{x}}_k{0}_{41},k=k_{\mathrm{imp}},k_{\mathrm{imp}}+1,.Math.,N,$ where N represents a total number of frames in the discrete-time trajectory optimization problem, S.sub.z is a selection matrix used for selecting a position component of a state vector in a Z-axis direction, A is a state matrix in a state equation, B is an input matrix in the state equation, u.sub.k is a control variable of a k-th frame, .sub.min and .sub.max are an artificially specified lower limit foot-end driving force and an artificially specified upper limit foot-end driving force in an operation space respectively, z.sub.c,min and z.sub.c,max are a minimum height of the swing leg from the ground and the maximum height of the swing leg from the ground respectively, S.sub.z, is a selection matrix used for selecting the position component and a velocity component of the state vector in the Z-axis direction, l.sub.c,min and l.sub.c,max are an artificially specified lower limit of an allowable collision impulse and an artificially specified upper limit of the allowable collision impulse respectively, .sub.c,max is an artificially specified maximum allowable angle between a velocity vector before touchdown of the foot end of the swing leg and a unit normal vector of a collision surface, is a terminal velocity direction constraint matrix, D.sub.n is a matrix formed by the unit normal vector of the collision surface, .sub.c,TO is an objective function of the discrete-time trajectory optimization problem, x.sub.N is a state vector of an N-th frame, $Z_h^{\mathrm{ref}}$ is a reference value of the maximum height of the swing leg from the ground, x.sub.c,k+1 is a foot-end position state of a (k+1)-th frame, {dot over (x)}.sub.c,k+1 is a foot-end velocity state of the (k+1)-th frame, t is a time interval between adjacent frames, .sub.c is a mass matrix in the operation space, h.sub.c is a term comprising a Coriolis force, a centripetal force, and a gravitational force in the operation space, x.sub.k is a state vector of the k-th frame, $U={\left[u_0^T,u_1^T,.Math.,u_{N-1}^T\right]}^T$ is a vector composed of a control variable of each frame, Q.sub.x.sub.N, Q.sub.u.sub.k, and Q.sub.z.sub.k are diagonal weight matrices with different dimensions, $x_0^{\mathrm{fb}}=\left[\begin{array}{c}{x}_{c,0}^{\mathrm{fb}}\\ {\stackrel{}{x}}_{c,0}^{\mathrm{fb}}\end{array}\right]$ is a feedback of a foot-end state (comprising a position and a velocity) at a current moment, $x_N^{\mathrm{ref}}=\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right]$ is a reference terminal foot-end state (comprising the position and the velocity), S.sub.=[0.sub.33I.sub.33] is a selection matrix of a foot-end linear velocity, k.sub.h.sub.1 and k.sub.h.sub.2 are an artificially specified start frame and an artificially specified end frame for maintaining a swing height, satisfying 0k.sub.h.sub.1k.sub.h.sub.2<N, and k.sub.imp is a start frame for enabling an impact-aware constraint and a terminal velocity direction constraint, satisfying 0k.sub.impN.

6. The planning and control method for the legged robot according to claim 1, wherein an optimization problem of the whole-body control is: ${\min }_{}{.Math.}_{i=1}^{n_{\mathrm{task}}}{.Math.W_i\left(A_i-b_i\right).Math.}_2^2,$ $s.t.{\mathrm{lb}}_j{C}_j{\mathrm{ub}}_j,j=1,2,.Math.,n_{\mathrm{constraint}},$ where is an optimization variable of the optimization problem of the whole-body control, A.sub.i is a task matrix, b.sub.i is a task vector, c.sub.j is a constraint matrix, lb.sub.j and ub.sub.j are a lower constraint bound and an upper constraint bound, W.sub.i is a weight matrix, n.sub.task is a quantity of tasks, and n.sub.constraint is a quantity of constraints.

7. The planning and control method for the legged robot according to claim 1, wherein tasks processed simultaneously by the whole-body control comprise two or more of: a trunk trajectory tracking task, a foot-end trajectory tracking task, a foot-sole force tracking task, a task of minimizing variation of a joint torque, and a task of minimizing variation of a foot-sole force; and constraints processed simultaneously by the impact-aware whole-body control comprise two or more of: a floating-base dynamic equality constraint, a foot-sole force inequality constraint, a joint output torque saturation inequality constraint, a joint rotational speed saturation inequality constraint, a joint output power saturation inequality constraint, and an impact-aware constraint.

8. The planning and control method for the legged robot according to claim 7, wherein said performing, based on the movement state, the reference state sequence, and the foot-end reference state, whole-body control on the legged robot comprises: if a leg is a swing leg in planning, setting the foot-end trajectory tracking task as tracking the impact-aware swing leg trajectory, setting a target foot-sole force of the foot-sole force tracking task as a predetermined value, setting a foot-sole force constraint in the foot-sole force inequality constraint as a predetermined value, and disabling the impact-aware constraint; if a leg is the supporting leg in planning and touches the ground, setting a target linear acceleration of the foot-end trajectory tracking task as a predetermined value, setting the target foot-sole force of the foot-sole force tracking task as a foot-sole force provided by MPC or another module, setting the foot-sole force constraint as a friction cone constraint, and disabling the impact-aware constraint; and if a leg is a supporting leg in planning but does not touch the ground, setting a target of the foot-end trajectory tracking task as tracking a velocity pointing to a collision surface, setting the target foot-sole force of the foot-sole force tracking task as a predetermined value, setting the foot-sole force constraint as a predetermined value, and enabling the impact-aware constraint.

9. The planning and control method for the legged robot according to claim 1, wherein: the whole-body dynamic model is: $M\left(q_g\right)\stackrel{.Math.}{q}+h\left(q_g,\stackrel{}{q}\right)=\left[\begin{array}{c}{0}_{61}\\ {}_j\end{array}\right]+J_c^T\left(q_g\right)F_c,$ where M(q.sub.g) is a generalized mass matrix, h(q.sub.g,{dot over (q)}) is a term comprising a Coriolis force, a centripetal force, and a gravitational force, j represents an output torque of a driving joint, 0.sub.nm represents a zero matrix with a size of nm, $J_c^T\left(q_g\right)$ and F.sub.c are an augmented Jacobian matrix formed by stacking a contact Jacobian matrix of each supporting leg and an augmented foot-sole force formed by stacking a ground reaction force on a foot sole of each supporting leg respectively, q.sub.g is a generalized joint space position, {dot over (q)} is a generalized joint space velocity, {umlaut over (q)} is a generalized joint space acceleration, and n.sub.q is a dimension size of the generalized joint space acceleration {umlaut over (q)}; and expressions for a matrix and a vector of the impact-aware constraint are: $\begin{array}{c}{C}_{6,l}=\left[-t\left(1+c_r\right){}_{c,l}\left(q_g^{\mathrm{fb}}\right)P_n{J}_{c,l}\left(q_g^{\mathrm{fb}}\right)0_{3n_F}\right]\\ {\mathrm{lb}}_6={\mathrm{.Math.}}_{c,l,\min }+\left(1+c_r\right){}_{c,l}\left(q_g^{\mathrm{fb}}\right)P_n\left(J_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{.}{q}}^{\mathrm{fb}}+t{\stackrel{.}{J}}_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{.}{q}}^{\mathrm{fb}}\right)\\ {\mathrm{ub}}_6={\mathrm{.Math.}}_{c,l,\max }+\left(1+c_r\right){}_{c,l}\left(q_g^{\mathrm{fb}}\right)P_n\left(J_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{.}{q}}^{\mathrm{fb}}+t{\stackrel{.}{J}}_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{.}{q}}^{\mathrm{fb}}\right),\end{array}$ where l.sub.c,l,min, l.sub.c,l,max are an artificially specified lower limit of an allowable collision impulse and an artificially specified upper limit of the allowable collision impulse respectively, t represents a time interval between a current moment and the next moment, C.sub.6,l the matrix of the impact-aware constraint, lb.sub.6 is a lower limit vector of the impact-aware constraint, ub.sub.6 is an upper limit vector of the impact-aware constraint, n.sub. is a dimension size of the optimization variable, a variable marked with fb in its top right corner represents that the variable is a feedback variable, J.sub.c,l represent the contact Jacobian matrix of an l-th leg, {dot over (J)}.sub.c,l is a derivative of the contact Jacobian matrix of the l-th leg with respect to time.

10. A legged robot, comprising: a memory; a processor; and a computer program stored in the memory and executable on the processor, wherein the processor is configured to execute the program to implement a planning and control method for a legged robot, the method comprising: obtaining a movement instruction and a movement state of the legged robot; generating, based on the movement instruction and the movement state, a reference state sequence of the legged robot and a swing parameter of each leg at a next moment; determining, based on the movement state and the swing parameter of each leg at the next moment, a foot-end reference state of each leg, and planning, based on a foot-end dynamic model and a discrete collision model, an impact-aware swing leg trajectory for a leg that starts to swing at the next moment, to enable an influence caused by an early collision to be within a target constraint range; and performing, based on the movement state, the reference state sequence, and the foot-end reference state, whole-body control on the legged robot, and performing, based on a whole-body dynamic model and the discrete collision model, impact-aware whole-body control for a leg that is a supporting leg in planning but does not touch the ground, to enable an influence caused by a delayed collision to be within the target constraint range.

11. The legged robot according to claim 10, wherein the foot-end dynamic model is: $A_{c,l}{\stackrel{.Math.}{x}}_{c,l}+h_{c,l}=F_j,$ where {umlaut over (x)}.sub.c,l is a linear acceleration of a foot end of an l-th leg in an inertial system I, .sub.c,l is a mass matrix in an operation space, h.sub.c,l is a term comprising a Coriolis force, a centripetal force, and a gravitational force in the operation space, and F.sub.j is a foot-end driving force obtained by mapping a torque .sub.j of a driving joint to the operation space; and the discrete collision model is: ${\stackrel{.}{x}}_{c,l}=P_n\left({\stackrel{.}{x}}_1^{+}-{\stackrel{.}{x}}_1^{-}\right)=-\left(1+c_r\right)P_n{\stackrel{.}{x}}_{c,l}^{-},$ where P.sub.n=nn.sup.T is a projection operator on a collision normal vector, n is a unit normal vector of a collision surface, ${\stackrel{}{x}}_1^{+},{\stackrel{}{x}}_1^{-}$ are a velocity of a first object before collision and a velocity of the first object after the collision respectively, ${\stackrel{.}{x}}_2^{+},{\stackrel{.}{x}}_2^{-}$ are a velocity of a second object before collision and a velocity of the second object after the collision respectively, c.sub.r is a restitution coefficient, ${\stackrel{.}{x}}_{c,l}^{-}$ is a velocity of the foot-end of the l-th leg before collision in the inertial system, and {dot over (x)}.sub.c,l is a velocity change amount between the velocity of the foot-end of the l-th leg before collision in the inertial system and a velocity of the foot-end of the l-th leg after collision in the inertial system.

12. The legged robot according to claim 10, wherein said planning, based on the foot-end dynamic model and the discrete collision model, the impact-aware swing leg trajectory comprises: obtaining a pre-defined state variable and a target constraint condition; constructing, based on the foot-end dynamic model, the pre-defined state variable, and the target constraint condition, a continuous-time trajectory optimization problem for the leg that starts to swing at the next moment; and discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for a foot end of a swing leg, and solving the discrete-time trajectory optimization problem to obtain the impact-aware swing leg trajectory.

13. The legged robot according to claim 12, wherein: the pre-defined state variable comprises one or more of: an initial state of the foot end of the swing leg, a touchdown state of the foot end of the swing leg, a swing duration, and a desired maximum height of the foot end of the swing leg from the ground; and the target constraint condition comprises one or more of: a state equation equality constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equality constraint, a terminal state equality constraint, an impact-aware inequality constraint, and a terminal velocity direction inequality constraint.

14. The legged robot according to claim 12, wherein: an objective function of the continuous-time trajectory optimization problem is: ${\min }_{x\left(t\right),u\left(t\right)}{f}_{TO}={.Math.x\left(t_F\right)-\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right].Math.}_{Q_{x_F}}+{}_{t_0}^{t_F}{.Math.u\left(\right).Math.}_{Q_u}d+{}_{t_{h_1}}^{t_{h_2}}{.Math.z_c\left(\right)-z_h^{\mathrm{ref}}.Math.}_{Q_z}d,$ where x(t) is a state vector about time t, u(t) is a control vector about time t, Q.sub.x.sub.F, Q.sub.u, and Q.sub.z are weight matrices (mostly diagonal matrices) with different dimensions, a.sub.Q=a.sup.TQa represents a weighted norm of a vector a under a weight matrix Q, $Q,{.Math.x\left(t_F\right)-\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right].Math.}_{Q_{x_F}}$ reflects a task of a touchdown state of the foot end of the swing leg, $x_{c,F}^{\mathrm{ref}}\mathrm{and}{\stackrel{}{x}}_{c,F}^{\mathrm{ref}}$ are a known terminal position of the foot end of the swing leg and a known linear velocity of the foot end of the swing leg respectively, ${}_{t_0}^{t_F}{.Math.u\left(\right).Math.}_{Q_u}d$ is a minimum control variable (i.e., a linear acceleration) in an entire swing process, and t.sub.0=0, t.sub.F=T.sub.sw, where T.sub.sw is a swing duration, ${}_{t_{h_1}}^{t_{h_2}}{.Math.z_c\left(\right)-z_h^{\mathrm{ref}}.Math.}_{Q_z}d$ reflects a task of a maximum height of the swing leg from the ground, t.sub.h1 and t.sub.h2 are parameters specifying a time period to reach the maximum height, satisfying t.sub.0t.sub.h1t.sub.h2t.sub.F, and z.sub.c is a third component of the state variable, i.e., a component representing a height of the foot end from the ground in the state variable; and the discrete-time trajectory optimization problem is: $\underset{U}{\min }{f}_{c,TO}={.Math.x_N-x_N^{ref}.Math.}_{Q_{x_N}}+\underset{k=0}{\overset{N-1}{.Math.}}{.Math.u_k.Math.}_{Q_{u_k}}+\underset{k=k_{h_1}}{\overset{k_{h_2}}{.Math.}}{.Math.S_z{x}_k-z_h^{\mathrm{ref}}.Math.}_{Q_{z_k}},s.t.\underset{x_{k+1}}{\underset{}{\left[\begin{array}{c}{x}_{c,k+1}\\ {\stackrel{.}{x}}_{c,k+1}\end{array}\right]}}=\underset{A}{\underset{}{\left[\begin{array}{cc}{I}_{33}& {\mathrm{tI}}_{33}\\ 0_{33}& I_{33}\end{array}\right]}}\underset{x_k}{\underset{}{\left[\begin{array}{c}{x}_{c,k}\\ {\stackrel{.}{x}}_{c,k}\end{array}\right]}}+\underset{B}{\underset{}{\left[\begin{array}{c}\frac{1}{2}{t}^2{I}_{33}\\ {\mathrm{tI}}_{33}\end{array}\right]}}\underset{u_k}{\underset{}{{\stackrel{.Math.}{x}}_{c,k}}},k=0,1,.Math.,N-1,$ $$\begin{gathered} {f}_{\min }{}_c{u}_k+h_c{f}_{\max },k=0,1,.Math.,N-1, \\ {z}_{c,\min }{S}_z{x}_k{z}_{c,\max },k=1,2,.Math.,N-1, \\ {x}_0=x_0^{\mathrm{fb}}, \\ {S}_{z,\stackrel{.}{z}}{x_N}_{{=}^{}}{S}_{z,\stackrel{.}{z}}{x}_N^{\mathrm{ref}}, \end{gathered}$$ ${\mathrm{.Math.}}_{c,\min }{A}_c\left[-\left(1+c_r\right)P_n{S}_v{x}_k\right]{\mathrm{.Math.}}_{c,\max },k=k_{\mathrm{imp}},k_{\mathrm{imp}}+1,.Math.,N,\left(C_{{}_1}+\frac{1}{\cos {}_{c,\max }}{D}_n\right)S_v{\stackrel{}{x}}_k{0}_{41},k=k_{\mathrm{imp}},k_{\mathrm{imp}}+1,.Math.,N,$ Where N represents a total number of frames in the discrete-time trajectory optimization problem, S.sub.z is a selection matrix used for selecting a position component of a state vector in a Z-axis direction, A is a state matrix in a state equation, B is an input matrix in the state equation, u.sub.k is a control variable of a k-th frame, .sub.min and .sub.max are an artificially specified lower limit foot-end driving force and an artificially specified upper limit foot-end driving force in an operation space respectively, z.sub.c,min and z.sub.c,max are a minimum height of the swing leg from the ground and the maximum height of the swing leg from the ground respectively, S.sub.z, is a selection matrix used for selecting the position component and a velocity component of the state vector in the Z-axis direction, l.sub.c,min and l.sub.c,max are an artificially specified lower limit of an allowable collision impulse and an artificially specified upper limit of the allowable collision impulse respectively, .sub.c,max is an artificially specified maximum allowable angle between a velocity vector before touchdown of the foot end of the swing leg and a unit normal vector of a collision surface, is a terminal velocity direction constraint matrix, D.sub.n is a matrix formed by the unit normal vector of the collision surface, .sub.c,T0 is an objective function of the discrete-time trajectory optimization problem, x.sub.N is a state vector of an N-th frame, $z_h^{\mathrm{ref}}$ is a reference value of the maximum height of the swing leg from the ground, x.sub.c,k+1 is a foot-end position state of a (k+1)-th frame, {dot over (x)}.sub.c,k+1 is a foot-end velocity state of the (k+1)-th frame, t is a time interval between adjacent frames, .sub.c is a mass matrix in the operation space, h.sub.c is a term comprising a Coriolis force, a centripetal force, and a gravitational force in the operation space, x.sub.k is a state vector of the k-th frame, $U={\left[u_0^T,u_1^T,.Math.,u_{N-1}^T\right]}^T$ is a vector composed of a control variable of each frame, Q.sub.x.sub.N, Q.sub.u.sub.k, and Q.sub.z.sub.k are diagonal weight matrices with different dimensions, $x_0^{\mathrm{fb}}=\left[\begin{array}{c}{x}_{c,0}^{\mathrm{fb}}\\ {\stackrel{}{x}}_{c,0}^{\mathrm{fb}}\end{array}\right]$ is a feedback of a foot-end state (comprising a position and a velocity) at a current moment, $x_N^{\mathrm{ref}}=\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right]$ is a reference terminal foot-end state (comprising the position and the velocity), S.sub.=[0.sub.33I.sub.33] is a selection matrix of a foot-end linear velocity, k.sub.h.sub.1 and k.sub.h.sub.2 are an artificially specified start frame and an artificially specified end frame for maintaining a swing height, satisfying 0<k.sub.h.sub.1k.sub.h.sub.2<N, and k.sub.imp is a start frame for enabling an impact-aware constraint and a terminal velocity direction constraint, satisfying 0k.sub.impN.

15. The legged robot according to claim 10, wherein an optimization problem of the whole-body control is: ${\min }_{}{.Math.}_{i=1}^{n_{\mathrm{task}}}{.Math.W_i\left(A_i-b_i\right).Math.}_2^2,$ $s.t.{\mathrm{lb}}_j{C}_j{\mathrm{ub}}_j,j=1,2,.Math.,n_{\mathrm{constraint}},$ where is an optimization variable of the optimization problem of the whole-body control, A.sub.i is a task matrix, b.sub.i is a task vector, c.sub.j is a constraint matrix, lb.sub.j and ub.sub.j are a lower constraint bound and an upper constraint bound, W.sub.i is a weight matrix, n.sub.task is a quantity of tasks, and n.sub.constraint is a quantity of constraints.

16. The legged robot according to claim 10, wherein tasks processed simultaneously by the whole-body control comprise two or more of a trunk trajectory tracking task, a foot-end trajectory tracking task, a foot-sole force tracking task, a task of minimizing variation of a joint torque, and a task of minimizing variation of a foot-sole force; and constraints processed simultaneously by the impact-aware whole-body control comprise two or more of: a floating-base dynamic equality constraint, a foot-sole force inequality constraint, a joint output torque saturation inequality constraint, a joint rotational speed saturation inequality constraint, a joint output power saturation inequality constraint, and an impact-aware constraint.

17. The legged robot according to claim 16, wherein said performing, based on the movement state, the reference state sequence, and the foot-end reference state, whole-body control on the legged robot comprises: if a leg is a swing leg in planning, setting the foot-end trajectory tracking task as tracking the impact-aware swing leg trajectory, setting a target foot-sole force of the foot-sole force tracking task as a predetermined value, setting a foot-sole force constraint in the foot-sole force inequality constraint as a predetermined value, and disabling the impact-aware constraint; if a leg is the supporting leg in planning and touches the ground, setting a target linear acceleration of the foot-end trajectory tracking task as a predetermined value, setting the target foot-sole force of the foot-sole force tracking task as a foot-sole force provided by MPC or another module, setting the foot-sole force constraint as a friction cone constraint, and disabling the impact-aware constraint; and if a leg is a supporting leg in planning but does not touch the ground, setting a target of the foot-end trajectory tracking task as tracking a velocity pointing to a collision surface, setting the target foot-sole force of the foot-sole force tracking task as a predetermined value, setting the foot-sole force constraint as a predetermined value, and enabling the impact-aware constraint.

18. The legged robot according to claim 10, wherein: the whole-body dynamic model is: $M\left(q_g\right)\stackrel{.Math.}{q}+h\left(q_g,\stackrel{}{q}\right)=\left[\begin{array}{c}{0}_{61}\\ {}_j\end{array}\right]+J_c^T\left(q_g\right)F_c,$ where M(q.sub.g) is a generalized mass matrix, h(q.sub.g,{dot over (q)}) is a term comprising a Coriolis force, a centripetal force, and a gravitational force, j represents an output torque of a driving joint, 0.sub.nm represents a zero matrix with a size of nm, $J_c^T\left(q_g\right)$ and F.sub.c are an augmented Jacobian matrix formed by stacking a contact Jacobian matrix of each supporting leg and an augmented foot-sole force formed by stacking a ground reaction force on a foot sole of each supporting leg respectively, q.sub.g is a generalized joint space position, {dot over (q)} is a generalized joint space velocity, {umlaut over (q)} is a generalized joint space acceleration, and n.sub.q is a dimension size of the generalized joint space acceleration {umlaut over (q)}; and expressions for a matrix and a vector of the impact-aware constraint are: $C_{6,l}=\left[-\left(1+c_r\right){}_{c,l}\left(q_g^{\mathrm{fb}}\right)P_n{J}_{c,l}\left(q_g^{\mathrm{fb}}\right)0_{3n_F}\right],$ ${\mathrm{lb}}_6={\mathrm{.Math.}}_{c,l,\min }+\left(1+c_r\right){}_{c,l}\left(q_g^{\mathrm{fb}}\right)P_n\left(J_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{}{q}}^{\mathrm{fb}}+t{\stackrel{.}{J}}_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{}{q}}^{\mathrm{fb}}\right),$ ${\mathrm{ub}}_6={\mathrm{.Math.}}_{c,l,\max }+\left(1+c_r\right){}_{c,l}\left(q_g^{\mathrm{fb}}\right)P_n\left(J_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{}{q}}^{\mathrm{fb}}+t{\stackrel{.}{J}}_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{}{q}}^{\mathrm{fb}}\right),$ where l.sub.c,l,min, l.sub.c,l,max are an artificially specified lower limit of an allowable collision impulse and an artificially specified upper limit of the allowable collision impulse respectively, t represents a time interval between a current moment and the next moment, C.sub.6,l the matrix of the impact-aware constraint, lb.sub.6 is a lower limit vector of the impact-aware constraint, ub.sub.6 is an upper limit vector of the impact-aware constraint, n, is a dimension size of the optimization variable, a variable marked with fb in its top right corner represents that the variable is a feedback variable, J.sub.c,l represent the contact Jacobian matrix of an l-th leg, {dot over (J)}.sub.c,l is a derivative of the contact Jacobian matrix of the l-th leg with respect to time.

19. A non-transitory computer-readable storage medium, having a computer program stored thereon, wherein the program is configured to, when executed by a processor, implement a planning and control method for a legged robot, the method comprising: obtaining a movement instruction and a movement state of the legged robot; generating, based on the movement instruction and the movement state, a reference state sequence of the legged robot and a swing parameter of each leg at a next moment; determining, based on the movement state and the swing parameter of each leg at the next moment, a foot-end reference state of each leg, and planning, based on a foot-end dynamic model and a discrete collision model, an impact-aware swing leg trajectory for a leg that starts to swing at the next moment, to enable an influence caused by an early collision to be within a target constraint range; and performing, based on the movement state, the reference state sequence, and the foot-end reference state, whole-body control on the legged robot, and performing, based on a whole-body dynamic model and the discrete collision model, impact-aware whole-body control for a leg that is a supporting leg in planning but does not touch the ground, to enable an influence caused by a delayed collision to be within the target constraint range.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The above and/or additional aspects and advantages of the present disclosure will become more apparent and more understandable from the following description of embodiments taken in conjunction with the accompanying drawings.

[0030] FIG. 1 is a flowchart illustrating a planning and control method for a legged robot according to an embodiment of the present disclosure.

[0031] FIG. 2 is a schematic diagram of a gait control system for an impact-aware quadruped robot according to an embodiment of the present disclosure.

[0032] FIG. 3 is a schematic diagram of a decision tree for impact-aware whole-body control according to an embodiment of the present disclosure.

[0033] FIG. 4 is an exemplary diagram of a planning and control apparatus for a legged robot according to an embodiment of the present disclosure.

[0034] FIG. 5 is a schematic structural diagram of a legged robot according to an embodiment of the present disclosure.

DETAILED DESCRIPTION

[0035] Embodiments of the present disclosure will be described in detail below with reference to examples thereof as illustrated in the accompanying drawings, throughout which same or similar elements, or elements having same or similar functions, are denoted by same or similar reference numerals. The embodiments described below with reference to the drawings are illustrative only, and are intended to explain, rather than limiting, the present disclosure.

[0036] In recent years, the field of a legged robot has developed rapidly. An increasing number of mature and stable methods are applied to movement planning and control for the legged robot. The legged robot from many research institutions is no longer confined to static walking but is evolving towards faster, more agile, more robust, safer, and more multifunctional robots. When the legged robot performs a highly dynamic movement, some problems that may be ignored under low-dynamic movement conditions become non-negligible, such as a collision problem between the foot sole and the ground when the legged robot runs at a high speed. Movement of the legged robot relies on continuous switching of a supporting leg. Each step of walking requires contact between the foot sole and the ground, which is an intentional collision with an environment and is also a common collision problem when the legged robot interacts with the environment. Improper control strategies may cause a violent collision between the foot sole and the ground, affecting a subsequent movement state of the robot, causing the robot to lose balance and fall, and even potentially resulting in a reduced hardware service life or hardware damage.

[0037] In the related art, solutions to the collision problem for the legged robot can be provided from two aspects: hardware and a control algorithm.

[0038] In terms of the hardware, a foot pad with a certain degree of flexibility may be mount at a foot sole of the robot. Alternatively, a buffer device such as a spring-damper may be mounted at a driving joint of the robot. Alternatively, a VIA or a VSA may be adopted. The VIA and the VSA can provide a desired passive mechanical impedance, achieving an effect of changing a joint impedance at a mechanical level, allowing a buffering effect to be adjusted as desired. A practice of mounting the buffer devices for the robot is commonly referred to as a passive compliance method.

[0039] In terms of the control algorithm, to minimize a negative influence of a collision between the foot sole and the ground on the robot's state during walking of the legged robot, a contact velocity between the foot sole and the ground can be planned to approach zero in movement planning. When a small or lightweight legged robot walks slowly on a flat terrain, performance of a controller may usually ensure that an actual movement trajectory of a swing leg is substantially consistent with a planned trajectory. Therefore, this solution works well. When a terrain is uneven, a visual sensor such as a camera or a laser radar may be used to estimate the terrain. A foot movement trajectory, where the contact velocity between the foot sole and the ground approaches zero, may be planned based on the estimated terrain, avoiding a collision impact caused by premature ground contact or delayed ground contact of the foot sole.

[0040] However, the methods in the related art have certain limitations.

[0041] In terms of the hardware, the passive compliance method can reduce an effect of the collision to some extent, which can also bring some problems. Firstly, introduction of these passive flexible materials may affect a response frequency of a robot system. If the control algorithm does not take this effect into account, unstable vibrations during movement of the robot may easily occur. On the other hand, a buffering effect of the passive compliance method is limited. When a material of the buffer device is determined, stiffness, damping, and inertia characteristics of the material are also determined, and thus the buffering effect cannot be adjusted as desired. When using the VIA method or the VSA method, two motors may be used. One motor is configured to independently control mechanical stiffness of a joint, while another motor is configured to generate a torque, which may result in an overly complex overall structure with a large volume and a large weight, making it difficult to apply such actuators to the legged robot that needs to perform the highly dynamic movement.

[0042] In terms of the control algorithm, when the legged robot performs a highly dynamic movement such as a high-speed running, there may be a significant error between the actual movement trajectory of the swing leg and the planned trajectory of the swing leg, causing the premature ground contact of the foot sole or the delayed ground contact of the foot sole. In an exemplary embodiment, in the case of the delayed ground contact, when the foot sole has not yet landed but is required by the planning to support a torso, this leg may rapidly step onto the ground, causing a violent collision and generating an enormous foot-sole impact force at an instant of the collision. For a heavy-duty legged robot, an impact force caused by such collisions is often even greater. When the legged robot runs at the high speed, a violent impact may cause zero drift in the joint, and a long-term frequent collision of the foot-sole may accelerate hardware aging.

[0043] In summary, control strategies in the related art consider the impact force during the collision as a disturbance, focusing an emphasis on balance control of the robot after the collision, while neglecting an effect of such frequent, intentional collisions on a hardware service life of the robot.

[0044] For the problems mentioned in the above background technology, the present disclosure provides a planning and control method for a legged robot. The planning and control method for the legged robot, the planning and control apparatus for the legged robot, the robot, and the storage medium according to the embodiments of the present disclosure are described below with reference to the accompanying drawings.

[0045] In an exemplary embodiment of the present disclosure, FIG. 1 is a schematic flowchart illustrating a planning and control method for a legged robot according to an embodiment of the present disclosure. As illustrated in FIG. 1, the planning and control method for the legged robot includes operations at blocks.

[0046] At block S101, a movement instruction and a movement state of the legged robot are obtained.

[0047] The movement instruction may include a movement velocity instruction and a movement direction instruction inputted by a user, etc. The movement state may include a robot trunk state, a foot-end state of each leg, and a touchdown state of each leg, etc.

[0048] It should be understood that embodiments of the present disclosure may obtain the movement instruction and the movement state of the legged robot in at least one way. An actuator of the legged robot in the embodiments of the present disclosure is a force-controllable motor, which is equipped with a sensor including an Inertial Measurement Unit (IMU), a motor angle encoder, a motor torque sensor, etc. The following embodiments are elaborated based on this.

[0049] For example, FIG. 2 is a block diagram of a gait control system for an impact-aware quadruped robot according to an embodiment of the present disclosure. The block diagram is applicable to both simulation and actual robots. An entire control framework mainly consists of 5 modules: a State Estimator module, a Rough Planning module, a Single Rigid Body Model Predictive Control (SRB-MPC) module, an Impact-aware Trajectory Optimization (IA-TO) module, and an Impact-aware Whole-body Control (IA-WBC) module. This system operates at a high frequency (e.g., 500 Hz, 1 kHz, etc). The IA-WBC operates at a same frequency. The SRB-MPC operates at around 40 Hz. An optimization problem in the IA-TO is calculated only once each time a leg needs to start swinging. In the following embodiments, this system as an example is described.

[0050] In an exemplary embodiment of the present disclosure, as illustrated in FIG. 2, the embodiments of the present disclosure may estimate a trunk state of the legged robot based on read IMU data and motor state information. The IMU data may include a three-dimensional Euler angle, a three-dimensional angular velocity, and a three-dimensional linear acceleration, etc. The motor state information may include a motor rotation angle, a rotational speed, and a feedback torque, etc. The trunk state of the legged robot may include a point position of a trunk, an attitude unit quaternion, a linear velocity, and an angular velocity, etc. The foot-end state of each leg may include the point position and the linear velocity. The embodiments of the present disclosure may use an Extended Kalman Filter (EKF) to estimate the trunk state. The present disclosure is not limited in this regard.

[0051] In the embodiments of the present disclosure, state estimation transfers corresponding information according to different needs. As illustrated in FIG. 2, for the SRB-MPC, only the trunk state is transferred. For the IA-TO, the trunk state, the foot-end state of each leg, and the joint state are transferred. For the IA-WBC, the trunk state, the foot-end state of each leg, the joint state, and a touchdown state of each leg are transferred. For the Rough Planning module, the trunk state and the foot-end state of each leg may be transferred. These transferring processes may be described in detail in the following embodiments, and thus details thereof will be omitted here.

[0052] At block S102, a reference state sequence of the legged robot and a swing parameter of each leg at a next moment are generated based on the movement instruction and the movement state.

[0053] The reference state sequence may be a series of reference variables from a current moment to a future time period. The reference state sequence may include a trunk reference state sequence, a reference position sequence of a foot end of the supporting leg, a reference contact state sequence, and an MPC reference frame interval sequence, etc. The swing parameter may include an initial state of a foot end of the swing leg (an initial position and a linear velocity), the touchdown state (a touchdown point position and a linear velocity), a swing duration, and the desired maximum height of the foot end of the swing leg from the ground, etc.

[0054] It should be understood that the embodiments of the present disclosure may utilize the obtained movement instruction and the obtained movement state to generate the reference state sequence of the legged robot and the swing parameter of each leg at the next moment. The embodiments of the present disclosure may perform generation on the reference state sequence and the swing parameter at the next moment in at least one way, such as utilizing the obtained information for movement planning.

[0055] In an exemplary embodiment of the present disclosure, as illustrated in FIG. 2, the Rough Planning module may perform a rough movement planning on the quadruped robot based on feedback information from the state estimation and the movement velocity instruction (including a forward/backward walking velocity, a left and right lateral translational velocity) and the movement direction instruction (i.e., a yaw angular velocity) inputted by the user (such as inputted via a remote controller). The Rough Planning module needs to provide the reference state sequence for the SRB-MPC. A quantity of the reference state sequences is the same as a predicted quantity of frames of the SRB-MPC. The Rough Planning module also needs to provide a touchdown point position for a next swing of each leg, a swing duration, and the current reference contact state for the IA-TO.

[0056] At block S103, a foot-end reference state of each leg is determined based on the movement state and the swing parameter of each leg at the next moment. Based on a foot-end dynamic model and a discrete collision model, an impact-aware swing leg trajectory is planned for a leg that starts to swing at the next moment, to enable an influence caused by an early collision to be within a target constraint range.

[0057] It should be understood that the embodiments of the present disclosure may determine the foot-end reference state of each leg based on the obtained movement state and the swing parameter of each leg of the legged robot at the next moment. When there is the leg starts to swing at the next moment, the impact-aware swing leg trajectory is planned using the foot-end dynamic model and the discrete collision model, to enable the influence caused by the early collision during movement of the swing leg to be within the target constraint range.

[0058] It should be noted that factors such as slight ground unevenness, state estimation errors, and control errors may cause the foot-end of the swing leg of the legged robot to contact with the ground before the planned touchdown time, resulting in an early collision. This unexpected early contact often brings unexpected collision impacts. This issue may actually be addressed during planning by planning an impact-aware food-end movement trajectory of the swing leg, causing that even if the early collision occurs, the impact force of the collision is within an expected range. Therefore, the embodiments of the present disclosure may formulate a trajectory optimization problem applicable to the swing leg of the legged robot. The employed robot dynamic model is a linearized dynamic model of the foot end of the swing leg in the operation space. The discrete collision model is a discrete collision dynamic model. In the trajectory optimization problem, a prediction time window depends on a planned foot-end swing duration, i.e., a pre-specified foot-end swing duration. To enable online trajectory optimization on the robot's computer, a corresponding optimization problem is designed as an easily solvable Quadratic Programming (QP) problem.

[0059] In an exemplary embodiment of the present disclosure, as illustrated in FIG. 2, the Rough Planning module may be configured to provide the IA-TO with the touchdown point position for the next swing of each leg, the swing duration, and the current reference contact state. The IA-TO module may be configured to receive feedback information from the state estimation and reference information from the Rough Planning, generate an appropriate reference state (including a reference point position and a linear velocity) for the foot end of each leg. The SRB-MPC module may be configured to solve an optimization problem in model predictive control based on the reference information from the Rough Planning, obtain an optimal trunk reference state and an optimal reference foot-sole force, which are directly transferred to the IA-WBC. The SRB-MPC may be configured to solve the optimization problem approximately every frame. When the optimization problem has no need to be solved, an output of the SRB-MPC is not updated, i.e., output data is zero-order held.

[0060] For the supporting leg, a reference state thereof is to remain stationary on the ground. For the leg that starts to swing, the impact-aware trajectory optimization problem is solved. For a leg already in swing, an optimal solution for the trajectory optimization is interpolated to obtain a reference state at a current moment. The IA-TO module is configured to solve the trajectory optimization only once before a leg needs to start swinging, requiring minimal computation at other times. The IA-TO module ultimately may transfer the reference state of the foot end of each leg, along with the reference contact state provided by the Rough Planning, to the IA-WBC.

[0061] In the embodiments of the present disclosure, the planning, based on the foot-end dynamic model and the discrete collision model, the impact-aware swing leg trajectory includes: obtaining a pre-defined state variable and a target constraint condition; constructing, based on the foot-end dynamic model, the pre-defined state variable, and the target constraint condition, a continuous-time trajectory optimization problem for the leg that starts to swing at a next moment; and discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for a foot end of a swing leg, and solving the discrete-time trajectory optimization problem to obtain the impact-aware swing leg trajectory.

[0062] The pre-defined state variable may include: an initial state of the foot end of the swing leg, including an initial point position and the linear velocity; a touchdown state of the foot end of the swing leg, including the touchdown point position and the linear velocity; a swing duration T.sub.sw; and a desired maximum height

[00036]$z_h^{\mathrm{ref}}$

of the foot end of the swing leg from the ground.

[0063] The pre-specified state variable in the embodiments of the present disclosure includes one or more of: an initial state of the foot end of the swing leg, a touchdown state of the foot end of the swing leg, a swing duration, and a desired maximum height of the foot end of the swing leg from the ground. The target constraint condition includes one or more of: a state equation equality constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equality constraint, a terminal state equality constraint, an impact-aware inequality constraint, and a terminal velocity direction inequality constraint.

[0064] It should be understood that before each leg starts to swing, a trajectory optimization problem needs to be solved to obtain an optimal foot swing trajectory. Considering a limited computing power on an actual robot, trajectory optimization problem for the same leg is not solved repeatedly during swinging of the foot end. Since an expression of the trajectory optimization problem for each leg is essentially identical, for the sake of simplicity in expression, all subscripts l used to indicate a leg index are omitted where no ambiguity exists in the embodiments of the disclosure.

[0065] In an exemplary embodiment of the present disclosure, the linear acceleration of the foot end of the swing leg may be used as a control variable, i.e., u={umlaut over (x)}.sub.c. A state variable is selected as a point position and a linear velocity of the foot end, i.e.,

[00037]$x={\left[\begin{array}{cc}{x}_c^T& {\stackrel{.}{x}}_c^T\end{array}\right]}^T{}^6.$

Based on this, an objective function, a constraint condition, and a discrete-time trajectory optimization problem of the embodiments of the present disclosure are described below.

I. Objective Function

[0066] In the embodiments of the present disclosure, the foot-end dynamic model is:

[00038]${}_{c,l}{\stackrel{.Math.}{x}}_{c,l}+h_{c,l}=F_j,$

where {umlaut over (x)}.sub.c,l is a linear acceleration of a foot end of an l-th leg in an inertial system I, .sub.c,l is a mass matrix in an operation space, h.sub.c,l is a term including a Coriolis force, a centripetal force, and a gravitational force in the operation space, and F.sub.j is a foot-end driving force obtained by mapping a torque .sub.j of a driving joint to the operation space. A process for obtaining the foot-end dynamic model in the embodiments of the present disclosure is as follows.

[0067] In the embodiments of the present disclosure, a point position, a linear velocity, and the linear acceleration of the foot end of the l-th leg in the inertial system I may be denoted as x.sub.c,l, {dot over (x)}.sub.c,l, {umlaut over (x)}.sub.c,l, respectively. Then, a dynamic equation for the foot end in the operation space is:

[00039]${}_{c,l}\left(q_g\right){\stackrel{.Math.}{x}}_{c,l}+h_{c,l}\left(q_g,\stackrel{.}{q}\right)=F_j+F_{c,l}$

where F.sub.j, F.sub.c,l are the foot-end driving force obtained by mapping the torque .sub.j of the driving joint to the operation space and a ground reaction force on the foot end (the latter exists only during contact with the ground) respectively, .sub.c,l(q.sub.g) is a mass matrix in the operation space, and h.sub.c,l(q.sub.g,{dot over (q)}) is a term including the Coriolis force, the centripetal force, and the gravitational force in the operation space.

[0068] Considering that even if the legged robot performs a dynamic movement, the swing leg of the legged robot mostly remains within certain ranges under a trunk. The embodiments of the present disclosure may assume that a range of leg configuration variation in a single swing phase is not excessively large. Therefore, .sub.c,l(q.sub.g) may be approximately considered as configuration-independent and treated as a constant matrix. Experimental evidence shows that a nonlinear term h.sub.c,l(q.sub.g,{dot over (q)}) also varies insignificantly during leg swing and may be approximately as a constant vector.

[0069] Consequently, the simplified foot-end dynamic model is obtained as:

[00040]${}_{c,l}{\stackrel{.Math.}{x}}_{c,l}+h_{c,l}=F_j,$

treating a foot sole of the l-th leg as a first object and the ground as a second object, and considering that a ground mass is much larger than a robot mass and that a ground velocity remains zero before the collision and after the collision, the discrete collision model of the embodiments of the present disclosure is further derived as:

[00041]${\stackrel{.}{x}}_{c,l}=P_n\left({\stackrel{.}{x}}_1^{+}-{\stackrel{.}{x}}_1^{-}\right)=-\left(1+c_r\right)P_n{\stackrel{.}{x}}_{c,l}^{-},$

where P.sub.n=nn.sup.T is a projection operator on a collision normal vector, n is a unit normal vector of a collision surface,

[00042]${\stackrel{.}{x}}_1^{+},{\stackrel{.}{x}}_1^{-}{}^3$

are a velocity of a first object before collision and a velocity of the first object after the collision respectively,

[00043]${\stackrel{.}{x}}_2^{+},{\stackrel{.}{x}}_2^{-}{}^3$

are a velocity of a second object before collision and a velocity of the second object after the collision respectively, and c.sub.r is a restitution coefficient, which is defined as

[00044]$c_r\stackrel{\mathrm{def}}{=}-\frac{n^T\left({\stackrel{.}{x}}_2^{+}-{\stackrel{.}{x}}_1^{+}\right)}{n^T\left({\stackrel{.}{x}}_2^{-}-{\stackrel{.}{x}}_1^{-}\right)}.$

[0070] In the embodiments of the present disclosure, an objective function of the continuous-time trajectory optimization problem is:

[00045]${\min }_{x\left(t\right),u(\mathrm{t0}}{f}_{\mathrm{TO}}={.Math.x\left(t_F\right)-\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{.}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right].Math.}_{Q_{x_F}}+{}_{t_0}^{t_F}{.Math.u\left(\right).Math.}_{Q_u}d+{}_{t_{h_1}}^{t_{h_2}}{.Math.z_c\left(\right)-z_h^{\mathrm{ref}}.Math.}_{Q_z}d,$

where Q.sub.x.sub.F, Q.sub.u, and Q.sub.z are weight matrices (mostly diagonal matrices) with different dimensions, a.sub.Q=a.sup.TQa represents a weighted norm of a vector a under a weight matrix Q,

[00046]${.Math.x\left(t_F\right)-\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{.}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right].Math.}_{Q_{x_F}}$

reflects a task of a touchdown state of the foot end of the swing leg,

[00047]$x_{c,F}^{\mathrm{ref}}{}^3\mathrm{and}{x}_{c,F}^{\mathrm{ref}}{}^3$

are a known terminal position of the foot end of the swing leg and a known linear velocity of the foot end of the swing leg respectively,

[00048]${}_{t_0}^{t_F}{.Math.u\left(\right).Math.}_{Q_u}d$

is a minimum control variable (i.e., a linear acceleration) in an entire swing process, and t.sub.0=0, t.sub.F=T.sub.sw, where T.sub.sw is a swing duration,

[00049]${}_{t_{h_1}}^{t_{h_2}}{.Math.z_c\left(\right)-z_h^{\mathrm{ref}}.Math.}_{Q_z}d$

reflects a task of a maximum height of the swing leg from the ground, t.sub.h1 and t.sub.h2 are parameters specifying a time period to reach the maximum height, satisfying t.sub.0t.sub.h1t.sub.h2t.sub.F, and z.sub.c is a third component of the state variable, i.e., a component representing a height of the foot end from the ground in the state variable.

II. Constraints

[0071] (1) State Equation Equality Constraint: since the control variable is selected as the linear acceleration of the foot end, the state equation is straightforward. Define:

[00050]$A_c=\left[\begin{array}{cc}{0}_{33}& I_{33}\\ 0_{33}& 0_{33}\end{array}\right],B_c=\left[\begin{array}{c}{0}_{33}\\ I_{33}\end{array}\right],$

Then, a state equation of a system can be written as:

[00051]$\frac{d}{\mathrm{dt}}x\left(t\right)=A_{c}x\left(t\right)+B_{c}u\left(t\right),\mathrm{for}t\left[t_0,t_F\right].$

[0072] (2) Driving Force Constraint: due to output torque limits of an actual actuator, i.e., an upper limit of an output torque and a lower limit of the output torque, correspondingly the foot-end driving force in the operation space has a lower limit .sub.min and an upper limit .sub.max. Therefore, the driving force constraint can be written as:

[00052]$f_{\min }{S}_{2}x\left(t\right){}_{c,\max },\mathrm{for}t\left[t_0,t_F\right],$

[0073] (3) Trajectory Height Constraint: to prevent an optimized trajectory from being below the ground or higher than the desired maximum height from the ground, constraints on a trajectory height are needed, i.e.,

[00053]$z_{c,\min }{S}_{2}x\left(t\right)z_{c,\max },\mathrm{for}t\left[t_0,t_F\right],$

where z.sub.c,min, z.sub.c,max are an upper limit and a lower limit of the trajectory height, and S.sub.z=[0,0,1,0,0,0] is a selection matrix used to select a third component of a state.

[0074] (4) Initial State Constraint: the initial state in the trajectory optimization of the foot end of the swing leg is a state just before the foot end leaves the ground. Therefore, the initial state constraint is:

[00054]$x\left(t_0\right)=\left[\begin{array}{c}{x}_{c,0}^{\mathrm{fb}}\\ {\stackrel{.}{x}}_{c,0}^{\mathrm{fb}}\end{array}\right],$

where

[00055]$x_{c,0}^{\mathrm{fb}},{\stackrel{.}{x}}_{c,0}^{\mathrm{fb}}{}^3$

are a latest feedback point position and a latest linear velocity of the foot end of the swing leg before solving the trajectory optimization problem, respectively.

[0075] (5) Terminal State Constraint: although the objective function already incorporates a task of a terminal state, the objective function cannot guarantee that a height of an end of the optimized foot-end trajectory from the ground is zero. That is, the end of the trajectory may be suspended in the air or penetrate the ground. Therefore, a terminal state constraint is needed, i.e.,

[00056]$S_{z,\stackrel{.}{z}}x\left(t_F\right)=\left[\begin{array}{c}{z}_{c,F}^{\mathrm{ref}}\\ {\stackrel{.}{z}}_{c,F}^{\mathrm{ref}}\end{array}\right],$

where

[00057]$z_{c,F}^{\mathrm{ref}}\mathrm{and}{\stackrel{.}{z}}_{c,F}^{\mathrm{ref}}$

are a reference height and a vertical direction reference velocity at the trajectory's end, respectively, and where

[00058]${\stackrel{.}{z}}_{c,F}^{\mathrm{ref}}=0.$

S.sub.z, is a corresponding selection matrix, i.e.,

[00059]$S_{z,\stackrel{.}{z}}=\left[\begin{array}{c}001000\\ 000001\end{array}\right].$

[0076] It should be noted that the terminal state constraint in the embodiments of the present disclosure does not constrain a component of a horizontal direction in the state. This is because, under conditions with many constraints, a desired terminal state in the horizontal direction may be unattainable. Enforcing the terminal state constraint in the horizontal direction may result in an infeasible optimization problem.

[0077] (6) Impact-aware Constraint: to ensure sufficient deceleration of the swing leg before touchdown to reach a safe velocity, an impact-aware constraint is considered at an end of the trajectory in the embodiments of the present disclosure. Assuming a collision process starts at a time of t.sub.c and ends at a time of t.sub.c+t, both sides of an equation of .sub.c,l(q.sub.g){umlaut over (x)}.sub.c,l+h.sub.c,l(q.sub.g,{dot over (q)})=F.sub.j+F.sub.c,l is integrated, it follows that:

[00060]${}_{t_c}^{t_c+t}{}_c\left(q_g\right){\stackrel{.Math.}{x}}_c+h_c\left(q_c,\stackrel{.}{q}\right)\mathrm{dt}={}_{t_c}^{t_c+t}{F}_j+F_c\mathrm{dt}.$

[0078] Since a configuration of the robot remains almost unchanged before the collision and after the collision, i.e.,

[00061]$q_g^{+}=q_g^{-}.$

In addition, a collision process is extremely short, i.e., t.fwdarw.0. In this case, an impulse from a limited force (such as the gravitational force, the Coriolis force, and the centripetal force) may appear in this limit, leaving only an impulse generated by the collision. Therefore, the above formula becomes

[00062]${}_c{\stackrel{.}{x}}_c={}_c\left({\stackrel{.}{x}}_c^{+}-{\stackrel{.}{x}}_c^{-}\right)={\mathrm{.Math.}}_c$

where t.fwdarw.0, where l.sub.c represents an impulse exerted by the ground on the foot end. Substituting this expression into a pre-collision model yields the impact-aware constraint:

[00063]${\mathrm{.Math.}}_{c,\min }{}_c\left(-\left(1+c_r\right)P_n{\stackrel{.}{x}}_c\left(t\right)\right){\mathrm{.Math.}}_{c,\max },\mathrm{for}t\left[t_{\mathrm{imp}},t_F\right],$

where l.sub.c,min, l.sub.c,max are an artificially specified lower limit of an allowable collision impulse and an artificially specified upper limit of the allowable collision impulse respectively, and t.sub.imp is an artificially specified duration to activate the impact-aware constraint, satisfying t.sub.0t.sub.impt.sub.F.

[0079] It should be understood that a formula of the impact-aware constraint may indicate that if the early collision occurs due to certain reasons during a time period of [t.sub.imp, t.sub.F] immediately before the foot end of the swing leg touches the ground, an impulse caused by the collision may fall within a constraint range of this equation. Further, this constraint cannot be activated throughout the entire swing process. Otherwise, the entire swing movement may become excessively slow, resulting in a failure in realizing normal running movement.

[0080] (7) Terminal Velocity Direction Constraint: if a velocity direction of the foot end of the swing leg is nearly horizontal just before touchdown, then due to errors in controlling a height of the foot end, the foot sole touching the ground prematurely may occur, resulting in the foot end of the swing leg not reaching the planned foot point position, which may potentially affect stability of a center of mass of the robot. Therefore, the embodiments of the present disclosure can design the terminal velocity direction constraint for the foot end, in such a manner that an angle .sub.c between a velocity {dot over (x)}.sub.c(t) of the foot end of the swing leg before touchdown and a unit normal vector n (a negative sign indicates a direction pointing inwardly) of the collision surface is within a certain specified range, i.e.:

[00064]$\cos {}_{c,\max }\frac{-n^T{\stackrel{.}{x}}_c\left(t\right)}{{.Math.{\stackrel{.}{x}}_c\left(t\right).Math.}_2},\mathrm{for}t\left[t_{\mathrm{imp}},t_F\right],$

where

[00065]${}_{c,\max }\left(0,\frac{}{2}\right)$

is an artificially specified maximum allowable angle. The above nonlinear expression may be linearized through scaling. Based on a relationship between a norm and a norm of the vector, it follows that:

[00066]$\frac{-n^T{\stackrel{.}{x}}_c\left(t\right)}{{.Math.{\stackrel{.}{x}}_c\left(t\right).Math.}_1}\frac{-n^T{\stackrel{.}{x}}_c\left(t\right)}{{.Math.{\stackrel{.}{x}}_c\left(t\right).Math.}_2},x_c\left(t\right)0_{31}\mathrm{and}{x}_c\left(t\right)$

[0081] Therefore, the rewritten

[00067]$\cos {}_{c,\max }\frac{-n^T{\stackrel{.}{x}}_c\left(t\right)}{{.Math.{\stackrel{.}{x}}_c\left(t\right).Math.}_2},\mathrm{for}t\left[t_{\mathrm{imp}},t_F\right],$

may be obtained. Any solution that satisfies the rewritten formula also satisfies the original formula. Therefore, the rewritten formula may be used as the terminal velocity direction constraint for TO, which can be further written as:

[00068]$.Math.{\stackrel{.}{x}}_{c,x}\left(t\right).Math.+.Math.{\stackrel{.}{x}}_{c,y}\left(t\right).Math.+.Math.{\stackrel{.}{x}}_{c,z}\left(t\right).Math.-\frac{1}{\cos {}_{c,\max }}{n}^T{\stackrel{.}{x}}_c\left(t\right),\mathrm{for}t\left[t_{\mathrm{imp}},t_F\right],$

where {dot over (x)}.sub.c,x(t),{dot over (x)}.sub.c,y(t),{dot over (x)}.sub.c,z(t) is respective components of {dot over (x)}.sub.c(t). By traversing all cases within absolute value symbols, the above expression can be further written as eight equivalent inequalities, which are:

[00069]$\left[\begin{array}{ccc}1& 1& 1\\ -1& 1& 1\\ 1& -1& 1\\ -1& -1& 1\\ 1& 1& -1\\ -1& 1& -1\\ 1& -1& -1\\ -1& -1& -1\end{array}\right]{\stackrel{.}{x}}_c\left(t\right)-\frac{1}{\cos {}_{c,\max }}\left[\begin{array}{c}{n}^T\\ n^T\\ n^T\\ n^T\\ n^T\\ n^T\\ n^T\\ n^T\end{array}\right]{\stackrel{.}{x}}_c\left(t\right),\mathrm{for}t\left[t_{\mathrm{imp}},t_F\right]$

[0082] However, during a short time period before the swing leg touches the ground, it is usually the case that {dot over (x)}.sub.c,z(t)0. The first four rows of the equation are typically redundant. Therefore, the terminal velocity direction constraint can be simplified to:

[00070]$\left(C_{{}_1}+\frac{1}{\cos {}_{c,\max }}{D}_n\right)x_c\left(t\right)0_{41},\mathrm{for}t\left[t_{\mathrm{imp}},t_F\right],\mathrm{where}{C}_{{}_1}=\left[\begin{array}{ccc}1& 1& -1\\ -1& 1& -1\\ 1& -1& -1\\ -1& -1& -1\end{array}\right],D_n=\left[\begin{array}{c}{n}^T\\ n^T\\ n^T\\ n^T\end{array}\right].$

III. Discrete-Time Trajectory Optimization Problem

[0083] It should be understood that, to facilitate the solution of the above continuous-time trajectory optimization problem on a computer, the embodiments of the present disclosure may also perform discretization. In particular, at least one discretization method can be used in the embodiments of the present disclosure. For example, an integral term in the objective function can be replaced by a finite sum, and the state equation constraint can be replaced by a Taylor expansion formula with higher-order terms neglected.

[0084] In the embodiments of the present disclosure, assume that a swing process is evenly divided into N segments over time, i.e., there are N frames, with a frame interval of t=T.sub.sw/N. Let t.sub.k=t.sub.0+k(t.sub.Ft.sub.0)/N, k=1, 2, . . . , N, then we can denote x.sub.k=x(t.sub.k) and u.sub.k=u(t.sub.k). The entire discrete-time trajectory optimization problem can be formulated as.

[00071]$g$$z_{c,\min }{S}_z{x}_k{z}_{c,\max },k=1,2,.Math.,N-1,$$x_0=x_0^{\mathrm{fb}},$$S_{Z,\stackrel{.}{Z}}{x}_N=S_{Z,\stackrel{.}{Z}}{x}_N^{\mathrm{ref}},$${\mathrm{.Math.}}_{c,\min }{}_c\left[-\left(1+c_r\right)P_n{S}_v{x}_k\right]{\mathrm{.Math.}}_{c,\max },k=k_{\mathrm{imp}},k_{\mathrm{imp}}+1,.Math.,N,$$\left(C_{{}_1}+\frac{1}{\cos {}_{c,\max }}{D}_n\right)S_v{x}_k{0}_{41},k=k_{\mathrm{imp}},k_{\mathrm{imp}}+1,.Math.,N,$

where

[00072]$U={\left[u_0^T,u_1^T,.Math.,u_{N-1}^T\right]}^T$

is a vector composed of a control variable of each frame, Q.sub.x.sub.N, Q.sub.u.sub.k, and Q.sub.z are diagonal weight matrices with different dimensions,

[00073]$x_0^{\mathrm{fb}}=\left[\begin{array}{c}{x}_{c,0}^{\mathrm{fb}}\\ {\stackrel{.}{x}}_{c,0}^{\mathrm{fb}}\end{array}\right]$

is a feedback of a foot-end state (including a position and a velocity) at a current moment,

[00074]$x_N^{\mathrm{ref}}=\left[\begin{array}{c}{x}_{c,F}^{\mathrm{ref}}\\ {\stackrel{.}{x}}_{c,F}^{\mathrm{ref}}\end{array}\right]$

is a reference terminal foot-end state (including the position and the velocity), S.sub.=[0.sub.33I.sub.33] is a selection matrix of a foot-end linear velocity, k.sub.h.sub.1 and k.sub.h.sub.2 are an artificially specified start frame and an artificially specified end frame for maintaining a swing height, satisfying 0k.sub.h.sub.1k.sub.h.sub.2<N, and k.sub.imp is a start frame for enabling an impact-aware constraint and a terminal velocity direction constraint, satisfying 0k.sub.impN.

[0085] It should be understood that, since trajectory optimization problems in many studies are highly nonlinear and typically belong to non-convex optimization problems, using some professional nonlinear solvers can help find local optimal solutions for the trajectory optimization problems, but cannot guarantee a global optimal solution. In the embodiments of the present disclosure, the trajectory optimization problem is a QP problem, which belongs to a typical convex optimization problem. Therefore, as long as the problem has a non-empty solution set, an optimal solution found by the optimizer is the global optimal solution.

[0086] For the objective function of the discrete-time trajectory optimization problem of the foot end of the swing leg described above, some QP solvers can be used for solving the function. For example, in the embodiments of the present disclosure, an open-source library of qpOASES based on C++ can be used, which requires the QP problem to be converted into a standard form:

[00075]${\min }_U\frac{1}{2}{U}^T\mathrm{HU}+g^{T}U,$$s.t.lb\mathrm{CU}\mathrm{ub}.$

[0087] After solving the optimal solution of the above equation, only an optimal control variable U* of N frames is obtained, which is used to determine an optimal position and a velocity trajectory. The above N rows of discrete dynamic equations are written as a linear expression in terms of an initial state and an optimization variable, i.e.,

[00076]$X=A_{\mathrm{QP}}{x}_0^{\mathrm{fb}}+B_{\mathrm{QP}}U,$$\mathrm{where}X={\left[x_0^T,x_1^T,.Math.,x_N^T\right]}^T{}^{6N},\mathrm{and}{A}_{\mathrm{QP}}\mathrm{and}{B}_{\mathrm{QP}}\mathrm{are}$$A_{\mathrm{QP}}=\left[\begin{array}{c}A\\ A^2\\ .Math.\\ A^N\end{array}\right]{}^{6N6}\mathrm{and}{B}_{\mathrm{QP}}=\left[\begin{array}{cccc}B& 0_{63}& .Math.& 0_{63}\\ \mathrm{AB}& B& .Math.& 0_{63}\\ .Math.& .Math.& & .Math.\\ A^{N-1}B& A^{N-2}B& .Math.& B\end{array}\right]{}^{6N3N}.$

Therefore, by substituting U* into the above linear expression, a corresponding optimal position and a corresponding velocity for the N frames are obtained.

[0088] Since the subsequent control program may require a corresponding reference trajectory at any moment, in this case it is necessary to convert N discrete points into a continuous curve by an interpolation method. In the embodiments of the present disclosure, at least one method can be used to achieve interpolation. For a maximum computational efficiency, a simplest linear interpolation can be employed in the embodiment of the disclosure.

[0089] For example, assume that from the moment when the swing leg leaves the ground, in this case a time of t.sub.sw, has elapsed, where t.sub.sw[0, T.sub.sw]. A closest previous frame to this moment is a k-th frame, with the corresponding optimal position being

[00077]$x_{c,k}^{*}.$

Using a linear interpolation method, an optimal reference position in this case is:

[00078]$x_c^{*}\left(t_{\mathrm{sw}}\right)=x_{c,k}^{*}+\frac{t_{\mathrm{sw}}-kt}{t}{x}_{c,k+1}^{*}.$

Similarly, a corresponding optimal reference velocity

[00079]${\stackrel{.}{x}}_c^{*}\left(t_{\mathrm{sw}}\right)$

may be obtained.

[0090] It should be noted that this interpolation method fails to guarantee that a time integral of the velocity trajectory is a position curve. To achieve this, a higher-order interpolation function may be required. In this case, a trade-off is made by sacrificing some interpolation accuracy to maximize a computational efficiency.

[0091] At block S104, whole-body control is performed on the legged robot based on the movement state, the reference state sequence, and the foot-end reference state. Based on a whole-body dynamic model and the discrete collision model, impact-aware whole-body control is performed for a leg that is a supporting leg in planning but does not touch the ground, to enable an influence caused by a delayed collision to be within the target constraint range

[0092] It should be understood that, as illustrated in FIG. 2, the embodiments of the present disclosure can solve a Weighted Quadratic Programming (WQP) problem and calculate an optimal joint torque instruction based on feedback information from state estimation and reference information from the SRB-MPC module and the IA-TO module, achieving whole-body control of the legged robot and addressing an issue of the delayed collision.

[0093] It should be noted that the term delayed collision refers to a situation where, due to certain reasons (e.g., the slight ground unevenness, errors in state estimation, or control inaccuracies), the foot end has not yet made contact with the ground at an originally planned time point, in this case contact between the foot end and the ground necessarily occurs later than the originally planned time point. Without an appropriate control strategy, such unintended delayed collisions can easily result in unintended collision impact. The problem of delayed collision is substantially different from that of early collision, in that the delayed collision occurs outside a planned swing process. Therefore, a distance between the foot sole and the ground (which is expected to be zero) cannot be accurately determined, and also an exact timing of ground contact (which is supposed to occur at a specific moment) becomes uncertain, which may be effectively solved in a control stage. Therefore, based on a core concept of one step predictive control, the embodiments of the present disclosure may use an impact-aware Whole Body Control (WBC) method.

[0094] In the following embodiments, the quadruped robot is used as an example to describe the WBC method of the embodiments of the present disclosure. The impact-aware constraint is not always enabled. Depending on different situations, a task objective and constraint setting of whole-body control vary accordingly. Therefore, the embodiments of the present disclosure may be expressed using a decision tree as illustrated in FIG. 3. The impact-aware whole-body control in the embodiment of the present disclosure is detailed as follows.

I. Whole-Body Control Based on Weighted Quadratic Programming

[0095] It should be understood that, to enable the whole-body control to properly handle an inequality constraint such as the impact-aware constraint, an optimization-based approach is selected for implementation of the whole-body control. Also, considering limited computational resources available on the actual robot and thus there are multiple programs (e.g., communication, control algorithms) that need to operate simultaneously, the embodiments of the present disclosure may adopt a whole-body control method implemented based on the WQP.

[0096] In the embodiments of the present disclosure, an optimization problem of the whole-body control is:

[00080]${\min }_{}{.Math.}_{i=1}^{n_{\mathrm{task}}}{.Math.W_i\left(A_i-b_i\right).Math.}_2^2,$$s.t.l{b}_j{C}_j{\mathrm{ub}}_j,j=1,2,.Math.,n_{\mathrm{constraint}},$

where A.sub.i is a task matrix, b.sub.i is a task vector, c.sub.j is a constraint matrix, lb.sub.j and ub.sub.j are a lower constraint bound and an upper constraint bound, W.sub.i is a weight matrix, n.sub.task is a quantity of tasks, and n.sub.constraint is a quantity of constraints.

[0097] It should be understood that, in the WQP, all tasks that need to be completed are represented in the objective function in a form of A.sub.ib.sub.i, and a relative priority of tasks is adjusted by the weight matrix W.sub.i. All constraints are expressed in a form of lb.sub.jc.sub.jub.sub.j. The equality constraint may be achieved by setting lb.sub.j=ub.sub.j. In the embodiments of the present disclosure, optimization variables of the WBC are selected as a generalized joint space acceleration of {umlaut over (q)} and an augmented foot-sole force of F.sub.c formed by stacking forces acting on foot ends of four feet, i.e.,

[00081]$={\left[{\stackrel{.Math.}{q}}^T,F_c^T\right]}^T{}^{n_{}},$

where

[00082]$n_{}=n_q+n_F.$

[0098] When the quadruped robot is in a stationary standing state, the whole-body dynamic model of the legged robot in the embodiments of the present disclosure is:

[00083]$M\left(q_g\right)\stackrel{.Math.}{q}+h\left(q_g,\stackrel{.}{q}\right)=\left[\begin{array}{c}{0}_{61}\\ {}_j\end{array}\right]+J_c^T\left(q_g\right)F_c,$

where M(q.sub.q) is a generalized mass matrix, h(q.sub.g,{dot over (q)}) is a term including a Coriolis force, a centripetal force, and a gravitational force, .sub.j represents an output torque of a driving joint, 0.sub.nm represents a zero matrix with a size of nm,

[00084]$J_c^T\left(q_g\right)$

and F.sub.c are an augmented Jacobian matrix formed by stacking a contact Jacobian matrix of each supporting leg and an augmented foot-sole force formed by stacking a ground reaction force on a foot sole of each supporting leg, respectively, q.sub.g is a generalized joint space position, {dot over (q)} is a generalized joint space velocity, and {umlaut over (q)} is a generalized joint space acceleration.

[0099] By solving the above QP problem, the optimal solution * may be obtained. Combining with a complete dynamic equation, the optimal joint torque instruction

[00085]${}_j^{\mathrm{cmd}}$

can be easily calculated as:

[00086]${}_j^{\mathrm{cmd}}=\left(S_{j}M\left(q_g^{\mathrm{fb}}\right)-S_j{J}_c^T\left(q_g^{\mathrm{fb}}\right)\right){}^{*}+S_{j}h\left(q_g^{\mathrm{fb}},{\stackrel{.}{q}}^{\mathrm{fb}}\right),$

where S.sub.j=[0.sub.n.sub.j.sub.6I.sub.n.sub.j.sub.n.sub.j] is a selection matrix of the driving joint, and

[00087]$q_g^{\mathrm{fb}}$

and {dot over (q)}.sup.fb are the generalized joint space position and a velocity feedback provided by the state estimation respectively.

II. Task Setting

[0100] In the embodiments of the present disclosure, tasks processed simultaneously by the whole-body control include two or more of: a trunk trajectory tracking task, a foot-end trajectory tracking task, a foot-sole force tracking task, a task of minimizing variation of a joint torque, and a task of minimizing variation of a foot-sole force. Constraints processed simultaneously by the impact-aware whole-body control include two or more of: a floating-base dynamic equality constraint, a foot-sole force inequality constraint, a joint output torque saturation inequality constraint, a joint rotational speed saturation inequality constraint, a joint output power saturation inequality constraint, and an impact-aware constraint.

[0101] In an exemplary embodiment of the present disclosure, (1) a trunk tracking task: the trunk trajectory tracking task is a basic task allowing for movement of the quadruped robot, which is indispensable. In the embodiments of the present disclosure, a trunk reference movement trajectory provided by the Model Predictive Control (MPC) may be used. The trajectory includes an optimal reference point position

[00088]$r_b^{\mathrm{MPC}}$

of a trunk, an optimal reference linear velocity

[00089]${\stackrel{.}{r}}_b^{\mathrm{MPC}},$

and an optimal ZYX Euler angle

[00090]${}_b^{MPC}$

in the inertial system I, and an optimal reference angular velocity

[00091]${}^B{}_b^{\mathrm{MPC}}$

of the trunk under a floating-base system B.

[0102] In the WBC, PD control is performed based on an optimal reference trajectory provided by the MPC and a feedback state provided by the state estimation to obtain a linear acceleration instruction

[00092]${\stackrel{.Math.}{r}}_b^{\mathrm{cmd}}$

of the trunk in the inertial system I and an angular acceleration instruction

[00093]${}^B{\stackrel{.}{}}_b^{\mathrm{cmd}}$

under the floating-base system B. Then, corresponding tasks are designed to complete these instructions. The PD control is:

[00094]${\stackrel{.Math.}{r}}_b^{\mathrm{cmd}}=K_{p,r_b}\left(r_b^{\mathrm{MPC}}-r_b^{\mathrm{tb}}\right)+K_{d,r_b}\left({\stackrel{.}{r}}_b^{\mathrm{MPC}}-{\stackrel{.}{r}}_b^{\mathrm{fb}}\right),$${}^B{\stackrel{.}{}}_b^{\mathrm{cmd}}=K_{p,{}_b}{}^B{e}_{}+K_{d,{}_b}\left({}^B{}_b^{\mathrm{MPC}}-{}^B{}_b^{\mathrm{fb}}\right),$

where K.sub.p,r.sub.b, K.sub.d,r.sub.b, K.sub.p,.sub.b, K.sub.d,.sub.b are PD gain coefficient matrices, all of which are diagonal matrices, and B.sub.e.sub. is an axis angle representing a pose error calculated based on a reference ZYX Euler angle

[00095]${}_b^{\mathrm{MPC}}$

and a feedback ZYX Euler angle

[00096]${}_b^{\mathrm{fb}}.$

[0103] Since the first 6 rows of {umlaut over (q)} correspond to a linear acceleration and an angular acceleration of a floating base, a corresponding matrix and a corresponding vector for the trunk trajectory tracking task are written as:

[00097]$A_1=\left[\begin{array}{cc}{I}_{66}& 0_{6\left(n_j+n_F\right)}\end{array}\right]\mathrm{and}{b}_1=\left[\begin{array}{c}{\stackrel{.Math.}{r}}_b^{\mathrm{cmd}}\\ {}^B{\stackrel{.}{}}_b^{\mathrm{cmd}}\end{array}\right].$

[0104] (2) Foot-end trajectory tracking task: the foot end trajectory tracking task is also a basic task for the movement of the quadruped robot, which is indispensable. For a foot end of the l-th leg, according to (non-rotational) acceleration-level kinematics, it follows that:

[00098]${\stackrel{.Math.}{x}}_{c,l}=J_{c,l}\left(q_g\right)\stackrel{.Math.}{q}+{\stackrel{.}{J}}_{c,l}\left(q_g\right)\stackrel{}{q}.$

[0105] Therefore, a corresponding matrix and a corresponding vector for a foot-end trajectory tracking subtask of the l-th leg can be written as:

[00099]$A_{2,l}=[\begin{array}{cc}{J}_{c,l}\left(q_g^{\mathrm{fb}}\right)& 0_{3n_F}]\end{array},$$b_{2,l}={\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}-{\stackrel{.}{J}}_{c,l}\left(q_g^{\mathrm{fb}}\right){\stackrel{.}{q}}^{\mathrm{fb}},\mathrm{and}$$l=1,2,3,4,$

where

[00100]${\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}$

is a linear acceleration instruction of the foot end of the l-th leg in the inertial system I. When the leg is under different states, the instruction varies.

[0106] For example, when the leg acts as the supporting leg, the foot end should be fixed on the ground without movement, i.e.,

[00101]${\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}=0_{31}.$

When the leg acts as the supporting leg but does not touch the ground (delayed contact with the ground),

[00102]${\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}$

is micro-control to track a velocity pointing to the collision surface, and

[00103]${\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}=K_{p,x_{c,l}}\left(-{}_{\mathrm{imp}}n-{\stackrel{}{x}}_{c,l}^{\mathrm{fb}}\right),$

where .sub.imp is an artificially specified desired collision velocity, and n is the unit normal vector of the collision surface previously mentioned, pointing outwardly from the collision surface. When the leg acts as the swing leg,

[00104]${\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}$

is obtained by the PD control:

[00105]${\stackrel{.Math.}{x}}_{c,l}^{\mathrm{cmd}}=K_{p,x_{c,l}}\left(x_{c,l}^{\mathrm{TO}}-x_{c,l}^{\mathrm{fb}}\right)+K_{d,x_{c,l}}\left({\stackrel{.}{x}}_{c,l}^{\mathrm{TO}}-{\stackrel{.}{x}}_{c,l}^{\mathrm{fb}}\right),$

where K.sub.p,x.sub.c,l, K.sub.d,x.sub.c,l is the PD gain coefficient matrices, both of which are diagonal matrices.

[00106]$x_{c,l}^{\mathrm{TO}},{\stackrel{.}{x}}_{c,l}^{\mathrm{TO}}$

are the optimal reference point position and the linear velocity of the foot end of the swing leg calculated in the above embodiments.

[0107] It should be noted that foot-end trajectory tracking subtasks of four legs in the embodiments of the present disclosure together constitute a complete foot-end trajectory tracking task. Therefore, the corresponding matrix and the corresponding vector can be written as:

[00107]$A_2=\left[\begin{array}{c}{A}_{2,1}\\ A_{2,2}\\ A_{2,3}\\ A_{2,4}\end{array}\right]\mathrm{and}{b}_2=\left[\begin{array}{c}{b}_{2,1}\\ b_{2,2}\\ b_{2,3}\\ b_{2,4}\end{array}\right],\mathrm{respectively}.$

[0108] (3) Foot-sole Force Tracking Task: a linear momentum and an angular momentum of a center of mass of the quadruped robot are affected not only by the gravitational force but also by the ground reaction force on the foot end (i.e., the foot-sole force). During high-dynamic movement, appropriate foot-sole forces are very important.

[0109] The embodiments of the present disclosure may provide a method to optimize reference-valuable foot-sole forces

[00108]$F_c^{\mathrm{MPC}}$

using a single-rigid-body model MPC. Corresponding tasks need to be set in the WBC to achieve foot-sole force tracking.

[0110] Since the quadruped robot used in the embodiments of the present disclosure does not have a force/torque sensor at the foot end of the quadruped robot, it is difficult to implement foot-sole force feedback control in the WBC. Therefore, the foot-sole force tracking task can only be set as a pure feedforward force task. The corresponding matrix and the corresponding vector can be written as:

[00109]$A_3=\left[0_{n_k{n}_q}{I}_{n_F{n}_F}\right]\mathrm{and}{b}_3=F_c^{\mathrm{ref}},$

respectively,

[00110]$F_c^{\mathrm{ref}}=\left[\begin{array}{c}{F}_{c,1}^{\mathrm{ref}}\\ F_{c,2}^{\mathrm{ref}}\\ F_{c,3}^{\mathrm{ref}}\\ F_{c,4}^{\mathrm{ref}}\end{array}\right].$

where
When the l-th leg touches the ground and acts as the supporting leg, the leg may execute the reference foot-sole force provided by the MPC. Therefore,

[00111]$F_{c,l}^{\mathrm{ref}}=F_{c,l}^{\mathrm{MPC}}.$

When the leg is the swing leg or is the supporting leg in planning but does not touch the ground (i.e., delayed contact with the ground), the foot end of the leg is not subjected to an external force. Therefore,

[00112]$F_{c,l}^{\mathrm{ref}}=0_{31}.$

[0111] (4) Task of Minimizing Variation of Joint Torque: due to a limited bandwidth of actual motor force control, excessively drastic changes in the output torque are difficult for practical implementation. When the actual motor cannot execute actions according to an optimal solution of the WBC, the control error tends to increase. Therefore, the embodiments of the present disclosure can incorporate consideration of reducing variation of the joint torque into the WBC, making it easier for the actual motor to execute actions according to the optimal solution of the WBC.

[0112] Assuming that an optimal joint torque instruction calculated by the WBC in a previous control cycle is

[00113]${}_j^{\mathrm{pre}},$

a matrix and a vector for the task of minimizing variation of a joint Torque in this control cycle can be written as:

[00114]$A_4=\left[S_{j}M\left(q_g^{\mathrm{fb}}\right)-S_j{J}_c^T\left(q_g^{\mathrm{fb}}\right)\right]\mathrm{and}$$b_4={}_j^{\mathrm{pre}}.fwdarw.S_{j}h\left(q_g^{fb},{\stackrel{.}{q}}^{\mathrm{fb}}\right),\mathrm{respectively}.$

[0113] It should be noted that this task may potentially conflict with the previously mentioned tasks, including the trunk trajectory tracking task, the foot-end trajectory tracking task, and the foot-sole force tracking task. Therefore, in the embodiments of the present disclosure, a weight assigned to this task may not be too high. Otherwise, execution performance of the above tasks may be affected.

[0114] (4) Task of Minimizing Variation of Foot-sole Force: to make variation in the foot-sole force smoother (thus making state variation of the center of mass smoother), the embodiments of the present disclosure can incorporate a task of reducing variation of the foot-sole force in the WBC.

[0115] Assume that an optimal foot-sole force calculated by the WBC in the previous control cycle is

[00115]$F_c^{\mathrm{pre}},$

a matrix and a vector for the task of minimizing variation of the foot-sole force in this control cycle can be written as:

[00116]$A_5=\left[0_{n_F{n}_q}{I}_{n_F{n}_F}\right]\mathrm{and}{b}_5=F_c^{\mathrm{pre}},$

respectively.

[0116] It should be noted that, similarly, this task may potentially conflict with the previously mentioned tasks, including the trunk trajectory tracking task, the foot-end trajectory tracking task, and the foot-sole force tracking task. Therefore, the weight assigned to this task may not be too high. Otherwise, execution performance of the other tasks may be affected.

III. Constraint Settings

[0117] (1) Floating Base Dynamic Equality Constraint: a trunk of the quadruped robot does not have a direct driving force and a torque. Therefore, the first six rows of a complete movement equation do not include a joint output torque. These six rows are floating base dynamic equations, which are equality constraints that need to be satisfied. A corresponding matrix and a corresponding vector are:

[00117]$C_1=\left[S_{b}M\left(q_g^{\mathrm{fb}}\right)-S_b{J}_c^T\left(q_g^{\mathrm{fb}}\right)\right]\mathrm{and}$${\mathrm{lb}}_1=u{b}_1=-S_{b}h\left(q_g^{\mathrm{fb}},{\stackrel{.}{q}}^{\mathrm{fg}}\right),\mathrm{repectively},$

where S.sub.b=[I.sub.66 0.sub.6n.sub.j] is a selection matrix of the floating base.

[0118] (2) Foot-sole Force Inequality Constraint: to avoid sliding of the foot end of the supporting leg relative to the ground, a ratio of a magnitude of a component parallel to a contact surface in the foot-sole force to a magnitude of a component perpendicular to the contact surface in the foot-sole force is smaller than a friction coefficient. That is, a foot-sole force of F.sub.c,l of the l-th leg may satisfy the following constraint to avoid slipping: (I.sub.33nn.sup.T)F.sub.c,l.sub.2|n.sup.TF.sub.c,l|, where is the friction coefficient.

[0119] It should be understood that the above formula describes a conical region. Therefore, this constraint for avoiding foot-end slippage is also called a friction cone constraint. Since the formula includes a norm .sub.2, which is a nonlinear expression, for the convenience of solving the WBC optimization problem, the embodiments of the present disclosure can simplify the formula.

[0120] In the embodiments of the present disclosure, the formula can be linearized, i.e., using a tetrahedral cone instead of a circular cone. Then, assuming that the quadruped robot usually walks on a flat ground with a slope close to zero, a Z-axis component of F.sub.c,l can be considered as the component perpendicular to the contact surface. Therefore, the above formula can be rewritten as:

[00118]$\{\begin{array}{c}.Math.F_{c,l,x}.Math..Math.F_{c,l,z}.Math.\\ .Math.F_{c,l,y}.Math..Math.F_{c,l,z}.Math.\end{array}.$

[0121] It should be noted that the foot-sole force is the ground reaction force on the foot sole, and thus the Z-axis component is always greater than zero. To avoid an excessively large foot-sole force from being optimized and causing hardware damage, the embodiments of the present disclosure may also have a limit on a magnitude of the Z-axis component of F.sub.c,l. Therefore, a foot-sole force inequality constraint for the l-th leg can be written as:

[00119]$\{\begin{array}{c}.Math.F_{c,l,x}.Math.F_{c,l,z}\\ .Math.F_{c,l,y}.Math.F_{c,l,z}\\ 0F_{c,l,z}{F}_{c,l,z_{\max }}\end{array},$

where F.sub.c,l,z.sub.max is an artificially specified maximum foot-sole force in a Z-axis direction. When this leg is already the supporting leg, F.sub.c,l,z.sub.max is a predetermined positive number. When this leg is the swing leg or is the supporting leg in planning but does not touch the ground (delayed contact with the ground), a foot-sole force of this leg is zero. Therefore, F.sub.c,l,z.sub.max=0. In the embodiments of the present disclosure, the above expression can equivalently be represented in matrix form as:

[00120]$0_{51}{C}_F{F}_{c,l}u{b}_F,$$\mathrm{where}{C}_F=\left[\begin{array}{ccc}1& 0& \\ -1& 0& \\ 0& 1& \\ 0& -1& \\ 0& 0& 1\end{array}\right],{\mathrm{ub}}_F=\left[\begin{array}{c}+\\ +\\ +\\ +\\ F_{c,l,z_{\max }}\end{array}\right].$

[0122] The final foot-sole force constraints for all four legs, when combined, constitute a complete foot force inequality constraint. A corresponding matrix and a corresponding vector can be written as:

[00121]$C_2=\left[\begin{array}{ccccc}& C_F& & & \\ & & C_F& & \\ 0_{20n_q}& & & C_F& \\ & & & & C_F\end{array}\right]\mathrm{and}$${\mathrm{lb}}_2=0_{201},{\mathrm{ub}}_2=\left[\begin{array}{c}{\mathrm{ub}}_F\\ {\mathrm{ub}}_F\\ {\mathrm{ub}}_F\\ {\mathrm{ub}}_F\end{array}\right],\mathrm{respectively}.$