Method and device for planning and controlling a leg robot, as well as robot and storage medium

Abstract

A method and a device for planning and controlling a leg robot, as well as a robot and a storage medium, wherein the method comprises: determining a foot-end reference state of each leg based on the motion state and swing parameter of the leg of a leg robot in the next instant; planning a collision-aware swing leg trajectory based on the foot-end dynamics model and the discrete collision model for the leg that will begin to swing in the next instant; performing whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot-end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model.This solves the problems of limited passive flexible buffering of collisions in leg robots in related technology, the large computational volume of the control algorithm, the low control accuracy, the high wear of the robot hardware, the short lifespan and the inability to meet the needs of practical applications.

Description

Cross-references of related applications

[0001] This application is based on the Chinese patent application with application number 202311511514.9 and a filing date of November 13, 2023, and claims priority from the Chinese patent application. The entire content of the Chinese patent application is incorporated herein by reference. Technical field

[0002] The present application relates to the technical field of robotics, in particular a method and a device for planning and controlling a leg robot, as well as a robot and a storage medium. State of the art

[0003] Leg robots are developing rapidly, and many are no longer limited to static walking but are becoming ever faster, more sensitive, more robust, safer, and more versatile. When a leg robot performs highly dynamic movements, the problem of collisions between the soles of its feet and the ground cannot be ignored.

[0004] In the appropriate technology, the leg robot can be adapted in terms of both hardware and control algorithm. Regarding hardware, foot pads with a specific degree of flexibility can be installed on the robot's soles, or spring damping and other damping devices can be installed on the robot's drive joints, or VIA (Variable Impedance Actuators) or VSA (Variable Stiffness Actuators) can be used to provide the desired passive mechanical impedance and achieve the effect of changing joint impedance at a mechanical level, allowing the damping effect to be adjusted according to actual needs. Regarding control algorithms, the speed of the foot sole contacting the ground can be brought close to zero in the motion planning.

[0005] However, there are certain shortcomings in the process of related technologies: with regard to the hardware, the introduction of the material has an influence on the response frequency of the robot system, and the stiffness, damping and inertia properties of the material are fixed, so that it is not possible to adapt the damping effect to the actual needs; the overall structure when using VIA or VSA is too complex, and the volume and weight are large, so that it is difficult to apply this type of actuator to leg robots that have to perform a highly dynamic movement.Regarding the control algorithms, when the leg robot performs highly dynamic movements, the actual movement trajectory of the swing leg can obviously deviate from the planned trajectory, which can cause the soles of the feet to touch the ground prematurely or be delayed, causing enormous shocks to the soles of the feet, which can drift the zero position of the joints and accelerate the aging of the hardware. Content of the invention

[0006] The present application provides a method and a device for planning and controlling a leg robot, as well as a robot and a storage medium, to solve the problems of limited passive flexible buffering of the collision of leg robots in the related technology, the large computational volume of the control algorithm, the low control accuracy, the high wear of the robot hardware, the short lifespan and the inability to meet the needs of practical applications.

[0007] An embodiment of the first aspect of the present application provides a method for planning and controlling a leg robot, comprising the following steps: acquiring a motion instruction and a motion state of the leg robot; generating a reference state sequence of the leg robot and a swing parameter of each leg at the next instant based on the motion instruction and the motion state; determining a foot-end reference state of each leg based on the motion state and the swing parameter of the leg at the next instant; planning a collision-aware swing leg trajectory based on the foot-end dynamic model and the discrete collision model for the leg that will begin to swing at the next instant, such that the effects of the premature collision are within the target constraint range;Performing whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model, such that the effects after a delayed collision are within the target constraint range.

[0008] The optional dynamic foot end model is: Λc,lx¨c,l+hc,l=Fj, where x¨c,l∈R3 the linear acceleration of the foot end of the first leg in inertial frame I is, ∧ c,l the mass matrix in the operating room is, h c,l the term that includes Coriolis, centripetal, and gravitational forces in the operating space, and Fj∈R3 the foot-end driving force of the torque τ jof the drive joint according to the figure into the operating space; and where the discrete collision model is: Δx˙c,l=Pn(x˙1+−x˙1−)=−(1+cr)Pnx˙c,l−, where Pn=nnT∈R3×3 the projection operator on the collision normal vector is, n∈R3 the unit normal vector of the collision surface; x˙1+,x˙1−∈R3 The speeds of object 1 before and after the collision are as follows: x˙2+,x˙2−∈R3 The speeds of object 2 before and after the collision are, respectively, and c r The coefficient of restitution is.

[0009] Optionally, planning a collision-aware swing leg trajectory based on the foot-end dynamics model and the discrete collision model includes the following: capturing a predefined state variable and goal constraints; constructing a continuous-time trajectory optimization problem for a leg that will begin swinging in the next moment, based on the foot-end dynamics model, the predefined state variable, and the goal constraints; discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot end of the swing leg; and solving the problem to obtain a collision-aware swing leg trajectory.

[0010] Optionally, the predefined state variables include one or more of an initial state of the foot end of the swing leg, a landing state of the foot end of the swing leg, a duration of the swing, and a desired maximum height of the foot end of the swing leg above the ground, wherein the target constraints include one or more of a state equation constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equation constraint, a final state equation constraint, a collision awareness inequality constraint, and a final velocity direction inequality constraint.

[0011] Optionally, the objective function of the continuous-time trajectory optimization problem is: minx(t),u(t)fTO=‖x(tF)−[xc,Frefx˙c,Fref]‖QxF+∫t0tF‖u(τ)‖Qudτ+∫th1th2‖zc(τ)−zhref‖Qzdτ, where QxF∈R6×6,Qu∈R3×3 and Qz∈R Weight matrices with different dimensions are (diagonal matrices are predominant); ||a|| Q = a T Qa represents the weighted modulus length of the vector a under the weight matrix Q; ‖x(tF)−[xc,Frefx˙c,Fref]‖QxF reflects the function of the landing position of the foot end of the swinging leg; xc,Fref∈R3 and x˙c,Fref∈R3 The known position of the foot end of the swing leg and the linear speed are in each case; ∫t0tF‖u(τ)‖Qudτ The control variable (i.e., the linear acceleration) for minimizing the overall momentum process is t0 = 0, t F = T SW , T SW the swing duration is; ∫th1th2‖zc(τ)−zhref‖Qzdτ The task of determining the maximum height of the swing leg above the ground reflects the two parameters t h1 and t h2 Specify the time required to reach the maximum height and the condition t0 ≤ th1 ≤ t h2 ≤ t F fulfill; z c the third component of the state variable is the height of the foot end above the ground. and where the entire discrete-time trajectory optimization problem is: minU fc,TO=‖xN−xNref‖QxN+∑k=0N−1‖uk‖Quk+∑k=kh1kh2‖Szxk−zhref‖Qzk, st[xc,k+1x˙c,k+1]︸xk+1=[I3×3ΔtI3×303×3I3×3]︸A[xc,kx˙c,k]︸xk+[12Δt2I3×3ΔtI3×3]x¨c,k︸uk,k=0,1,…,N−1, fmin≤Λcuk+hc≤fmax,k=0,1,…,N−1, zc,min≤Szxk≤zc,max,k=1,2,…,N−1, x0=x0fb, Sz,z˙xN=Sz,z˙xNref, lc,min≤Λc[−(1+cr)PnSvx˙k]≤lc,max,k=kimp,kimp+1,…,N, (Cl1+1cos θc.maxDn)Svx˙k≤04×1,k=kimp,kimp+1,…,N, where U=[u0T,u1T,…,uN−1T]T∈R3N the vector is which consists of the control variables of the respective frames; QxN∈R6×6, Quk∈R3×3 and Qzk∈R diagonal weight matrices of different dimensions; the foot end state of the current moment (including position and velocity) as x0fb=[xc,0fbx˙c,0fb]∈R6 The final state of the foot end (including position and speed) is reported back. xNref=[xc,Frefx˙c,Fref]∈R6 is used as a reference Sv=[03×3I3×3]∈R3×6 the selection matrix of the linear velocity of the foot end is; k h1 and k h2 The user-defined start and end frames for maintaining the swing heights are and 0 < k h1 ≤ k h2 < N fulfill; k imp The starting frame for activating collision awareness restrictions and final speed direction restrictions is and 0 ≤ k imp ≤ N is satisfied.

[0012] Optionally, the optimization problem for whole-body control is: minχ∑i=1ntask‖Wi(Aiχ−bi)‖22, st lbj≤Cjχ≤ubj,j=1,2,…,nconstraint, where A i the task matrix is, b i the task vector is C j the constraint matrix is, lb j and ub j the lower and upper limits of the restrictions are, W i the weight matrix, n task the number of tasks and n constraint the number of restrictions.

[0013] Optionally, the tasks processed concurrently by the whole-body control include several of a torso trajectory tracking task, a foot end trajectory tracking task, a sole force tracking task, a joint torque change minimization task, and a sole force change minimization task; wherein the constraints processed concurrently by the collision-aware whole-body control include several of a floating base dynamic equation constraint, a sole force inequality constraint, a joint output torque saturation inequality constraint, a joint speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0014] Optionally, performing whole-body control of the leg robot based on the motion state, reference state sequence, and foot-end reference state includes the following: if the leg is planned as a swing leg, setting the foot-end trajectory tracking task to track the collision-aware swing leg trajectory, setting the target sole force of the sole force tracking task to a preset value, setting the sole force constraint in the sole force inequality constraint to a preset value, and disabling the collision awareness constraint;If the leg is designated as a support leg in the design and the support leg is in contact with the ground, set the target linear acceleration of the foot end trajectory tracking task to a preset value, set the target sole force of the sole force tracking task to a sole force provided by MPC (Model Predictive Control) or other modules, set the sole force limit to the friction cone limit, and disable the collision awareness limit; if the leg is designated as a support leg in the design and the support leg is not in contact with the ground, set the target of the foot end trajectory tracking task to track a velocity in the direction of the impact surface, set the target sole force of the sole force tracking task to a preset value, set the sole force limit to a preset value, and enable the collision awareness limit.

[0015] Optionally, the whole-body dynamics model is: M(qg)q¨+h(qg,q˙)=[06×1τj]+JCT(qg)Fc, where the general M(qg)∈Rnq×nq my mass matrix is, h(qg,q˙)∈Rnq a term that includes Coriolis, centripetal, and gravitational forces, τj∈Rnj represents the output torques of the drive joints; 0 n×m represents the zero matrix with size n×m; JcT(qg) and F c each represents the extended Jacobian matrix formed by stacking the contact Jacobian matrices of each supporting leg, and the extended sole force formed by stacking the ground reaction forces on the sole of each supporting leg; q gthe generalized joint space position, q the generalized joint space velocity, and q̈ the generalized joint space acceleration; and where the collision awareness constraint matrix and vector are expressed as follows: C6,l=[−δt(1+cr)Λc,l(qgfb)PnJc,l(qgfb)03×nF]∈R3×nχ, lb6=lc,l,min+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, ub6=lc,l,max+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, where lc,l,min,lc,l,max∈R3 where are the lower and upper limits of the permissible collision pulses specified by the user, and δt represents a time interval between the current moment and the next moment.

[0016] An embodiment of the second aspect of the present application provides a device for planning and controlling a leg robot, comprising: a detection module for detecting a motion instruction and a motion state of the leg robot; a generation module for generating a sequence of reference states of the leg robot and a swing parameter of each leg at the next moment based on the motion instruction and the motion state; a planning module used to determine a foot-end reference state of each leg based on the motion state and the swing parameter of the leg at the next moment, and to plan a collision-aware swing leg trajectory based on the foot-end dynamic model and the discrete collision model for the leg that will begin to swing at the next moment, such that the effects of the premature collision are within the target constraint range;a control module used to perform whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model, such that the effects after a delayed collision are within the target constraint range.

[0017] Optionally, the foot end dynamic model of the device for planning and controlling a leg robot is: Λc,lx¨c,l+hc,l=Fj, and whereby x¨c,l∈R3 the linear acceleration of the foot end of the first leg in inertial frame I is, ∧ c,l the mass matrix in the operating room is, h c,lthe term that includes Coriolis, centripetal, and gravitational forces in the operating space, and Fj∈R3 the foot-end driving force of the torque τ j of the drive joint according to the figure into the operating space; and where the discrete collision model is: Δx˙c,l=Pn(x˙1+−x˙1−)=−(1+cr)Pnx˙c,l−, and whereby Pn=nnT∈R3×3 the projection operator on the collision normal vector is, n∈R3 the unit normal vector of the collision surface; x˙1+,x˙1−∈R3 The speeds of object 1 before and after the collision are as follows: x˙2+,x˙2−∈R3 The speeds of object 2 before and after the collision are, respectively, and c r The coefficient of restitution is.

[0018] Optionally, the planning module is further used for the following purposes: capturing a predefined state variable and target constraints; constructing a continuous-time trajectory optimization problem for a leg that will begin to swing in the next moment, based on the foot-end dynamics model, the predefined state variable, and the target constraints; discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot end of the swinging leg, and solving the problem to obtain a collision-aware swinging leg trajectory.

[0019] Optionally, the state variables predefined by the planning module include one or more of an initial state of the foot end of the swing leg, a landing state of the foot end of the swing leg, a duration of the swing, and a desired maximum height of the foot end of the swing leg above the ground, wherein the target constraints include one or more of a state equation constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equation constraint, a final state equation constraint, a collision awareness inequality constraint, and a final velocity direction inequality constraint.

[0020] Optionally, the objective function of the continuous-time trajectory optimization problem of the planning module is: minx(t),u(t)fTO=‖x(tF)−[xc,Frefx˙c,Fref]‖QxF+∫t0tF‖u(τ)‖Qudτ+∫th1th2‖zc(τ)−zhref‖Qzdτ, where QxF∈R6×6,Qu∈R3×3 and Qz∈R Weight matrices with different dimensions are (diagonal matrices are predominant); ||a|| Q = a T Qa represents the weighted modulus length of the vector a under the weight matrix Q; ‖x(tF)−[xc,Frefx˙c,Fref]‖QxF reflects the function of the landing position of the foot end of the swinging leg; xc,Fref∈R3 and x˙c,Fref∈R3 in each case the known endpoint position of the foot end of the swing leg and the linear velocity; ∫t0tF‖u(τ)‖Qudτ The control variable (i.e., the linear acceleration) for minimizing the overall momentum process is t0 = 0, t F = T SW , T SW the swing duration is; ∫th1th2‖zc(τ)−zhref‖Qzdτ The task of determining the maximum height of the swing leg above the ground reflects the two parameters t h1 and t h2 Specify the time required to reach the maximum height and the condition t0 ≤ th1 ≤ t h2 ≤ t F fulfill. z c is the third component of the state variable, which indicates the height of the foot end above the ground; and where the entire discrete-time trajectory optimization problem is: minU fc,TO=‖xN−xNref‖QxN+∑k=0N−1‖uk‖Quk+∑k=kh1kh2‖Szxk−zhref‖Qzk, st[xc,k+1x˙c,k+1]︸xk+1=[I3×3ΔtI3×303×3I3×3]︸A[xc,kx˙c,k]︸xk+[12Δt2I3×3ΔtI3×3]︸Bx¨c,k︸uk,k=0,1,…,N−1, fmin≤Λcuk+hc≤fmax,k=0,1,…,N−1, zc,min≤Szxk≤zc,max,k=1,2,…,N−1, x0=x0fb, Sz,z˙xN=Sz,z˙xNref, lc,min≤Λc[−(1+cr)PnSvx˙k]≤lc,max,k=kimp,kimp+1,…,N, (Cl1+1cos θc,maxDn)Svx˙k≤04×1,k=kimp,kimp+1,…,N, where U=[u0T,u1T,…,uN−1T]T∈R3N the vector is which consists of the control variables of the respective frames; QxN∈R6×6,Quk∈R3×3 and Qzk∈R diagonal weight matrices of different dimensions; the foot end state of the current moment (including position and velocity) as x0fb=[xc,0fbx˙c,0fb]∈R6 The final state of the foot end (including position and speed) is reported back. xNref=[xc,Frefx˙c,Fref]∈R6 is used as a reference Sv=[03×3I3×3]∈R3×6 the selection matrix of the linear velocity of the foot end is; k h1 and k h2 The user-defined start and end frames for maintaining the swing heights are and 0 < k h1 ≤ k h2 < N fulfill; k imp The starting frame for activating collision awareness restrictions and final speed direction restrictions is and 0 ≤ k imp ≤ N is satisfied.

[0021] Optionally, the optimization problem for the whole-body control of the device for planning and controlling a leg robot is: minχ∑i=1ntask‖Wi(Aiχ−bi)‖22, stlbj≤Cjχ≤ubj,j=1,2,…,nconstraint, where A i the task matrix is, b i the task vector is C j the constraint matrix is, lb j and ub j the lower and upper limits of the restrictions are, W i the weight matrix, n task the number of tasks and n constraint the number of restrictions.

[0022] Optionally, the tasks processed concurrently by the whole-body control include several of a torso trajectory tracking task, a foot end trajectory tracking task, a sole force tracking task, a joint torque change minimization task, and a sole force change minimization task; wherein the constraints processed concurrently by the collision-aware whole-body control include several of a floating base dynamic equation constraint, a sole force inequality constraint, a joint output torque saturation inequality constraint, a joint speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0023] Optionally, performing whole-body control of the leg robot based on the motion state, reference state sequence, and foot-end reference state includes the following: if the leg is planned as a swing leg, setting the foot-end trajectory tracking task to track the collision-aware swing leg trajectory, setting the target sole force of the sole force tracking task to a preset value, setting the sole force constraint in the sole force inequality constraint to a preset value, and disabling the collision awareness constraint;If the leg is designed as a support leg and the support leg is in contact with the ground, set the target linear acceleration of the foot end trajectory tracking task to a preset value, set the target sole force of the sole force tracking task to a sole force provided by MPC or other modules, set the sole force limit to the friction cone limit, and disable the collision awareness limit; if the leg is designed as a support leg and the support leg is not in contact with the ground, set the target of the foot end trajectory tracking task to track a velocity in the direction of the impact surface, set the target sole force of the sole force tracking task to a preset value, set the sole force limit to a preset value, and enable the collision awareness limit.

[0024] Optionally, the whole-body dynamics model of the device for planning and controlling a leg robot is: M(qg)q¨+h(qg,q˙)=[06×1τj]+JcT(qg)Fc, where M(qg)∈Rnq×nq the generalized mass matrix is, h(qg,q˙)∈Rnq a term that includes Coriolis, centripetal, and gravitational forces, τj∈Rnj represents the output torques of the drive joints; 0 n×m represents the zero matrix with size n×m; JcT(qg) and F c each represents the extended Jacobian matrix formed by stacking the contact Jacobian matrices of each supporting leg, and the extended sole force formed by stacking the ground reaction forces on the sole of each supporting leg; q gthe generalized joint space position, q the generalized joint space velocity, and q̈ the generalized joint space acceleration; and where the collision awareness constraint matrix and vector are expressed as follows: C6,l=[−δt(1+cr)Λc,l(qgfb)PnJc,l(qgfb) 03×nF]∈R3×nχ, lb6=lc,l,min+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, ub6=lc,l,max+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, where lc,l,min,lc,l,max∈R3 The lower and upper limits of the permissible collision pulses specified by the user are, and δt represents a time interval between the current moment and the next moment.

[0025] An embodiment of the third aspect of the present application provides a leg robot comprising a memory, a processor and a computer program that is stored in the memory and executable on the processor, wherein the processor executes the program to implement the method for planning and controlling a leg robot in the above embodiments.

[0026] An embodiment of the fourth aspect of the present application further provides a computer-readable storage medium on which a computer program is stored, wherein the program is executed by the processor to implement the steps of the method for planning and controlling a leg robot in the above embodiments.

[0027] Therefore, the present application has at least the following beneficial effects: The embodiments of the present application can simplify the foot end dynamics model and optimize the trajectory of the discrete collision model, and plan the premature collision problem from within the swing process, so that the effects generated after the premature collision are within the target constraint range; and at the same time, it is possible to perform whole-body control on the whole-body dynamics model and the discrete collision model of the leg robot and to process the delayed collision problem from within the control process, so that the effects generated after the delayed collision are within the target constraint range;Since both premature and delayed collisions are taken into account simultaneously, the embodiments of the present application can reduce the impact at the moment of collision between the sole of the foot and the ground in order to decrease the shock and thus increase the stability of the robot's state of motion and extend the service life of the robot hardware.

[0028] The additional aspects and advantages of the present application are partly specified in the following description, and some will be obvious from the following description or will be understood through the practice of the present application. Brief description of the drawing

[0029] The above and / or additional aspects and advantages of the present application will become obvious and easily understandable from the explanation of the exemplary embodiments in connection with the following drawings. Fig. Figure 1 shows a flowchart of a method for planning and controlling a leg robot in the embodiments of the present application; Fig. Figure 2 shows a schematic diagram of a gait control system of a collision-aware quadruped robot in the embodiments of the present application; Fig. Figure 3 shows a schematic diagram of a decision tree of a collision-aware whole-body control system in the embodiments of the present application; Fig. Figure 4 shows an example diagram of a device for planning and controlling a leg robot in the embodiments of the present application; Fig. Figure 5 shows a schematic diagram of the structure of a leg robot in the embodiments of the present application. Detailed descriptions

[0030] The embodiment of the present application is explained in more detail below. All examples of the embodiment are illustrated in figures, where the reference numerals that are identical or similar from beginning to end denote identical or similar elements or elements with the same or similar function. The embodiments explained in connection with the figures are exemplary, serve to explain the present application, and cannot be understood as limiting the present application.

[0031] In recent years, leg robots have developed rapidly, with increasingly sophisticated and stable methods being applied to their motion planning and control. The leg robots developed by many research institutes are no longer limited to static walking but are becoming ever faster, more sensitive, more robust, safer, and more versatile. When a leg robot performs highly dynamic movements, some problems that might be neglected in low-dynamic movements become unavoidable, such as collisions between the soles of the feet and the ground when the robot is walking at high speed. The leg robot's movement is achieved through the constant switching of its supporting legs, and each step requires contact between the soles of the feet and the ground. This constitutes an intentional collision with the environment and a common collision problem when the leg robot interacts with its surroundings.Inappropriate control strategies can lead to violent collisions between the soles of the feet and the ground, which can affect the subsequent movement state of the robot, unbalance the robot, cause it to fall over, or even shorten the lifespan of the hardware or destroy it.

[0032] In the related technology, a solution to the leg robot collision problem can be proposed from the perspectives of hardware and control algorithm.

[0033] Regarding hardware, options include installing flexible foot pads on the robot's feet, installing damping devices such as spring dampers on the robot's drive joints, or using VIA or VSA. VIA and VSA can provide the desired passive mechanical impedance and modify the joint impedance at the mechanical level, allowing the damping effect to be adjusted to the specific requirements. This method of installing a damping device on the robot is often referred to as passive compliance.

[0034] To minimize the negative effects of foot-floor collisions on a robot's condition while walking, the foot-floor contact speed can be kept close to zero during motion planning. When a small or lightweight leg robot walks at a low speed on flat terrain, the controller's performance can typically ensure that the actual trajectory of the swing leg is essentially the same as the planned trajectory, resulting in more effective solutions.

[0035] If the terrain is uneven, a visual sensor, e.g., a camera or a LIDAR, can be used to estimate the terrain, and a foot sole motion trajectory can be planned based on the estimated terrain so that the contact velocity of the foot sole with the ground is close to 0, thus avoiding collision impacts caused by the foot sole touching the ground prematurely or belatedly.

[0036] However, the state-of-the-art methods have certain limitations.

[0037] Regarding hardware, passive compliance can reduce the effects of collisions to some extent, but it also presents several challenges. First, the introduction of these passive flexible materials will affect the robot system's response frequency, and if the control algorithm doesn't account for this effect, the robot may generate unstable vibrations during movement. Second, the damping effect of passive compliance is limited. When the damping material is chosen, its stiffness, damping, and inertial properties are also predetermined, preventing the damping effect from being tailored to specific needs. In the VIA or VSA method, two motors can be used: one independently controlling the mechanical stiffness of the joints, while the other generates the torque.The overall structure is too complex, and the volume and weight are too large, making it difficult to use this type of actuator for leg robots that need to perform highly dynamic movements.

[0038] Regarding the control algorithms, when the leg robot performs highly dynamic movements, such as high-speed running, the actual movement trajectory of the swing leg can obviously deviate from the planned trajectory, which can cause the soles of the feet to touch the ground prematurely or be delayed. If the sole of the foot touches the ground with a delay, it does not yet make contact with the ground, but the plan dictates that it will play a role in supporting the torso, and the leg will strike the ground quickly, resulting in a violent collision that causes an enormous jolt of force from the sole of the foot at the moment of impact. In heavy leg robots, the jolt force caused by the collision is often even greater.When the leg robot runs at high speed, the violent impact can cause the zero position of the joints to shift, and the frequent collisions of the soles of the feet can accelerate the aging of the hardware.

[0039] In summary, the control strategy in the relevant technology treats the impact force during the collision as a disturbance and focuses on the balance control of the robot after the collision, while neglecting the effects of such frequent and intentional collisions on the lifetime of the robot hardware.

[0040] With regard to the problems in the prior art described above, the present application provides a method for planning and controlling a leg robot. With reference to the accompanying drawings, a method and a device for planning and controlling a leg robot, a robot, and a storage medium in the embodiments of the present application are explained in more detail below.

[0041] More precisely, it shows Fig. 1 a flowchart of a method for planning and controlling a leg robot in the embodiments of the present application; as in Fig. As shown in Figure 1, the procedure for planning and controlling a leg robot comprises the following steps: Step S101: Capturing a movement instruction and a movement state of the leg robot.

[0042] The drive instruction can include a movement speed instruction, a direction instruction, etc., which is entered by the user, and the movement state can include the state of the robot's torso, the state of the foot end of each leg, and the state of each leg touching, etc.

[0043] It is understood that the embodiments of the present application can use at least one method to obtain the motion instructions and motion states of the leg robot; the actuators of the leg robot in the embodiments of the present application are force-controlling motors, and their sensors include an IMU (Inertial Measurement Unit), a motor angle scale, a motor torque sensor, etc. The following embodiments will explain this on that basis.

[0044] For example, it shows Fig. 2. A block diagram of a gait control system of a collision-aware quadrupedal robot in the embodiments of the present application, and the block diagram is applicable in both simulation and reality. The entire control system consists of five modules, namely State Estimator, Rough Planning, SRB-MPC (Single Rigid Body Model Predictive Control), IA-TO (Impact-aware Trajectory Optimization), and IA-WBC (Impact-aware Whole-body Control). The system operates at a higher frequency (e.g., 500 Hz, 1 kHz, etc.), with IA-WBC operating at the same frequency and SRB-MPC at approximately 40 Hz, and the optimization problem in IA-TO being calculated only once before each leg swing. The above system is used as an example in the following embodiments.

[0045] In particular, the embodiment of the present application, as shown in Fig. 2 shown, estimate the torso state of the leg robot based on the read IMU data and motor state information, wherein the IMU data may include the 3D Euler angle, 3D angular velocity, and 3D linear acceleration, etc., and wherein the motor state information may include the motor rotation angle, rotational speed, and feedback torque, etc., and wherein the torso state of the leg robot may include the point position of the torso, the unit of attitude quaternion, the linear velocity, and the angular velocity, etc., and wherein the state of the foot end of each leg may include the point position and the linear velocity; the embodiment of the present application may use an EKF (Extended Kalman Filter) to estimate the torso state without any specific limitation.

[0046] The state estimation of the embodiment of the present application can transmit relevant information according to different requirements, as described in Fig. As shown in Figure 2, for SRB-MPC only the torso state is transferred; for IA-TO the torso state, the state of the foot end of each leg and the state of the joints are transferred; for IA-WBC the torso state, the state of the foot end of each leg, the state of the joints and the state of contact of each leg are transferred; for the rough planning module the torso state and the state of the foot end of each leg can be transferred; the above transfer process is described in the following embodiment and is not repeated here.

[0047] Step S102: Generating a reference state sequence of the leg robot and a swing parameter of each leg in the next moment based on the movement instruction and the movement state;

[0048] The reference state sequence can be a series of reference quantities from the current moment to a future time period; the reference state sequence can include a trunk reference state sequence, a reference position sequence of the foot end of the supporting leg, a reference contact state sequence, and an MPC reference frame interval sequence, etc.; the swing parameters include the initial state of the foot end of the swinging leg (the initial point position and linear velocity), the touchdown state (the point position and linear velocity at the time of touchdown), the duration of the swing, and the desired maximum height of the foot end of the swinging leg above the ground.

[0049] It is understood that the embodiment of the present application can use the captured motion instructions and motion states to generate a reference state sequence of the leg robot and a swing parameter for the next moment of each leg; the embodiment of the present application can use at least one way to generate the reference state sequence and the swing parameter for the next moment, for example, to perform motion planning using the captured information.

[0050] In particular, the rough planning module, as described in Fig. As shown in Figure 2, the rough planning module performs a rough motion plan for the quadruped robot based on feedback from the state estimation and the motion speed instructions (including forward and reverse speed, lateral speed) and direction instructions (i.e., yaw rate) entered by the user (e.g., via the remote control). The rough planning module must provide the SRB-MPC with a reference state sequence, the number of reference state sequences corresponding to the number of frames predicted by the SRB-MPC. The rough planning module must also provide the IA-TO with the position of the next leg swing's touchdown point, the swing duration, and the current reference contact state.

[0051] Step S103: Determine a foot-end reference state of each leg based on the motion state and swing parameter of the leg at the next moment, plan a collision-aware swing leg trajectory based on the foot-end dynamics model and the discrete collision model for the leg that will begin to swing at the next moment, such that the effects of premature collision are within the target constraint range.

[0052] It is understood that the embodiment of the present application can determine the reference state of the foot end of each leg based on the detected motion state and the swing parameter of each leg of the leg robot in the next moment; if there is a leg that will start to swing in the next moment, collision-aware planning for the trajectory of the swing leg is carried out using the foot end dynamics model and the discrete collision model, so that the effects of the premature collision during the movement of the swing leg are within the target constraint area.

[0053] It should be noted that due to slight ground undulation, state estimation errors, and control errors, the foot end of a leg robot's swing leg may touch the ground before the planned landing time, resulting in an early collision; and this unintended early contact often leads to an unintended collision impact. This problem can indeed be solved by planning a collision-aware motion trajectory for the foot end of the swing leg, such that even in the event of a premature collision, the impact remains within the expected range. Thus, the embodiment of the present application can construct a trajectory optimization problem applicable to the swing leg of a leg robot, wherein the robot dynamics model used is a linear dynamics model of the foot end of the swing leg in the operating space, and the discrete collision model is a discrete collision dynamics model.In this trajectory optimization problem, the prediction time window depends on the planned swing time of the foot end, which is predetermined. To enable the computer on the robot to perform the trajectory optimization online, the corresponding optimization problem is designed as an easy-to-solve quadratic programming (QP) problem.

[0054] In particular, the rough planning module can provide the IA-TO with the position of the next leg's swing point, the swing duration, and the current reference contact state, as shown in Fig. Figure 2 shows that the IA-TO module receives feedback information from the state estimation and reference information provided by the rough plan and generates the corresponding reference state (including the reference point position and linear velocity) for the foot end of each leg. The SRB-MPC module can solve an optimization problem in model predictive control based on the reference information provided by the rough plan and obtain the optimal trunk reference state and optimal reference sole force, which are passed directly to the IA-WBC. The SRB-MPC module can solve the optimization problem in approximately any frame, and if the problem does not need to be solved, its output is not updated; that is, the output data is kept at order zero.

[0055] For the supporting leg, the reference state is to remain on the ground; for the leg that is just beginning to swing, the collision-aware trajectory optimization problem is solved; for the leg that is already swinging, the optimal solution of the trajectory optimization is interpolated to obtain the reference state at the current time. The IA-TO module solves the trajectory optimization only once, before a leg needs to swing, while the computation time for the remaining time is minimal; the IA-TO module finally transfers the reference state of the foot end of each leg to the IA-WBC, along with the reference contact state provided by the rough planning.

[0056] In the embodiment of the present application, planning a collision-aware swing leg trajectory based on the foot-end dynamics model and the discrete collision model comprises the following: acquiring a predefined state variable and target constraints; constructing a continuous-time trajectory optimization problem for a leg that will begin to swing in the next moment, based on the foot-end dynamics model, the predefined state variable, and the target constraints; discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot end of the swing leg; and solving the problem to obtain a collision-aware swing leg trajectory.

[0057] Predefined parameters may include: an initial state of the foot end of the swinging leg, comprising an initial point position and a linear velocity; a landing state of the foot end of the swinging leg, comprising a point position and a linear velocity at the moment of landing; a duration of the swing. TSW∈R; and a desired maximum height of the foot end of the swinging leg above the ground zhref∈R.

[0058] The predefined state variables in the embodiment of the present application comprise one or more of an initial state of the foot end of the swing leg, a landing state of the foot end of the swing leg, a duration of the swing, and a desired maximum height of the foot end of the swing leg above the ground, wherein the target constraints comprise one or more of a state equation constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equation constraint, a final state equation constraint, a collision awareness inequality constraint, and a final velocity direction inequality constraint.

[0059] It is understood that each leg must solve a trajectory optimization problem to obtain the optimal swing trajectory for the foot end before it begins to swing. Given the limited computing power of the robot, the trajectory optimization problem for the same leg is not solved repeatedly during the swing of the foot end. Since the expression of the trajectory optimization problem is not substantially different for each leg, in the absence of any ambiguity, in the embodiment of the present application, all indices 1 used to denote the sequential number of the leg are omitted.

[0060] In particular, in the embodiment of the present application, the linear acceleration of the foot end of the swing leg can be used as a control variable, i.e. u=x¨c∈R3, and the point position and the linear velocity of the foot end are selected as state variables, i.e. x=[xcTx˙cT]T∈R6; 2 6 ; on this basis, the objective function, the constraints and the discrete-time trajectory optimization problem of the embodiment of the present application are described as follows: I. Objective Function

[0061] In the embodiment of the present application, the foot end dynamic model is as follows: Λc,lx¨c,l+hc,l=Fj, where x¨c,l∈R3 the linear acceleration of the foot end of the first leg in inertial frame I is, ∧ c,l the mass matrix in the operating room is, h c,l the term that includes Coriolis, centripetal, and gravitational forces in the operating space, and Fj∈R3 the foot-end driving force of the torque τ jof the drive joint according to the illustration into the operating space; in the embodiment of the present application, the process for obtaining the foot end dynamic model can in particular be as follows:

[0062] In the embodiment of the present application, the point position, the linear velocity and the linear acceleration of the foot end of the first leg in the inertial system I can each be described as xc,l,x˙c,l,x¨c,l∈R3 If set, then the dynamic equation of the foot end in the operating space is: Λc,l(qg)x¨c,l+hc,l(qg,q˙)=Fj+Fc,l, are Fj,Fc,l∈R3 the foot-end driving force of the torque τ j of the drive joint according to the illustration into the operating space and the reaction force of the ground on the foot end (which only occurs upon contact with the ground); ∧ c,l (q g ) is the mass matrix in the operating space, and h c,l (q g, q) is the term that includes Coriolis, centripetal and gravitational forces in the operating space.

[0063] Given that the swing leg of the leg robot is usually located in a certain area below the torso, even when it is in dynamic motion, it can be assumed in the embodiment of the present application that the configuration of the leg does not change over a particularly large area during a single swing, and therefore ∧ c,l (q g ) can be considered approximately independent of the configuration and as a constant matrix; experiments have proven that the change in the nonlinear term h c,l (q g , q̇) is actually not very large in the process of leg swinging and can also be approximated as a vector of constants.

[0064] This results in the simplified foot end dynamic model: Λc,lx¨c,l+hc,l=Fj,

[0065] The sole of the first leg is considered object 1 and the ground object 2, and taking into account that the mass of the ground is much greater than the mass of the robot and the velocity of the ground before and after the collision is zero, the discrete collision model of the embodiment of the present application can be obtained as follows: Δx˙c,l=Pn(x˙1+−x˙1−)=−(1+cr)Pnx˙c,l−, where Pn=nnT∈R3×3 the projection operator on the collision normal vector is, n∈R3 the unit normal vector of the collision surface; x˙1+,x˙1−∈R3 The speeds of object 1 before and after the collision are as follows: x˙2+,x˙2−∈R3 The speeds of object 2 before and after the collision are, respectively, and c r the restitution coefficient, which is considered Cr=def−nT(x˙2+−x˙1+)nT(x˙2−−x˙1−) is defined.

[0066] In the embodiment of the present application, the objective function of the continuous-time trajectory optimization problem is: minx(t),u(t)fTO=‖x(tF)−[xc,Frefx˙c,Fref]‖QxF+∫t0tF‖u(τ)‖Qudτ+∫th1th2‖zc(τ)−zhref‖Qzdτ, where QxF∈R6×6,Qu∈R3×3 and Qz∈R Q x Weight matrices with different dimensions are (diagonal matrices are predominant); ||a|| Q = a T Qa represents the weighted modulus length of the vector a under the weight matrix Q; ‖x(tF)−[xc,Frefx˙c,Fref]‖QxF reflects the function of the landing position of the foot end of the swinging leg; xc,Fref∈R3 and x˙c,Fref∈R3 in each case the known endpoint position of the foot end of the swing leg and the linear velocity; ∫t0tF‖u(τ)‖Qudτ The control variable (i.e., the linear acceleration) for minimizing the overall momentum process is t0 = 0, t F = T SW , T SW the swing duration is; ∫th1th2‖zc(τ)−zhref‖Qzdτ The task of determining the maximum height of the swing leg above the ground reflects the two parameters t h1 and t h2 Specify the time required to reach the maximum height and the condition t0 ≤ t h1 ≤ t h2 ≤ t F fulfill. z c is the third component of the state variable, which indicates the height of the foot end above the ground; II. Restrictions:

[0067] (1) State equation restriction: the control variable is chosen as the linear acceleration of the foot end, so that the state equation is very easy to express; if Ac=[03×3I3×303×303×3],Bc=[03×3I3×3], The equation of state of the system can be expressed as follows: ddtx(t)=Acx(t)+Bcu(t),for t∈[t0,tF].

[0068] (2) Drive force limitation: since there is a lower limit and an upper limit to the output torque of the actual actuator, which in the operating range of the lower limit fmin∈R3 and the upper limit fmax∈R3 The driving force corresponds to the foot end; therefore, the driving force limitation can be expressed as follows: fmin≤Λcu(t)+hc≤fmax,for t∈[t0,tF].

[0069] (3) Trajectory height limitation: to avoid the optimized trajectory being below ground level or above the desired maximum height above ground level, the height of the trajectory must be limited, i.e. zc,min≤Szx(t)≤zc,max,for t∈[t0,tF], where zc,min,zc,max∈R the upper and lower limits of the trajectory height; Sz=[0,0,1,0,0,0]∈R1×6 The selection matrix is ​​used to select the third component of the state.

[0070] (4) Initial state constraint: the initial state in the trajectory optimization of the foot end of the swing leg should be the state of the foot end at the moment before it leaves the ground; therefore, the initial state constraint is: x(t0)=[xc,0fbx˙c,0fb], where xc,0fb,x˙c,0fb∈R3 The position of the last feedback point of the foot end of the swing leg before the solution of the trajectory optimization problem and the linear velocity are, respectively.

[0071] (5) Final state constraint: although the objective function embodies the task of the final state, it cannot guarantee that the end of the optimized foot-end trajectory has a height of zero above the ground, i.e., it may cause the end of the trajectory to be suspended in the air or stuck in the ground, so a final state constraint is necessary. Sz,z˙x(tF)=[Zc,FrefZ˙c,Fref], where zc,Fref∈R□ and z˙c,Fref∈R□ where each is the reference height of the end of the trajectory and the reference velocity in the vertical direction, and where z˙c,Fref=0; and where S z,ż the corresponding selection matrix is, namely Sz,z˙=[001000000001],

[0072] It should be noted that the final state constraint in the embodiment of the present application does not concern itself with the component of the state in the horizontal direction, since the desired final state in the horizontal direction may not be attainable in the case of more constraints and enforcing the final state constraint in the horizontal direction may result in the optimization problem being left unsolved.

[0073] (6) Collision awareness limitation: in order to allow the swing leg to decelerate to a sufficiently safe speed before touchdown, the embodiment of the present application can take into account the collision awareness limitation at the end of the trajectory: assuming that the collision process from the beginning of the c -moments until the end of the t c + δt -moment lasts, and by integrating both sides of the equals sign for ∧ c,l (qg )ẍ c,l + h c,l (q g ,q̇) = F j + F c,l , surrendered: ∫tctc+δtΛc(qg)x¨c+hc(qg,q˙)dt=∫tctc+δtFj+Fcdt, since the robot configuration is virtually unchanged both before and after the collision, i.e. qg+=qg−, and the collision process is very short, i.e., δt → 0, the momenta of finite forces (e.g., gravity, Coriolis force, centripetal force, etc.) vanish at this limit, so that only the momenta generated by the collision remain; thus, as δt → 0, the above equation becomes: ΛcΔx˙c=Λc(x˙c+−x˙c−)=lc, where lc∈R3 denotes the momentum of the ground on the foot end; this equation is introduced into the premature collision model, and the collision awareness constraint can be obtained: lc,min≤Λc(−(1+cr)Pnx˙c(t))≤lc,max,for t∈[timp,tF], where lc,min,lc,max∈R3 The permissible lower and upper limits of the collision pulse are selected by the user, and where t imp The user-selected time to activate collision awareness limitation is t0 ≤ t imp ≤ t F fulfilled.

[0074] It is to be understood that the collision awareness limitation equation can express that if a premature collision occurs for any reason during the time interval before the foot end of the swing leg is about to fall to the ground, i.e., during the time interval of [t imp , t F ], the momentum caused by the collision should lie within the constraint of the formula; however, the constraint cannot be switched on for the entire duration of the swing, as otherwise the entire swing process will be slow and normal running motion cannot be achieved.

[0075] (7) Speed ​​direction limitation of the end piece: if the speed direction of the foot end of the swing leg is close to the horizontal before it falls to the ground, the error in the height control of the foot end may cause the sole of the foot to touch the ground prematurely, resulting in the foot end of the swing leg not falling at the intended landing point and potentially affecting the stability of the robot's center of gravity. Therefore, the embodiment of the present application can design the speed direction limitation of the end piece of the foot end such that the angle θ c between the speed ẋ c (t) of the swing leg before contact with the ground and the unit vector -n of the collision surface (the minus sign corresponds to the direction inwards) lie in a certain area, i.e.: cos θc,max≤−nTx˙c(t)‖x˙c(t)‖2,for t∈[timp,tF], this is θc,max∈(0,π2) The user-specified maximum permissible angle; the equation above is a non-linear equation that can be transformed into a linear equation by scaling; from the relationship between the ℓ1 standard value and the ℓ2 standard value of the vector, the following results: −nTx˙c(t)‖x˙c(t)‖1≤−nTx˙c(t)‖x˙c(t)‖2,∀x˙c(t)≠03×1and x˙c(t)∈R3, This results in the rewritten equation cos θc,max≤−nTx˙c(t)‖x˙c(t)‖2,for t∈[timp,tF], All solutions that satisfy this rewritten equation also satisfy the original equation, so the rewritten equation can be used as the velocity direction constraint of the end piece for TO and can be further expressed as follows: |x˙c,x(t)|+|x˙c,y(t)|+|x˙c,z(t)|≤−1cos θc,maxnTx˙c(t),for t∈[timp,tF],

[0076] This includes ẋ c,x (t),ẋ c,y (t),ẋ c,z (t) the individual components of xc (t). By iterating through all cases within the absolute sign, the above equation can be further rewritten into eight equivalent inequalities, namely [111−1111−11−1−1111−1−11−11−1−1−1−1−1]x˙c(t)≤−1cos θc,max[nTnTnTnTnTnTnTnT]x˙c(t),for t∈[timp,tF],

[0077] However, since shortly before the swing leg touches down, ẋ c,z If (t) ≤ 0, the first four lines of this equation are usually superfluous, so that the velocity direction restriction of the end piece can ultimately be simplified to: (Cl1+1cos θc.maxDn)x˙c(t)≤04×1,for t∈[timp,tF], where Cl1=[11−1−11−11−1−1−1−1−1],Dn=[nTnTnTnT] III. Discrete-time trajectory optimization problem

[0078] It is understood that, in order to facilitate solving the above continuous-time trajectory optimization problem on the computer, the embodiment of the present application can also perform discretization; in doing so, the embodiment of the present application can use at least one discretization method, such as replacing the integral term of the objective function with a summation with finite terms and replacing the constraints of the state equation with a Taylor extension formula that ignores the higher-order terms, and so on.

[0079] In the embodiment of the present application, the swing process is uniformly divided into N segments according to time, i.e., there are N frames, and the frame interval is Δt = T sw / N. Be t k = t0 + k(t F - t0) / N, k = 1, 2, ..., N, then it can be abbreviated as x k = x(t k ),u k = u(t k ); and the entire discrete-time trajectory optimization problem can be obtained: minU fc,TO=‖xN−xNref‖QxN+∑k=0N−1‖uk‖Quk+∑k=kh1kh2‖Szxk−zhref‖Qzk, st[xc,k+1x˙c,k+1]︸xk+1=[I3×3ΔtI3×303×3I3×3]︸A[xc,kx˙c,k]︸xk+[12Δt2I3×3ΔtI3×3]︸Bx¨c,k︸uk,k=0,1,…,N−1, fmin≤Λcuk+hc≤fmax,k=0,1,…,N−1, zc,min≤Szxk≤zc,max,k=1,2,…,N−1, x0=x0fb, Sz,z˙xN=Sz,z˙xNref, lc,min≤Λc[−(1+cr)PnSvx˙k]≤lc,max,k=kimp,kimp+1,…,N, (Cl1+1cos θc,maxDn)Svx˙k≤04×1,k=kimp,kimp+1,…,N, where U=[u0T,u1T,…,uN−1T]T∈R3N the vector is which consists of the control variables of the respective frames; QxN∈R6×6,Quk∈R3×3 and Qzk∈R diagonal weight matrices of different dimensions; the foot end state of the current moment (including position and velocity) as x0fb=[xc,0fbx˙c,0fb]∈R6 The final state of the foot end (including position and speed) is reported back. xNfb=[xc,Frefx˙c,Fref]∈R6 is used as a reference Sv=[03×3I3×3]∈R3×6 the selection matrix of the linear velocity of the foot end is; k h1 and k h2 The user-defined start and end frames for maintaining the swing heights are and 0 < k h1 ≤ k h2 < N fulfill; k imp The starting frame for activating collision awareness restrictions and final speed direction restrictions is and 0 ≤ k imp ≤ N is satisfied.

[0080] It is to be understood that, since many trajectory optimization problems in research are highly nonlinear and usually non-convex optimization problems, the use of some professional nonlinear solvers can find the local optimal solution of these trajectory optimization problems, but cannot guarantee finding the global optimal solution; the trajectory optimization problem in the embodiment of the present application is a QP problem, which belongs to the typical convex optimization problems, so that, as long as there is a non-empty solution set, the optimal solution found by the optimizer must be the global optimal solution.

[0081] The objective function of the aforementioned discrete-time trajectory optimization problem for the foot end of the swing leg can be solved using a number of QP solvers. For the embodiment of the present application, for example, the C++-based open-source library qpOASES can be used, with which the QP problem must be transformed into standard form: Umin12UTHU+gTU, st lb≤CU≤ub,

[0082] After finding the optimal solution to the above equation, only the optimal control variable U* of the N frames is obtained, which is also needed to determine the optimal position and velocity trajectories. The above N rows of the discrete dynamics equations are written as linear expressions with respect to the initial state and the optimization variables, i.e.: X=AQPx0fb+BQPU where X=[x0T,x1T,...,xNT]T∈R6N, and where A QP and B QPeach are: AQP=[AA2⋮AN]∈R6N×6,BQP=[B06×3⋯06×3ABB⋯06×3⋮⋮⋱⋮AN−1BAN−2B⋯B]∈R6N×3N,

[0083] From this, the corresponding optimal positions and speeds of the N frames can be derived by introducing U* into the above linear expressions.

[0084] Since the subsequent control program may require the corresponding reference trajectory at any time, it is necessary to convert the N discrete points into continuous curves by interpolation; wherein the embodiment of the present application can use at least one way to achieve the interpolation; to maximize computational efficiency, the embodiment of the present application can use the simplest linear interpolation.

[0085] For example, it is assumed that from the moment the swinging leg leaves the ground, the time t at that moment sw has passed, t sw∈ [0, T sw ], and the nearest previous frame is the k-th frame, which is the optimal position xc,k* This corresponds to the following. Using the linear interpolation method, the optimal reference position at this time should be as follows: xc*(tsw)=xc,k*+tsw−kΔtΔtxc,k+1*, and in a similar way the corresponding optimal reference speed can be determined x˙c*(tsw) will be obtained.

[0086] It should be noted that this interpolation method cannot guarantee that the time integral of the velocity curve is the position curve, and that a higher-order interpolation function is required for this; here, a certain degree of interpolation accuracy is sacrificed to maximize computational efficiency.

[0087] Step S104: Performing whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model, so that the effects after a delayed collision are within the target constraint range.

[0088] It is to be understood that, as in Fig. Figure 2 shows that the embodiment of the present application can solve WQP (Weighted Quadratic Programming) and calculate the instruction for the optimal joint torque based on the feedback information of the state estimation, the reference information of the SRB-MPC and the IA-TO module in order to perform whole body control on the leg robot and to address the delayed collision problem.

[0089] It should be noted that a delayed collision means that, due to certain causes (e.g., slight unevenness of the ground, errors in state estimation, and control errors), the foot end has not yet made contact with the ground at the originally planned time, so that contact of the foot end with the ground inevitably occurs later than originally planned. Without a suitable control strategy, such an unexpected delayed collision can easily lead to an unexpected collision impact. The problem of delayed collision differs fundamentally from the problem of premature collision because the delayed collision occurs outside the planned swing process, and neither the distance between the sole of the foot and the ground (which should actually be zero) nor the time of ground contact (which should actually occur precisely) can be accurately determined.An effective solution can only be found within the framework of control. Therefore, the embodiment of the present application can use a collision-aware WBC (Whole Body Control) method based on the core concept of one-step predictive control.

[0090] More precisely, the following embodiments, using a quadruped robot as an example, will explain the WBC method of the embodiment of the present application in more detail. The collision awareness restriction is not constantly activated; rather, the task and the restriction settings of the whole-body control differ depending on the situation. Therefore, the embodiment of the present application can be represented in the form of a decision tree, as shown in Fig. Figure 3 illustrates the collision-aware whole-body control system of the embodiment described in the present disclosure, in particular as follows: I. Whole-body control based on a weighted quadratic plan

[0091] It is understood that the implementation of whole-body control may select an optimization-based method so that the whole-body control can handle inequality constraints such as collision awareness constraints well; given the limited computing resources of a real robot and the multitude of programs to be executed simultaneously (e.g., communication, control algorithms, etc.), a WQP-based whole-body control method can be used in this embodiment of the present application.

[0092] In the embodiment of the present application, the optimization problem for whole-body control is: minχ∑i=1ntask‖Wi(Aiχ−bi)‖22, st lbj≤Cjχ≤ubj,j=1,2,...,nconstraint, where A i the task matrix is, b i the task vector is C jthe constraint matrix is, lb j and ub j the lower and upper limits of the restrictions are, W i the weight matrix, n task the number of tasks and n constraint the number of restrictions.

[0093] It is to be understood that in WQP all tasks to be fulfilled are in the form of A i χ - b i The objective function represents the relative priority of the tasks, and the weighting matrix W represents the relative priority of the tasks. i is adjusted; all restrictions are expressed in lb j ≤ C j χ ≤ ub j expressed, and for equation constraints this can be expressed by lb j = ub j This can be achieved. In the embodiment of the present application, the general joint space acceleration q̈ and the extended sole force F generated by the forces acting on the ends of four legs are used as optimization variables for WBC. c selected, namely χ=[q¨T,FcT]T∈Rnχ, where nχ = n q + n F .

[0094] When the quadruped robot is in place, the whole-body dynamics model of the leg robot in the embodiment of the present application is: M(qg)q¨+h(qg,q˙)=[06×1 τj]+JcT(qg)Fc, where M(qg)∈Rnq×nq the generalized mass matrix is ​​h(q) g , q) h(qg,q˙)∈Rnq a term that includes Coriolis, centripetal, and gravitational forces, τj∈Rnj represents the output torques of the drive joints; 0 n×m represents the zero matrix with size n×m; JcT(qg) and F c each represents the extended Jacobian matrix formed by stacking the contact Jacobian matrices of each supporting leg, and the extended sole force formed by stacking the ground reaction forces on the sole of each supporting leg; qg the generalized joint space position, q̇ the generalized joint space velocity and q̈ the generalized joint space acceleration;

[0095] Solving the QP problem above yields the optimal solution χ*; in combination with the complete dynamic equations, the optimal joint moment index can easily be determined. τjcmd calculate: τjcmd=(SjM(qgfb)−SjJcT(qgfb))χ∗+ Sjh(qgfb,q˙fb),

[0096] This is Sj=[0nj×6Inj×nj]∈Rnj×nq the selection matrix for the drive joints, qgfb and q̇ fb These are the generalized joint space positions and velocity feedback provided by the state estimation. II. Task

[0097] In the embodiment of the present application, the tasks processed simultaneously by the whole-body controller include several of a torso trajectory tracking task, a foot end trajectory tracking task, a sole force tracking task, a joint torque change minimization task, and a sole force change minimization task; wherein the constraints processed simultaneously by the collision-aware whole-body controller include several of a floating base dynamic equation constraint, a sole force inequality constraint, a joint output torque saturation inequality constraint, a joint speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint. In particular, it is as follows:

[0098] (1) Trunk tracking task: The trunk trajectory tracking task is a fundamental and essential task for the movement of a quadrupedal robot. In the embodiment of the present application, a reference trajectory provided by MPC (Model Predictive Control) can be used for trunk movement, which determines the optimal reference point position. rbMPC of the fuselage in inertial system I, the optimal reference linear velocity r˙bMPC the optimal ZYX Euler angle IbMPC and the optimal reference angular velocity BωbMPC of the hull in the floating base system B.

[0099] In WBC, PD control is first performed using the optimal reference trajectory provided by MPC and the feedback states provided by the state estimation to execute the linear acceleration instruction. r¨bcmc of the fuselage in inertial system I and the angular acceleration instruction Bω˙bcmd to be obtained in the floating base system B. Subsequently, appropriate tasks are designed to fulfill these instructions. The PD control is as follows: r¨bcmd=Kp,rb(rbMPC−rbfb)+Kd,rb(r˙bMPC−r˙bfb), Bω˙bcmd=Kp,ωbBeθ+Kd,ωb(BωbMPC−Bωbfb), where Kp,rb,Kd,rb,Kp,ωb,Kd,ωb∈R3×3 The PD reinforcement matrices are all diagonal matrices; Beθ∈R3 is the axis angle, which indicates the positional error and is based on the reference ZYX Euler angle. IbMPC and the feedback ZYX Euler angle Ibfb is calculated.

[0100] Since the first 6 rows of q̈ correspond to the linear acceleration and the angular acceleration of the floating base, the corresponding matrices and vectors for the hull trajectory tracking problem are expressed as follows: A1=[I6×606×(nj+nF)]∈R6×nχ, b1=[r¨bcmdBω˙bcmd]∈R6,

[0101] (2) Foot-end trajectory tracking task: The foot-end trajectory tracking task is also a fundamental task for the movement of a quadrupedal robot and is indispensable. For the foot end of the first leg, the following applies according to the kinematics of the acceleration stage (without rotation): x¨c,l=Jc,l(qg)q¨+J˙c,l(qg)q˙, Therefore, the corresponding matrices and vectors for the subtask of tracking the toes of the first leg can be written as follows: A2,l=[Jc,l(qgfb)03×nF]∈R3×nχ, b2,l=x¨c,lcmd−J˙c,l(qgfb)q˙fb∈R3, l=1,2,3,4, where x¨c,lcmd∈R3 This is the linear acceleration instruction for the foot end of the first leg in inertial frame I. If this leg is in a different state, the instruction is different.

[0102] If this leg serves as a supporting leg, for example, the foot end should be firmly on the ground, namely x¨c,lcmd=03×1; if it is intended to serve as a support leg, but has not yet touched the ground (delayed ground contact), it is x¨c,lcmd a differential control that follows a velocity pointing towards the collision surface: x¨c,lcmd=Kp,xc,l(−vimpn−x˙c,lfb),

[0103] This is vimp∈R the user-specified expected collision speed and n∈R3 The previously mentioned unit vector of the collision surface, pointing outside the collision surface. If this leg acts as the swing leg, x¨c,lcmd Determined by PD control: x¨c,lcmd=Kp,xc,l(xc,lTO−xc,lfb)+Kd,xc,l(x˙c,lTO−x˙c,lfb),

[0104] This includes Kp,xc,l,Kd,xc,l∈R3×3 the PD amplification factor matrices, which are all diagonal matrices, and xc,lTO,x˙c,lTO are the optimal reference point position and linear velocity of the foot end of the swing leg calculated in the above-mentioned embodiments.

[0105] It should be noted that the foot-end trajectory tracking subtask for the four legs in the embodiment of the present application together constitutes the complete foot-end trajectory tracking task. Therefore, the corresponding matrices and vectors can be expressed as follows: A2=[A2,1A2,2A2,3A2,4]∈R12×nχ b2=[b2,1b2,2b2,3b2,4]∈R12

[0106] (3) Foot sole force tracking task: the linear momentum of the center of mass and the angular momentum of the center of mass of a quadruped robot are influenced not only by gravity but also by the reaction force of the ground on the foot end (i.e., the foot sole force); in highly dynamic movements, a suitable foot sole force is very important.

[0107] The embodiment of the present application represents a method for optimizing the reference sole force. FCMPC using a single rigid model MPC, where appropriate tasks for tracking plantar force must be defined in WBC.

[0108] Since the feet of the quadruped robot used in the embodiment of the present application do not have force / torque sensors, it is difficult to implement feedback control of the foot force in the WBC. Therefore, the foot force tracking task can only be defined as a pure feedforward feedback task. The corresponding matrices and vectors can be expressed as follows: A3=[0nF×nq InF×nF]∈RnF×nχ, b3=Fcref∈RnF, where: Fcref=[Fc,1refFc,2refFc,3refFc,4ref].

[0109] If the first leg is already touching the ground and serving as a supporting leg, it should execute the reference sole force provided by MPC; therefore, the following applies: Fc,lref=Fc,lMPC. However, if this leg is the swing leg or is intended as the supporting leg in the plan, but has not yet touched the ground (i.e., delayed ground contact), the foot end of this leg should not be subjected to any external forces; therefore, the following applies: Fc,lref=03×1.

[0110] (4) Joint torque change minimization problem: since the actual bandwidth of the motor force control is limited, excessively large changes in the output torque are not feasible in practice. If the actual motor cannot execute the movement according to the optimal WBC solution, the control error can easily increase. Therefore, in the embodiment of the present application, the "reduction of the change in joint torque" can be taken into account in WBC, so that the actual motor can more easily execute the movement according to the optimal WBC solution.

[0111] Assuming that the optimal joint torque instruction calculated by WBC in the last control cycle τjpre∈Rnj The matrix and vector for the joint moment change minimization task in this control cycle can be expressed as follows: A4=[SjM(qgfb)−SjJcT(qgfb)]∈Rnj×nχ, b4=τjpre−Sjh(qgfb,q˙fb)∈Rnj,

[0112] It should be noted that this task is in potential conflict with the aforementioned torso trajectory tracking task, foot end trajectory tracking task, and sole force tracking task. Therefore, the weighting of this task in the embodiment of the present application must not be too high, as this would otherwise impair the performance of the aforementioned tasks.

[0113] (4) Foot sole force change minimization problem: in order to make the change in the foot sole force smoother (and to make the change in the center of mass smoother), the problem of "reducing the change in the foot sole force" can be taken into account in WBC in the embodiment of the present application.

[0114] The optimal plantar force calculated by WBC in the last control cycle is referred to as Fcpre∈RnF If the parameters are set, then the matrix and vector for the plantar force change minimization task in this control cycle can be expressed as follows: A5=[0nF×nqInF×nF]∈RnF×nχ, b5=Fcpre∈RnF,

[0115] It should be noted that this task is similarly in potential conflict with the previously mentioned trunk trajectory tracking task, foot end trajectory tracking task, and plantar force tracking task. Therefore, the weighting of this task should not be too high, as this would otherwise impair the execution of other tasks. III. Lifting of restrictions

[0116] (1) Dynamic equation constraint of the floating base: the body of a quadruped robot has no direct propulsive force and no torque, therefore there are no joint output torques in the first 6 lines of the complete equation of motion. These six lines are the equations of the floating base dynamics and must satisfy the equation constraint. The corresponding matrices and vectors are: C1=[SbM(qgfb)−SbJcT(qgfb)]∈R6×nχ, lb1=ub1=−Sbh(qgfb,q˙fb)∈R6,

[0117] This is Sb=[I6×606×nj]∈R6×nq the selection matrix of the floating base.

[0118] (2) Inequality restriction of the force on the sole of the foot: in order to avoid relative sliding of the foot end of the supporting leg on the ground, the ratio between the magnitude of the component parallel to the contact surface and the magnitude of the component perpendicular to the contact surface of the force on the sole of the foot should be less than the coefficient of friction, i.e. the force on the sole of the foot F c,l The first leg should meet the following restrictions to prevent slipping: ‖(I3×3−nnT)Fc,l‖2≤μ|nTFc,l|, where µ is the coefficient of friction.

[0119] It is understood that the above equation describes a conical region; therefore, this constraint to prevent slippage of the sole of the foot is also referred to as a friction cone constraint. Since the equation contains the ℓ2 norm and is a nonlinear expression, it can be simplified to facilitate the solution of the WBC optimization problem in the embodiment of the present application.

[0120] In the embodiment of the present application, this equation can first be linearized, i.e., the cone is replaced by a tetrahedron. Secondly, it is assumed that a quadrupedal robot normally walks on a flat surface with a slope close to zero, such that the Z-axis component of F c,l can be considered as a component perpendicular to the contact surface. This results in the following transformation of the above equation: {|Fc,l,x|≤μ|Fc,l,z||Fc,l,y|≤μ|Fc,l,z|,

[0121] It should be noted that the sole force is the reaction force of the ground on the sole of the foot, so its Z-axis component is always greater than zero. To prevent an exceptionally large sole force from damaging the hardware, the embodiment of the present application can also adjust the magnitude of the Z-axis component of F. c,l limit. Thus, the inequality restriction for the plantar force of the first leg can be written as follows: {|Fc,l,x|≤μFc,l,z|Fc,l,y|≤μFc,l,z0≤Fc,l,z≤Fc,l,zmax,

[0122] This is Fc,l,zmax∈R The maximum foot force specified by the user in the Z-direction: if this leg is already the supporting leg, then F c,l,zmaxa previously determined positive number; however, if this leg is a swing leg or should be a supporting leg but has not yet touched down (i.e., if there is a delayed touchdown), the plantar force of this leg should be zero, therefore F c,l,z,max = 0. In the embodiment of the present application, the above equation can also be expressed equivalently in the form of a matrix, namely: 05×1≤CFFc,l≤ubF, The following applies: CF=[10μ−10μ01μ0−1μ001]∈R5×3,ubF=[+∞+∞+∞+∞Fc,l,zmax]∈R5. The plantar force constraints of the four legs together form the complete plantar force inequality constraint. The corresponding matrices and vectors can be expressed as follows: C2=[ CF CF 020×nq CF CF]∈R20×nχ, lb2=020×1,ub2=[ubFubFubFubF]∈R20

[0123] (3) Joint output torque saturation inequality constraint: the actual engine power is limited, so even in the locked state there is an upper limit to the output torque. In WBC, the actual torque limitation of the engine must be taken into account to better coordinate the individual joints and perform several desired tasks simultaneously. Therefore, the matrix and vector of the joint output torque saturation inequality constraint can be expressed as follows: C3=[SjM(qgfb)−SjJcT(qgfb)]∈Rnj×nχ, lb3=τj,min−Sjh(qgfb,q˙fb)∈Rnj, ub3=τj,max−Sjh(qgfb,q˙fb)∈Rnj, Here, τ j,min , τ j,max the lower and upper limits of the joint output torque, respectively.

[0124] (4) Joint speed saturation inequality limitation: analogous to the joint output torque saturation inequality limitation, there is also an upper limit for the actual engine speed under idling conditions. The actual engine speed limitation must be taken into account in WBC to better coordinate the individual joints and perform several desired tasks simultaneously.

[0125] By Taylor expansion of the joint velocity at time t and neglecting the higher terms above the acceleration, the embodiment of the present application can make a linear prediction for q̇. j Perform (t + δt): q˙j(t+δt)≈q˙j(t)+δtq¨j(t),

[0126] Within the same control cycle, the embodiment of the present application can display the last reported joint speed. q˙jfb than the joint speed q̇ j(t) of the current moment and the joint acceleration in the optimization variables as the joint acceleration q̈ j (t) is used for the current moment. Thus, the joint rotational speed at moment t + δt can be expressed linearly using the optimization variables. This results in the following matrices and vectors of the joint rotational speed saturation inequality constraint: C4=[0nj×6Inj×nj0nj×nF]∈Rnj×nχ, lb4=q˙j,min−q˙jfb∈Rnj, ub4=q˙j,max−q˙jfb∈Rnj,

[0127] This includes q̇ j,min , q̇ j,max The lower and upper limits of the joint speed, respectively.

[0128] (5) Joint output power saturation inequality limitation: analogous to the joint output torque saturation inequality limitation, there is also an upper limit for the actual output power of the motor. The actual output power limitation of the motor must be taken into account in WBC to better coordinate the individual joints and to perform several desired tasks simultaneously. Since the environment can also affect the motor, the embodiment of the present application does not limit the input power of the joints, but only the output power of the joints.

[0129] For the α-th joint, the output torque is set to τα∈R and the output speed to q˙α∈R If the output power of this joint is set to P, then the output power of this joint is P. α = τ α q̇ α If the output power of n j joints as Pj∈Rnj If asked, the following applies: Pj=diag(q˙j)τj, where the rotational speed of the joint is determined by q˙jfb can be replaced and the output torque of the joint can be expressed linearly by the optimization variables, so that the corresponding matrices and vectors of the joint output power saturation inequality constraint are as follows: C5=diag(q˙j)[SjM(qgfb)−SjJcT(qgfb)]∈Rnj×nχ, lb5=−∞nj×1, ub5=Pj,max−diag(q˙j)Sjh(qgfb,q˙fb)∈Rnj,

[0130] This is Pj,max∈Rnj the upper limit of the joint output power.

[0131] (6) Collision awareness constraint: the collision awareness constraint in WBC is similar to the collision awareness constraint in TO and uses the same discrete collision model, namely the discrete collision dynamics model.

[0132] It is understood that WBC assumes the collision will occur in the next instant (i.e., at moment t+δt). In the embodiment of the present application, the Taylor expansion can first be used to predict the velocity of the foot end in the instant before the collision, then to predict the sudden change in the velocity of the sole of the foot caused by the collision, then to predict the momentum caused by the collision, and finally to constrain this momentum within the current control cycle. This allows the joint acceleration optimized by WBC to reduce the velocity of the foot end to such an extent that, if the collision actually occurs in the next instant, the momentum caused by the collision will be within the collision awareness range defined by WBC.

[0133] If no collision occurs in the next moment, the robot exhibits the effect of a significant decrease in the movement speed of its foot. More precisely, for the first leg, this means that, using the robot's kinematics and Taylor's expansion formula, the linear velocity... x˙c,l,next− The position of the foot end before the collision at moment t + δt can be predicted linearly using the joint acceleration in the optimization variables: x˙c,l,next−≈x˙c,lfb+δτx¨c,l=Jc,l(qgfb)q˙fb+δt(J˙c,l(qgfb)q˙fb+Jc,l(qgfb)q¨) where the above equation is incorporated into the discrete collision dynamics model and ΛcΔx˙c=Λc(x˙c+−x˙c−)=lc, is introduced so that the sudden change in the sole linear velocity Δẋ caused by the predicted collision c,l,next The joint acceleration can be expressed linearly in the optimization variables: Δx˙c,l,next=−(1+cr)Pnx˙c,l,next−≈−(1+cr)Pn(Jc,l(qgfb)q˙fb+δt(J˙c,l(qgfb)q˙fb+Jc,l(qgfb)q¨)),

[0134] The linear acceleration of the joints in the optimization variables can be used to calculate the predicted collision impulse l c,l,next To represent linearly: lc,l,next=Λc,l(qgfb)Δx˙c,l,next≈−(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δt(J˙c,l(qgfb)q˙fb+Jc,l(qgfb)q¨)).

[0135] This preserves the collision awareness constraint, and the collision awareness constraint matrix and vector are expressed as follows: C6,l=[−δt(1+cr)Λc,l(qgfb)PnJc,l(qgfb) 03×nF]∈R3×nχ, lb6=lc,l,min+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, ub6=lc,l,max+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, where lc,l,min,lc,l,max∈R3 where are the lower and upper limits of the permissible collision pulses specified by the user, and δt represents a time interval between the current moment and the next moment.

[0136] It is understood that this leg does not require a collision awareness restriction if it is planned as a swing leg or has already touched the ground and become a support leg. Conversely, the appropriate collision awareness restriction must be activated for this leg if it is planned as a support leg but has not yet touched the ground (i.e., if there is a delayed touchdown).

[0137] More specifically, performing whole-body control of the leg robot based on the motion state, reference state sequence, and foot-end reference state involves the following: if the leg is planned as a swing leg, setting the foot-end trajectory tracking task to track the collision-aware swing leg trajectory, setting the target sole force of the sole force tracking task to a preset value, setting the sole force constraint in the sole force inequality constraint to a preset value, and disabling the collision awareness constraint;If the leg is designed as a support leg and the support leg is in contact with the ground, set the target linear acceleration of the foot end trajectory tracking task to a preset value, set the target sole force of the sole force tracking task to a sole force provided by MPC or other modules, set the sole force limit to the friction cone limit, and disable the collision awareness limit; if the leg is designed as a support leg and the support leg is not in contact with the ground, set the target of the foot end trajectory tracking task to track a velocity in the direction of the impact surface, set the target sole force of the sole force tracking task to a preset value, set the sole force limit to a preset value, and enable the collision awareness limit.

[0138] The module that can provide the target plantar force for the plantar force tracking task can be the aforementioned MPC module. The default values ​​for the target plantar force of the plantar force tracking task, the plantar force constraint in the inequality constraint, and the target acceleration of the tracking task can be 0. In the embodiment of the present application, this is explained using the default value of 0.

[0139] It should be noted that the embodiment of the present application can also further predict the effects of a collision on the joint space, e.g., by mapping the collision force onto the individual joints in order to predict the effects of the collision on the torque of the joints. However, the effects of a collision on the joint space are generally not very large as long as the momentum force caused by the collision in the operating space is sufficiently small. To maximize the computational efficiency of WBC, the collision awareness restriction in the embodiment of the present application is limited to the operating space, which is consistent with the trajectory optimization method described above. IV. Decision-making for the implementation of whole-body control

[0140] It is to be understood that in WBC, in the embodiment of the present application, there are three invariable tasks, namely the hull trajectory tracking task, the joint torque change minimization task, and the foot sole force change minimization task; there are four invariable constraints, namely the floating base dynamic equation constraint, the motor output torque saturation inequality constraint, the motor output speed saturation inequality constraint, and the motor output power saturation inequality constraint; and the settings for other tasks and constraints vary depending on the situation, which are, in each case, the foot end trajectory tracking task, the foot sole force tracking task, the foot sole force inequality constraint, and the collision awareness constraint.

[0141] It should be pointed out that, as in Fig. Figure 3 illustrates that if a leg is designated as a swing leg in the design and WBC activates the corresponding collision awareness constraint for this leg as a swing leg, two situations can occur: first, the reference linear velocity at the foot end of the leg is very low due to the collision awareness constraint already present in TO; in this case, the optimal solution optimized by WBC is not at the boundary of the corresponding collision awareness constraint, i.e., the corresponding collision awareness constraint in WBC has no effect; the other case is that the optimal solution optimized by WBC lies exactly at the boundary of the corresponding collision awareness constraint, i.e.,The constraint takes effect, causing the movement speed of the foot end of this leg to be even lower than the reference linear velocity specified by TO. Consequently, the foot end of this leg cannot reach the ground at the moment of ground contact specified by TO; that is, a delayed landing occurs. It follows that if a leg is designated as a swing leg in the design and the collision awareness constraint for that leg is enabled in WBC, it will either not take effect or will have a negative impact. Therefore, the collision awareness constraint in WBC is only activated if a delayed landing occurs.

[0142] If one leg is designated as a supporting leg in the planning, but no footfall has yet been detected (i.e., if there is a delayed footfall), the goal of the foot-end trajectory tracking task is to determine a velocity -v impto track n, which points towards the impact surface, where v imp The value should not be set too small, otherwise it will result in a long delay in touching down. However, it should not be taken into account that a setting of v that is too large will also cause problems. imp This leads to an excessively high impact speed, as the collision awareness limitation can restrict the foot end velocity. Each leg of the leg robot in the embodiment of the present application can be, according to the [reference to the application], Fig. The decision tree shown in section 3 is evaluated to express all tasks and constraints, i.e., to determine the expression of WQP.

[0143] In summary, the method for planning and controlling a leg robot in the embodiments of the present application can simplify the foot end dynamics model and optimize the trajectory of the discrete collision model, and plan the premature collision problem from within the swing process, so that the effects generated after the premature collision are within the target constraint range; and at the same time, it is possible to perform whole-body control on the whole-body dynamics model and the discrete collision model of the leg robot and to process the delayed collision problem from within the control process, so that the effects generated after the delayed collision are within the target constraint range;Since both premature and delayed collisions are taken into account simultaneously, the embodiments of the present application can reduce the impact at the moment of collision between the sole of the foot and the ground in order to decrease the shock and thus increase the stability of the robot's state of motion and extend the service life of the robot hardware.

[0144] Next, with reference to the attached drawings, the device provided according to the embodiment of the present application for planning and controlling a leg robot will be explained.

[0145] Fig. Figure 4 shows a block diagram of a device for planning and controlling a leg robot in the embodiments of the present application.

[0146] As in Fig. As shown in Figure 4, the device 10 for planning and controlling a leg robot comprises a detection module 100, a generation module 200, a planning module 300 and a control module 400. wherein the acquisition module 100 is used to acquire a motion instruction and a motion state of the leg robot; and wherein the generation module 200 is used to generate a sequence of reference states of the leg robot and a swing parameter of each leg at the next instant based on the motion instruction and the motion state; and wherein the planning module 300 is used to determine a foot-end reference state of each leg based on the motion state and the swing parameter of the leg at the next instant, and to plan a collision-aware swing leg trajectory based on the foot-end dynamic model and the discrete collision model for the leg that will begin to swing at the next instant, such that the effects of the premature collision are within the target constraint region;and wherein the control module 400 is used to perform whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model, such that the effects after a delayed collision are within the target constraint range.

[0147] In the embodiment of the present application, the foot end dynamic model of the device for planning and controlling a leg robot is: Λc,lx¨c,l+hc,l=Fj, where x¨c,l∈R3 the linear acceleration of the foot end of the first leg in inertial frame I is, ∧ c,l the mass matrix in the operating room is, h c,lthe term that includes Coriolis, centripetal, and gravitational forces in the operating space, and Fj∈R3 the foot-end driving force of the torque τ j of the drive joint according to the figure into the operating space; and where the discrete collision model is: Δx˙c,l=Pn(x˙1+−x˙1−)=−(1+cr)Pnx˙c,l−, where Pn=nnT∈R3×3 the projection operator on the collision normal vector is, n∈R3 the unit normal vector of the collision surface; x˙1+,x˙1−∈R3 The speeds of object 1 before and after the collision are as follows: x˙2+,x˙2−∈R3 The speeds of object 2 before and after the collision are, respectively, and c r The coefficient of restitution is.

[0148] In the embodiment of the present application, the planning module is further used for the following purposes: acquiring a predefined state variable and target constraints; constructing a continuous-time trajectory optimization problem for a leg that will begin to swing in the next moment, based on the foot-end dynamics model, the predefined state variable, and the target constraints; discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot end of the swinging leg; and solving the problem to obtain a collision-aware swinging leg trajectory.

[0149] The predefined state variables of the planning module in the embodiment of the present application comprise one or more of an initial state of the foot end of the swing leg, a landing state of the foot end of the swing leg, a duration of the swing, and a desired maximum height of the foot end of the swing leg above the ground, wherein the target constraints comprise one or more of a state equation constraint, a driving force inequality constraint, a trajectory height inequality constraint, an initial state equation constraint, a final state equation constraint, a collision awareness inequality constraint, and a final velocity direction inequality constraint.

[0150] In the embodiment of the present application, the objective function of the continuous-time trajectory optimization problem of the planning module is: minx(t),u(t)fTO=‖x(tF)−[xc,Frefx˙c,Fref]‖QxF+∫t0tF‖u(τ)‖Qudτ+∫th1th2‖zc(τ)−zhref‖Qzdτ, where QxF∈R6×6,Qu∈R3×3 and Qz∈R Weight matrices with different dimensions are (diagonal matrices are predominant); ||a|| Q = a T Qa represents the weighted modulus length of the vector a under the weight matrix Q; ‖x(tF)−[xc,Frefx˙c,Fref]‖QxF reflects the function of the landing position of the foot end of the swinging leg; xc,Fref∈R3 and x˙c,Fref∈R3 The known position of the foot end of the swing leg and the linear speed are in each case; ∫t0tF‖u(τ)‖Qudτ The control variable (i.e., the linear acceleration) for minimizing the overall momentum process is t0 = 0, t F = T SW , T SW the swing duration is; ∫th1th2‖zc(τ)−zhref‖Qzdτ The task of determining the maximum height of the swing leg above the ground reflects the two parameters t h1 and t h2 Specify the time required to reach the maximum height and the condition t0 ≤ t h1 ≤ t h2 ≤ t F fulfill; z c the third component of the state variable is the height of the foot end above the ground.

[0151] The complete discrete-time trajectory optimization problem is: minU fc,TO=‖xN−xNref‖QxN+∑k=0N−1‖uk‖Quk+∑k=kh1kh2‖Szxk−zhref‖Qzk, st[xc,k+1x˙c,k+1]︸xk+1=[I3×3ΔtI3×303×3I3×3]︸A[xc,kx˙c,k]︸xk+[12Δt2I3×3ΔtI3×3]︸Bx¨c,k︸uk,k=0,1,…,N−1, fmin≤Λcuk+hc≤fmax,k=0,1,…,N−1, zc,min≤Szxk≤zc,max,k=1,2,…,N−1, x0=x0fb, Sz,z˙xN=Sz,z˙xNref, lc,min≤Λc[−(1+cr)PnSvx˙k]≤lc,max,k=kimp,kimp+1,…,N, (Cl1+1cos θc,maxDn)Svx˙k≤04×1,k=kimp,kimp+1,…,N, where U=[u0T,u1T,…,uN−1T]T∈R3N the vector is which consists of the control variables of the respective frames; QxN∈R6×6,Quk∈R3×3 and Qzk∈R diagonal weight matrices of different dimensions; the foot end state of the current moment (including position and velocity) as x0fb=[xc,0fbx˙c,0fb]∈R6 The final state of the foot end (including position and speed) is reported back. xNref=[xc,Frefx˙c,Fref]∈R6 is used as a reference Sv=[03×3I3×3]∈R3×6 the selection matrix of the linear velocity of the foot end is; k h1 and k h2 The user-defined start and end frames for maintaining the swing heights are and 0 < k h1 ≤ k h2 < N fulfill; k imp The starting frame for activating collision awareness restrictions and final speed direction restrictions is and 0 ≤ k imp ≤ N is satisfied.

[0152] In the embodiment of the present application, the optimization problem for the whole-body control of the device for planning and controlling a leg robot is: minχ∑i=1ntask‖Wi(Aiχ−bi)‖22, stlbj≤Cjχ≤ubj,j=1,2,…,nconstraint, where A i the task matrix is, b i the task vector is C j the constraint matrix is, lb j and ub j the lower and upper limits of the restrictions are, W i the weight matrix, n task the number of tasks and n constraint the number of restrictions.

[0153] In the embodiment of the present application, the tasks processed simultaneously by the whole-body controller include several of a torso trajectory tracking task, a foot end trajectory tracking task, a sole force tracking task, a joint torque change minimization task, and a sole force change minimization task; wherein the constraints processed simultaneously by the collision-aware whole-body controller include several of a floating base dynamic equation constraint, a sole force inequality constraint, a joint output torque saturation inequality constraint, a joint speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0154] In the embodiment of the present application, performing whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state comprises the following: if the leg is designed as a swing leg, setting the foot end trajectory tracking task to track the collision-aware swing leg trajectory, setting the target sole force of the sole force tracking task to a preset value, setting the sole force constraint in the sole force inequality constraint to a preset value, and disabling the collision awareness constraint;If the leg is designed as a support leg and the support leg is in contact with the ground, set the target linear acceleration of the foot end trajectory tracking task to a preset value, set the target sole force of the sole force tracking task to a sole force provided by MPC or other modules, set the sole force limit to the friction cone limit, and disable the collision awareness limit; if the leg is designed as a support leg and the support leg is not in contact with the ground, set the target of the foot end trajectory tracking task to track a velocity in the direction of the impact surface, set the target sole force of the sole force tracking task to a preset value, set the sole force limit to a preset value, and enable the collision awareness limit.

[0155] In the embodiment of the present application, the whole-body dynamics model of the device for planning and controlling a leg robot is: M(qg)q¨+h(qg,q˙)=[06×1τj]+JcT(qg)Fc, where M(qg)∈Rnq×nq the generalized mass matrix is, h(qg,q˙)∈Rnq a term that includes Coriolis, centripetal, and gravitational forces, τj∈Rnj represents the output torques of the drive joints; 0 n×m represents the zero matrix with size n×m; JcT(qg) and F c each represents the extended Jacobian matrix formed by stacking the contact Jacobian matrices of each supporting leg, and the extended sole force formed by stacking the ground reaction forces on the sole of each supporting leg; q gthe generalized joint space position, ̇q̇ the generalized joint space velocity, and q̈ the generalized joint space acceleration; and where the collision awareness constraint matrix and vector are expressed as follows: C6,l=[−δt(1+cr)Λc,l(qgfb)PnJc,l(qgfb) 03×nF]∈R3×nχ, lb6=lc,l,min+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, ub6=lc,l,max+(1+cr)Λc,l(qgfb)Pn(Jc,l(qgfb)q˙fb+δtJ˙c,l(qgfb)q˙fb)∈R3, where lc,l,min,lc,l,max∈R3 where are the lower and upper limits of the permissible collision pulses specified by the user, and δt represents a time interval between the current moment and the next moment.

[0156] It should be noted that the preceding explanation of the exemplary embodiment of the method for planning and controlling a leg robot also applies to the device for planning and controlling a leg robot of the exemplary embodiment, which is not repeated here.

[0157] The device for planning and controlling a leg robot provided according to the embodiment of the present application can perform a simplification of the foot end dynamics model and a trajectory optimization of the discrete collision model and plan the premature collision problem from within the swing process, so that the effects generated after the premature collision are within the target constraint range; and at the same time it is possible to perform whole-body control on the whole-body dynamics model and the discrete collision model of the leg robot and to process the delayed collision problem from within the control process, so that the effects generated after the delayed collision are within the target constraint range;Since both premature and delayed collisions are taken into account simultaneously, the embodiments of the present application can reduce the impact at the moment of collision between the sole of the foot and the ground in order to decrease the shock and thus increase the stability of the robot's state of motion and extend the service life of the robot hardware.

[0158] Fig. Figure 5 shows a schematic diagram of the structure of a leg robot in the embodiments of the present application. The leg robot can comprise a memory 501, a processor 502, and computer programs that are stored in the memory 501 and can be executed on the processor 502. The processor 502 executes the program and thereby implements the method for planning and controlling a leg robot provided in the embodiment mentioned above.

[0159] Furthermore, the leg robot includes a communication interface 503 for communication between the memory 501 and the processor 502; a memory 501 for storing computer programs that can be executed on the processor 502; wherein the memory 501 may include a high-speed RAM (Random Access Memory) and may also include non-volatile memory, for example, at least one disk storage.

[0160] If the memory 501, the processor 502, and the communication interface 503 are implemented independently, they can be connected via a bus and communicate with each other. The bus can be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, or an EISA (Extended Industry Standard Architecture) bus. The bus can be classified as an address bus, data bus, control bus, etc. For the sake of simplicity, in Fig. 5 is shown as a thick line, but this does not mean that there is only one bus or one type of bus.

[0161] Optionally, in the specific implementation, if the memory 501, the processor 502 and the communication interface 503 are integrated on one chip, The memory 501, the processor 502 and the communication interface 503 communicate with each other via an internal interface.

[0162] The processor 502 can be a CPU (Central Processing Unit), an ASIC (Application Specific Integrated Circuit), or one or more integrated circuits configured to implement the embodiments of the present application.

[0163] The embodiment of the present application further provides a computer-readable storage medium on which a computer program is stored, wherein, when the program is executed by the processor, the above method for planning and controlling a leg robot is implemented.

[0164] In this description, the explanations used in connection with the technical terms "an embodiment," "some embodiments," "an example," "a specific example," or "some examples" mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In this description, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the described specific features, structures, materials, or characteristics may be combined appropriately in one or more embodiments or examples.In the absence of conflicts, experts in this field may additionally combine and connect the various embodiments or examples explained in this description, or the features in the various embodiments or examples.

[0165] Furthermore, "the first" and "the second" are used only to explain the objective and cannot be understood as indicating or implying relative importance or implicitly referring to the number of required technical features. Therefore, the features defined by "the first" and "the second" can, at the very least, explicitly or implicitly include one of the features.

[0166] In the explanatory notes to this application, “the number n” refers to at least 2, such as 2, 3, etc., unless otherwise specified.

[0167] Any explanation of a process or procedure in the flowcharts or otherwise described herein may be understood as representing a module, segment, or part of code containing one or N executable instructions for implementing a user-defined logical function or step of the process. Furthermore, the scope of the preferred embodiment of the present application includes alternative implementations in which the functions, including those involved, may be executed not in the sequence shown or discussed, but substantially simultaneously or in reverse order, and this should be understood by those skilled in the art in the technical field to which the embodiments of the present application belong.

[0168] It is understood that parts of the present application may be implemented by hardware, software, firmware, or a combination thereof. In the embodiments described above, N steps or procedures may be implemented in software or firmware, which are stored in memory and executed by a suitable instruction execution system. If, for example, it is implemented in hardware as in another embodiment, it may be implemented by one of the following techniques known in the prior art, or a combination thereof: discrete logic circuits with logic gate circuits for implementing logic functions on data signals, application-specific integrated circuits with suitable combinational logic gate circuits, programmable gate arrays, field-programmable gate arrays, etc.

[0169] The average person skilled in this technical field can understand that all or some of the steps in the method of the above embodiments can be realized by instructing the appropriate hardware by a program, the appropriate program being stored in a computer-readable storage medium, and when the program is executed, it containing one or a combination of the steps of the embodiment of the method.

[0170] Although the embodiments of the present application are presented and explained above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present application. The person skilled in the art in this field can make the changes, modifications, substitutions, and variations to the embodiments within the scope of the present application. QUOTES INCLUDED IN THE DESCRIPTION

[0000] This list of documents cited by the applicant was automatically generated and is included solely for the reader's convenience. The list is not part of the German patent or utility model application. The DPMA accepts no liability for any errors or omissions. Cited patent literature

[0000] CN 202311511514.9

[0001]

Claims

A method for planning and controlling a leg robot, characterized in that the method comprises the following steps: acquiring a motion instruction and a motion state of the leg robot; generating a reference state sequence of the leg robot and a swing parameter of each leg in the next instant based on the motion instruction and the motion state; determining a foot-end reference state of each leg based on the motion state and the swing parameter of the leg in the next instant; planning a collision-aware swing leg trajectory based on a foot-end dynamics model and a discrete collision model for the leg that will begin to swing in the next instant, such that the effects of the premature collision are within the target constraint area;and performing whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model, such that the effects after a delayed collision are within the target constraint range. Method for planning and controlling a leg robot according to claim 1, characterized in that the foot end dynamic model is: Λ c , lx ¨ c , l + hc , l = F j , where: x ¨ c , l ∈ R 3 the linear acceleration of the foot end of the l-th leg in inertial frame I is; ∧ c,l the mass matrix in the operating space; h c,l the term that includes Coriolis, centripetal, and gravitational forces in operating space; and F j ∈ R 3 F j the foot-end driving force of the torque τ j of the drive joint according to the illustration into the operating space; and where the discrete collision model is: Δ x ˙ c , l = P n ( x ˙ 1 + − x ˙ 1 − ) = − ( 1 + cr ) P nx ˙ c , l − , where P n = nn T ∈ R 3 × 3 the projection operator on the collision normal vector, n ∈ R 3 the unit normal vector of the collision surface; x ˙ 1 + , x ˙ 1 − ∈ R 3 The velocities of object 1 before and after the collision are, respectively; x ˙ 2 + , x ˙ 2 − ∈ R 3 the speeds of object 2 before and after the collision are respectively; and c r The coefficient of restitution is. A method for planning and controlling a leg robot according to claim 1 or 2, characterized in that the planning of a collision-aware swing leg trajectory based on the foot-end dynamics model and the discrete collision model comprises: acquiring a predefined state variable and target constraints; constructing a continuous-time trajectory optimization problem for a leg that will begin to swing in the next moment, based on the foot-end dynamics model, the predefined state variable, and the target constraints; and discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot end of the swing leg, and solving the problem to obtain a collision-aware swing leg trajectory. Method for planning and controlling a leg robot according to claim 3, characterized in that the predefined state variables comprise one or more of an initial state of the foot end of the swing leg, a landing state of the foot end of the swing leg, a duration of the swing and a desired maximum height of the foot end of the swing leg above the ground, and wherein the target constraints comprise one or more of a state equation constraint, a drive force inequality constraint, a trajectory height inequality constraint, an initial state equation constraint, a final state equation constraint, a collision awareness inequality constraint and a final velocity direction inequality constraint. Method for planning and controlling a leg robot according to claim 3, characterized in that the objective function of the continuous-time trajectory optimization problem is: min x ( t ) , u ( t ) f TO = ‖ x ( t F ) − [ xc , F ref x ˙ c , F ref ] ‖ Q x F + ∫ t 0 t F ‖ u ( τ ) ‖ Q ud τ+ ∫ th 1 th 2 ‖ zc ( τ ) − zh ref ‖ Q zd τ , where: Q x F ∈ R 6 × 6 , Q u ∈ R 3 × 3 and Q z ∈ R Weight matrices with different dimensions are (diagonal matrices are predominant); ||a|| Q = a T Qa represents the weighted modulus length of the vector a under the weight matrix Q; ‖ x ( t F ) − [ xc , F ref x ˙ c , F ref ] ‖ Q x F reflects the function of the landing position of the foot end of the swing leg; xc , F ref ∈ R 3 and x ˙ c , F ref ∈ R 3 where the known position of the foot end of the swing leg and the linear velocity are; ∫ t 0 t F ‖ u ( τ ) ‖ Q ud τ The control variable (i.e., the linear acceleration) is used to minimize the overall momentum process; t 0 = 0, t F = T SW , T SW the swing duration is; ∫ th 1 th 2 ‖ zc ( τ ) − zh ref ‖ Q zd τ The task of determining the maximum height of the swing leg above the ground reflects the two parameters t h1 and t h2 Specify the time required to reach the maximum height and the condition t 0 ≤ t h1 ≤ t h2 ≤ t F fulfill; and z c the third component of the state variable is the height of the foot end above the ground; und wobei das discrete boundary trajectory optimizationproblem: min U fc , TO = ‖ x N − x N ref ‖ Q x N + ∑ k = 0 N − 1 ‖ uk ‖ Q uk + ∑ re zf = x − k ‖ kh zk , s . t . [ xc , k + 1 x ̇ c , k + 1 ] ︸ xk + 1 = [ I 3 × 3 Δ t I 3 × 3 0 3 × 3 I 3 × 3 ] ︸ A [ xc , kx ̇ t , k ] ︸ I 2 xk + Δ [ 1 3 × 3 ] ︸ B x ¨ c , k ︸ uk , k = 0.1, … , N − 1, f min ≤ Λ cuk + hc ≤ f max , k = 0.1, … , N − 1, zc , min ≤ S zxk ≤ zc , max , k = 1.2, … , N − 1, x 0 = x 0 fb , S z , z x N = S z , z x N ref , lc , min ≤ Λ c [ − ( 1 + cr ) P n S vx ̇ k ] ≤ lc , max , k = kemp , kemp + 1, … , N , ( C l 1 + 1 cos θ c , max D n ) S vx ̇ k ≤ 0 4 × 1 , k = kimp , kimp + 1, … , N , wobei: U = [ u 0 T , u 1 T , … , u N − 1 T ] T ∈ R 3 N the vector is that which consists of the control variables of the respective frames; Q x N ∈ R 6 × 6 , Q uk ∈ R 3 × 3 and Q zk ∈ R diagonal weight matrices of different dimensions; the foot-end state of the current moment (including position and velocity) as x 0 fb = [ xc ,0 fb x ˙ c ,0 fb ] ∈ R 6 The final state of the foot end (including position and velocity) x N ref = [ xc , F ref x ˙ c , F ref ] ∈ R 6 is reported back. taken as a reference, S v = [ 0 3 × 3 I 3 × 3 ] ∈ R 3 × 6 the selection matrix of the linear velocity of the foot end is; k h1 and k h2 The user-defined start and end frames for maintaining the swing heights are and 0 < k h1 ≤ k h2 < N fulfill; and k imp The starting frame for activating collision awareness restrictions and final speed direction restrictions is and 0 ≤ k imp ≤ N is satisfied. Method for planning and controlling a leg robot according to claim 1, characterized in that the optimization problem for whole-body control is: min χ ∑ i = 1 n task ‖ W i ( A i χ − bi ) ‖ 2 2 , s. t. lbj ≤ C j χ ≤ ubj , j = 1,2, … , n constraint , where: A i the task matrix is, b i the task vector is; C j the constraint matrix is; lb j and ub j the lower and upper limits of the restrictions are; W i the weight matrix, n task the number of tasks and n constraint the number of restrictions. Method for planning and controlling a leg robot according to claim 1, characterized in that the tasks processed simultaneously by the whole-body controller comprise several of a trunk trajectory tracking task, a foot end trajectory tracking task, a sole force tracking task, a joint torque change minimization task, and a sole force change minimization task, wherein the constraints processed simultaneously by the collision-aware whole-body controller comprise several of a floating base dynamic equation constraint, a sole force inequality constraint, a joint output torque saturation inequality constraint, a joint speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint. A method for planning and controlling a leg robot according to claim 7, characterized in that performing whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state comprises the following: if the leg is provided as a swing leg in the planning, setting the foot end trajectory tracking task to track the collision-aware swing leg trajectory, setting the target sole force of the sole force tracking task to a preset value, setting the sole force constraint in the sole force inequality constraint to a preset value, and disabling the collision awareness constraint;If the leg is designed as a support leg and the support leg is in contact with the ground, set the target linear acceleration of the foot end trajectory tracking task to a preset value, set the target sole force of the sole force tracking task to a sole force provided by MPC or other modules, set the sole force limit to the friction cone limit, and disable the collision awareness limit; if the leg is designed as a support leg and the support leg is not in contact with the ground, set the target of the foot end trajectory tracking task to track a velocity in the direction of the impact surface, set the target sole force of the sole force tracking task to a preset value, set the sole force limit to a preset value, and enable the collision awareness limit. Method for planning and controlling a leg robot according to claim 1, characterized in that the whole-body dynamics model is: M ( qg ) q ¨ + h ( qg , q ˙ ) = [ 0 6 × 1 τ j ] + J c T ( qg ) F c , where: M ( qg ) ∈ R nq × nq the generalized mass matrix is; h ( qg , q ˙ ) ∈ R nq a term that includes Coriolis, centripetal, and gravitational forces; τ j ∈ R nj represents the output torques of the drive joints; 0 n×m represents the zero matrix with size n×m; J c T ( qg ) and F c each represents the extended Jacobian matrix formed by stacking the contact Jacobian matrices of each supporting leg, and the extended sole force formed by stacking the ground reaction forces on the sole of each supporting leg; q G the generalized joint space position, q̇ the generalized joint space velocity and q̈ the generalized joint space acceleration; and where the matrix and vector of the collision awareness constraint are expressed as follows: C 6, l = [ − δ t ( 1 + cr ) Λ c , l ( qg fb ) P n J c , l ( qg fb ) 0 3 × n F ] ∈ R 3 × n χ , lb 6 = lc , l , min + ( 1 + cr ) Λ c , l ( qg fb ) P n ( J ​​c , l ( qg fb ) q ˙ fb + δ t J ˙ c , l ( qg fb ) q ˙ fb ) ∈ R 3 , ub 6 = lc , l , max + ( 1 + cr ) Λ c , l ( qg fb ) P n ( J ​​c , l ( qg fb ) q ˙ fb + δ t J ˙ c , l ( qg fb ) q ˙ fb ) ∈ R 3 , where lc , l , min , lc , l , max ∈ R 3 where are the lower and upper limits of the permissible collision pulses specified by the user, and δt represents a time interval between the current moment and the next moment. Device for planning and controlling a leg robot, characterized in that the device comprises: a detection module for detecting a motion instruction and a motion state of the leg robot; a generation module for generating a sequence of reference states of the leg robot and a swing parameter of each leg in the next moment based on the motion instruction and the motion state; a planning module that is used to determine a foot-end reference state of each leg based on the motion state and the swing parameter of the leg in the next moment, and to plan a collision-aware swing leg trajectory based on a foot-end dynamics model and a discrete collision model for the leg that will begin to swing in the next moment, such that the effects of the premature collision are within the target constraint range;and a control module used to perform whole-body control of the leg robot based on the motion state, the reference state sequence, and the foot end reference state, wherein for the leg that was intended to be a support leg during planning and that did not touch the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model, such that the effects after a delayed collision are within the target constraint area. Leg robot comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the method for planning and controlling a leg robot according to any one of claims 1 to 9. A computer-readable storage medium on which a computer program is stored, characterized in that the program is executed by the processor to implement the steps of the method for planning and controlling a leg robot according to one of claims 1 to 9.

Images