Legged robot planning and control method and apparatus, robot and storage medium

Abstract

A legged robot planning and control method and apparatus, a robot, and a storage medium. The method comprises: according to the motion state of a legged robot and swing parameters of each leg at the next moment, determining a foot-end reference state of each leg; for a leg to swing at the next moment, according to a foot-end dynamics model and a discrete collision model, planning a swing-leg trajectory having collision awareness; and, according to the motion state, a reference state sequence and the foot-end reference states, controlling the whole body of the legged robot, wherein for a leg which should be a planned supporting leg but is not in contact with the ground, whole-body control having collision awareness is performed according to a whole-body dynamics model and the discrete collision model. Thus, the problems are solved that legged robots in the prior art are limited in passive flexible buffering in collision, involves a large amount of control algorithm calculation and relatively low control precision, exhibit a relatively high degree of hardware wear and relatively short service lives, and cannot meet actual needs.

Description

Legged robot planning and control method, device, robot and storage medium

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS

[0002] This application is based on the Chinese patent application with application number 202311511514.9 and application date of November 13, 2023, and claims the priority of the Chinese patent application. The entire content of the Chinese patent application is hereby introduced into this application as a reference. Technical Field

[0003] The present application relates to the field of robotics technology, and in particular to a legged robot planning and control method, device, robot, and storage medium. Background Art

[0004] Legged robots are developing rapidly. Many are no longer limited to static walking, but are developing towards faster, more sensitive, more robust, safer, and more versatile. When legged robots perform highly dynamic movements, the collision problem between their soles and the ground cannot be ignored.

[0005] In related technologies, legged robots can be adjusted from the hardware and control algorithm aspects. In terms of hardware, you can choose to install a foot pad with a certain degree of flexibility on the sole of the robot, or install a buffer device such as a spring damper at the robot's driving joint. You can also use VIA (Variable Impedance Actuators) or VSA (Variable Stiffness Actuators) to provide the desired passive mechanical impedance, thereby achieving the effect of changing the joint impedance at the mechanical level, so that the buffering effect can be adjusted according to actual needs; in terms of control algorithms, the contact speed between the sole of the foot and the ground can be made close to 0 in motion planning.

[0006] However, these methods have certain shortcomings: Regarding hardware, the introduction of materials affects the response frequency of the robot system, and because the stiffness, damping, and inertia characteristics of the materials are fixed, the cushioning effect cannot be adjusted according to actual needs. When using VIA or VSA, the overall structure is too complex, and the size and weight are large, making it difficult to apply such actuators to legged robots that require high-dynamic movements. Regarding control algorithms, when a legged robot performs high-dynamic movements, there may be a significant error between the actual motion trajectory of the swinging leg and the planned trajectory, causing the foot to touch the ground prematurely or lagging behind, which may generate a huge plantar force impact, causing the joint to drift in zero position and accelerating hardware aging.

[0007] Summary of the Invention

[0008] The present application provides a legged robot planning and control method, device, robot and storage medium to solve the problems in related technologies such as limited passive flexible buffering of legged robots during collision, large computational complexity of control algorithms and low control accuracy, resulting in high wear and tear of robot hardware and short service life, which cannot meet the needs of actual applications.

[0009] The first aspect of the present application provides a legged robot planning and control method, comprising the following steps: obtaining motion instructions and motion states of the legged robot; generating a reference state sequence of the legged robot and the next moment swing parameters of each leg according to the motion instructions and the motion states; determining the foot-end reference state of each leg according to the motion state and the next moment swing parameters of each leg, and for the leg that starts to swing at the next moment, planning a swinging leg trajectory with collision awareness according to the foot-end dynamics model and the discrete collision model, so that the impact after the early collision is within the target constraint range; performing whole-body control of the legged robot according to the motion state, the reference state sequence and the foot-end reference state, wherein, for the leg that should be the supporting leg in planning and the supporting leg has not touched the ground, performing whole-body control with collision awareness according to the whole-body dynamics model and the discrete collision model, so that the impact after the delayed collision is within the target constraint range.

[0010] Optionally, the foot end dynamics model is:

[0011] in, is the linear acceleration of the foot end of the lth leg in the inertial system I, Λ c,l is the mass matrix in the operating space, h c,l is a term that includes the Coriolis force, centripetal force and gravity in the operating space, is the torque τ driving the joint j The foot end driving force after mapping to the operating space; the discrete collision model is:

[0012] in, is the projection operator on the collision normal vector, is the unit normal vector of the collision surface; are the velocities of object 1 before and after the collision, are the velocities of object 2 before and after the collision, c r is the coefficient of restitution.

[0013] Optionally, planning the collision-aware swinging leg trajectory based on the foot-end dynamics model and the discrete collision model includes: obtaining predetermined state quantities and target constraints; constructing a continuous-time trajectory optimization problem for the leg that starts swinging at the next moment based on the foot-end dynamics model, the predetermined state quantities 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.

[0014] Optionally, the predetermined state quantities include one or more of the initial state of the swing leg foot end, the landing state of the swing leg foot end, the duration of the swing and the expected maximum height of the swing leg foot end from the ground, and the target constraints include one or more of the state equation equality constraint, the driving force inequality constraint, the trajectory height inequality constraint, the initial state equality constraint, the terminal state equality constraint, the collision awareness inequality constraint and the terminal speed direction inequality constraint.

[0015] Optionally, the objective function of the continuous-time trajectory optimization problem is:

[0016] in, and are weight matrices of different dimensions (mostly diagonal matrices); ‖a‖ Q =a T Qa represents the weighted modulus of vector a under the weight matrix Q; Reflects the task of the swing leg foot end landing state, and are the known terminal position and linear velocity of the foot end of the swing leg respectively; is the control quantity (i.e. linear acceleration) that minimizes the entire swing process, t0=0, t F =T SW , T SW is the duration of the swing; Reflects the maximum height of the swing leg off the ground, t h1 and t h2 The two parameters specify the time period to reach the maximum height, satisfying t0≤t h1 ≤t h2 ≤t F ;z c is the third component of the state quantity, that is, the component of the state quantity representing the height of the foot end from the ground; the entire discrete time trajectory optimization problem is: f min ≤Λ c u k +h c ≤f max,k=0,1,…,N-1, z c,min ≤S z x k ≤z c,max ,k=1,2,…,N-1,

[0017] in, is a vector composed of the control quantities of each frame; It is a diagonal weight matrix with different dimensions; the current foot state (including position and speed) feedback Reference terminal foot state (including position and velocity) is the selection matrix of foot end linear velocity; and The start frame and end frame of the swing height are manually specified to meet k imp It is the starting frame for opening the collision awareness constraint and the terminal velocity direction constraint, satisfying 0≤k imp ≤N.

[0018] Optionally, the optimization problem of whole-body control is:

[0019] Among them, A i is the task matrix, b i is the task vector, C j is the constraint matrix, lb j and ub j are the lower and upper bounds of the constraints, W i is the weight matrix, n task is the number of tasks, and nconstraint is the number of constraints.

[0020] Optionally, the tasks processed simultaneously by the whole-body control include multiple tasks including trunk trajectory tracking tasks, foot trajectory tracking tasks, plantar force tracking tasks, minimizing joint torque variation tasks, and minimizing plantar force variation tasks; the constraints processed simultaneously by the whole-body control with collision awareness include multiple tasks including: floating basis dynamics equality constraints, plantar force inequality constraints, joint output torque saturation inequality constraints, joint speed saturation inequality constraints, joint output power saturation inequality constraints, and collision awareness constraints.

[0021] Optionally, the whole-body control of the foot-type robot is performed according to the motion state, the reference state sequence and the foot-end reference state, including: if the planning should be a swing leg, setting the foot-end trajectory tracking task to track the swing leg trajectory with collision awareness, the target plantar force of the plantar force tracking task to a preset value, the plantar force constraint in the plantar force inequality constraint to a preset value and disabling the collision awareness constraint; if the planning should be a supporting leg, and the supporting leg touches the ground, setting the target linear acceleration of the foot-end trajectory tracking task to a preset value, the target plantar force of the plantar force tracking task to the plantar force provided by MPC (Model Predictive Control) or other modules, the plantar force constraint to a friction cone constraint and disabling the collision awareness constraint; if the planning should be a supporting leg, and the supporting leg does not touch the ground, setting the target of the foot-end trajectory tracking task to track a speed pointing to the impact surface, the target plantar force of the plantar force tracking task to a preset value, the plantar force constraint to a preset value and enabling the collision awareness constraint.

[0022] Optionally, the whole-body dynamics model is:

[0023] in, is the generalized mass matrix, is a term that includes the Coriolis force, centripetal force, and gravity. Indicates the output torque of the driving joint; 0 n×m represents a zero matrix of size n×m; and F c They are the augmented Jacobian matrix formed by stacking the contact Jacobian matrix of each supporting leg and the augmented plantar force formed by stacking the reaction force of the ground on the sole of each supporting leg; q g is the generalized joint space position, is the generalized joint space velocity, is the generalized joint space acceleration; the collision awareness constraint matrix and vector are expressed as:

[0024] in, are the lower and upper limits of the allowable collision impulse specified by humans, and δt expresses the time interval between the current moment and the next moment.

[0025] The second aspect of the present application provides a legged robot planning and control device, including: an acquisition module for acquiring motion instructions and motion states of the legged robot; a generation module for generating a reference state sequence of the legged robot and the next moment swing parameters of each leg according to the motion instructions and the motion states; a planning module for determining the foot-end reference state of each leg according to the motion state and the next moment swing parameters of each leg, and for the leg that starts to swing at the next moment, planning a swinging leg trajectory with collision awareness according to the foot-end dynamics model and the discrete collision model, so that the impact after the early collision is within the target constraint range; a control module for performing whole-body control of the legged robot according to the motion state, the reference state sequence and the foot-end reference state, wherein, for the leg that should be the supporting leg in planning and the supporting leg has not touched the ground, performing whole-body control with collision awareness according to the whole-body dynamics model and the discrete collision model, so that the impact after the delayed collision is within the target constraint range.

[0026] Optionally, the foot-end dynamics model of the legged robot planning and control device is:

[0027] in, is the linear acceleration of the foot end of the lth leg in the inertial system I, Λ c,l is the mass matrix in the operating space, h c,l is a term that includes the Coriolis force, centripetal force and gravity in the operating space, is the torque τ driving the joint j The foot end driving force after mapping to the operating space; the discrete collision model is:

[0028] in, is the projection operator on the collision normal vector, is the unit normal vector of the collision surface; are the velocities of object 1 before and after the collision, are the velocities of object 2 before and after the collision, c r is the coefficient of restitution.

[0029] Optionally, the planning module is further used to: obtain predetermined state quantities and target constraints; construct a continuous-time trajectory optimization problem for the leg that starts swinging at the next moment based on the foot-end dynamics model, the predetermined state quantities and the target constraints; discretize the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot-end of the swinging leg, and solve the problem to obtain a swinging leg trajectory with collision awareness.

[0030] Optionally, the pre-established state quantities of the planning module include one or more of the initial state of the swing leg foot end, the landing state of the swing leg foot end, the duration of the swing and the expected maximum height of the swing leg foot end from the ground, and the target constraints include one or more of the state equation equality constraint, the driving force inequality constraint, the trajectory height inequality constraint, the initial state equality constraint, the terminal state equality constraint, the collision awareness inequality constraint and the terminal speed direction inequality constraint.

[0031] Optionally, the objective function of the continuous-time trajectory optimization problem of the planning module is:

[0032] in, and are weight matrices of different dimensions (mostly diagonal matrices); ‖a‖ Q =a T Qa represents the weighted modulus of vector a under the weight matrix Q; Reflects the task of the swing leg foot end landing state, and are the known terminal position and linear velocity of the foot end of the swing leg respectively; is the control quantity (i.e. linear acceleration) that minimizes the entire swing process, t0=0, t F =T SW , T SW is the duration of the swing; Reflects the maximum height of the swing leg off the ground, t h1 and t h2 The two parameters specify the time period to reach the maximum height, satisfying t0≤t h1 ≤t h2 ≤t F . z c is the third component of the state quantity, that is, the component of the state quantity representing the height of the foot end from the ground; the entire discrete time trajectory optimization problem is: f min ≤Λ c u k +h c ≤f max ,k=0,1,…,N-1, z c,min ≤S z x k ≤z c,max ,k=1,2,…,N-1,

[0033] in, is a vector composed of the control quantities of each frame; It is a diagonal weight matrix with different dimensions; the current foot state (including position and speed) feedback Reference terminal foot state (including position and velocity) is the selection matrix of foot end linear velocity; and The start frame and end frame of the swing height are manually specified to meet k imp It is the starting frame for opening the collision awareness constraint and the terminal velocity direction constraint, satisfying 0≤k imp ≤N.

[0034] Optionally, the optimization problem of the whole-body control of the legged robot planning and control device is:

[0035] Among them, A i is the task matrix, b i is the task vector, C j is the constraint matrix, lb j and ub j are the lower and upper bounds of the constraints, W i is the weight matrix, n task is the number of tasks, and nconstraint is the number of constraints.

[0036] Optionally, the tasks processed simultaneously by the whole-body control include multiple tasks including trunk trajectory tracking tasks, foot trajectory tracking tasks, plantar force tracking tasks, minimizing joint torque variation tasks, and minimizing plantar force variation tasks; the constraints processed simultaneously by the whole-body control with collision awareness include multiple tasks including: floating basis dynamics equality constraints, plantar force inequality constraints, joint output torque saturation inequality constraints, joint speed saturation inequality constraints, joint output power saturation inequality constraints, and collision awareness constraints.

[0037] Optionally, the whole-body control of the foot-type robot is performed according to the motion state, the reference state sequence and the foot-end reference state, including: if the planning should be a swing leg, setting the foot-end trajectory tracking task to track the swing leg trajectory with collision awareness, the target plantar force of the plantar force tracking task to a preset value, the plantar force constraint in the plantar force inequality constraint to a preset value and disabling the collision awareness constraint; if the planning should be a supporting leg, and the supporting leg touches the ground, setting the target linear acceleration of the foot-end trajectory tracking task to a preset value, the target plantar force of the plantar force tracking task to the plantar force provided by MPC or other modules, the plantar force constraint to a friction cone constraint and disabling the collision awareness constraint; if the planning should be a supporting leg, and the supporting leg does not touch the ground, setting the target of the foot-end trajectory tracking task to track a speed pointing to the impact surface, the target plantar force of the plantar force tracking task to a preset value, the plantar force constraint to a preset value and enabling the collision awareness constraint.

[0038] Optionally, the whole-body dynamics model of the legged robot planning and control device is:

[0039] in, is the generalized mass matrix, is a term that includes the Coriolis force, centripetal force, and gravity. Indicates the output torque of the driving joint; 0 n×m represents a zero matrix of size n×m; and F c They are the augmented Jacobian matrix formed by stacking the contact Jacobian matrix of each supporting leg and the augmented plantar force formed by stacking the reaction force of the ground on the sole of each supporting leg; q g is the generalized joint space position, is the generalized joint space velocity, is the generalized joint space acceleration; the collision awareness constraint matrix and vector are expressed as:

[0040] in, are the lower and upper limits of the allowable collision impulse specified by humans, and δt expresses the time interval between the current moment and the next moment.

[0041] The third aspect of the present application provides a legged 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 legged robot planning and control method as described in the above embodiment.

[0042] The fourth aspect of the present application provides a computer-readable storage medium on which a computer program is stored. The program is executed by a processor to implement the legged robot planning and control method as described in the above embodiment.

[0043] Therefore, this application has at least the following beneficial effects:

[0044] 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 early collision problem from the swing process, so that the impact after the early collision is within the target constraint range; at the same time, the whole-body dynamics model and discrete collision model of the foot-type robot can be controlled, and the lag problem can be processed from the control link, so that the impact after the lag collision is within the target constraint range; since both early and lagging collisions are taken into consideration at the same time, the embodiments of the present application can reduce the impact generated at the moment of collision between the sole of the foot and the ground, reduce the impact of the collision, and thereby improve the stability of the robot's motion state and increase the service life of the robot hardware.

[0045] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0047] FIG1 is a flow chart of a legged robot planning and control method according to an embodiment of the present application;

[0048] FIG2 is a schematic diagram of a collision-aware quadruped robot gait control system according to an embodiment of the present application;

[0049] FIG3 is a schematic diagram of a decision tree for whole-body control with collision awareness according to an embodiment of the present application;

[0050] FIG4 is an example diagram of a planning and control device for a legged robot according to an embodiment of the present application;

[0051] FIG5 is a schematic structural diagram of a legged robot according to an embodiment of the present application. DETAILED DESCRIPTION

[0052] The following describes in detail embodiments of the present application, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present application, and should not be construed as limiting the present application.

[0053] In recent years, the field of legged robots has developed rapidly, with an increasing number of mature and stable methods being applied to their motion planning and control. Many research institutions are moving beyond static walking to develop legged robots that are faster, more agile, more robust, safer, and more versatile. When legged robots perform high-dynamic motion, issues that were previously negligible in low-dynamic situations become critical, such as foot-to-ground collisions during high-speed running. Legged robots move by continuously switching between their supporting legs. Each step requires contact between the foot and the ground, creating a purposeful collision with the environment and a common collision issue when legged robots interact with their environment. Improper control strategies can cause violent foot-to-ground collisions, impacting subsequent robot motion, causing the robot to lose balance and fall, and potentially even shortening or damaging the hardware's lifespan.

[0054] In related technologies, solutions to the collision problem of legged robots can be proposed from two aspects: hardware and control algorithms.

[0055] In terms of hardware, options include installing flexible foot pads on the robot's soles, or installing buffering devices such as spring dampers at the robot's drive joints. Alternatively, VIA or VSA can be used. These provide the desired passive mechanical impedance, mechanically changing the joint impedance and thus adjusting the buffering effect according to actual needs. Installing buffering devices on robots is generally referred to as passive compliance.

[0056] In terms of control algorithms, to minimize the negative impact of foot-to-ground collisions on a legged robot's state during walking, motion planning can be used to keep the contact velocity of the foot close to zero. When a small or lightweight legged robot walks slowly on flat terrain, the controller's performance generally ensures that the actual trajectory of the swinging legs is roughly consistent with the planned trajectory, making this approach more effective. When the terrain is uneven, visual sensors such as cameras or lidar can be used to estimate the terrain. Based on this estimated terrain, a trajectory with a contact velocity close to zero is planned, thus avoiding collisions caused by premature or delayed foot contact.

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

[0058] In terms of hardware, passive compliance methods can reduce the impact of collisions to a certain extent, but there are also some problems. First, the introduction of these passive flexible materials will affect the response frequency of the robot system. If the control algorithm does not take this effect into account, it is easy to cause unstable vibrations during the robot's movement. Secondly, the buffering effect of passive compliance methods is limited. When the material of the buffer device is determined, its stiffness, damping, and inertia characteristics are also determined, making it impossible to adjust the buffering effect according to actual needs. When using the VIA or VSA method, two motors can be used, one of which independently controls the mechanical stiffness of the joint, while the other generates torque. However, the overall structure is too complex, and the volume and weight are large, making it difficult to apply this actuator to legged robots that need to perform high-dynamic movements.

[0059] In terms of control algorithms, when a legged robot performs highly dynamic movements, such as high-speed running, there can be significant discrepancies between the actual trajectory of the swinging leg and the planned trajectory, resulting in premature or delayed foot contact. In the case of delayed contact, the leg will quickly slam into the ground before the foot has touched the ground, even though the plan requires it to support the torso. This can cause a violent collision, generating a massive impact force at the moment of impact. For heavy legged robots, the impact force is often even greater. When running at high speed, the intense impact can cause joint zero-position drift, and frequent, long-term foot contact can accelerate hardware aging.

[0060] In summary, the control strategies in related technologies regard the impact force during a collision as a disturbance and focus on the balance control problem of the robot after the collision, while ignoring the impact of such frequent and purposeful collisions on the life of the robot hardware.

[0061] In response to the problems mentioned in the above background technology, the present application provides a legged robot planning and control method. The following describes the legged robot planning and control method, device, robot and storage medium of the embodiments of the present application with reference to the accompanying drawings.

[0062] Specifically, Figure 1 is a flow chart of a legged robot planning and control method provided by an embodiment of the present application. As shown in Figure 1, the legged robot planning and control method includes the following steps:

[0063] In step S101, the motion instructions and motion state of the legged robot are obtained.

[0064] Among them, the driving instructions may include movement speed instructions, direction instructions, etc. input by the user, and the movement state may include the robot torso state, the state of each leg foot end and the ground contact state of each leg.

[0065] It can be understood that the embodiments of the present application can use at least one method to obtain the motion instructions and motion status of the legged robot; the actuators of the legged robot in the embodiments of the present application are all motors with force control capabilities, and the sensors they have include IMU (Inertial Measurement Unit), motor angle encoder, motor torque sensor, etc. The following embodiments will be explained based on this.

[0066] For example, Figure 2 is a block diagram of a collision-aware quadruped robot gait control system according to an embodiment of the present application, which is applicable to both simulation and physical objects. The entire control framework is mainly composed 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 system frequency (e.g., 500Hz, 1kHz, etc.), IA-WBC operates at the same frequency, SRB-MPC operates at about 40Hz, and the optimization problem in IA-TO is only calculated once each time a leg needs to swing. In the following embodiments, the above system will be used as an example for explanation.

[0067] Specifically, as shown in Figure 2, the embodiment of the present application can estimate the trunk state of the legged robot based on the read IMU data and motor state information; wherein, the IMU data may include three-dimensional Euler angles, three-dimensional angular velocity and three-dimensional linear acceleration, etc., the motor state information may include motor rotation angle, speed and feedback torque, etc., the trunk state of the legged robot may include the point position of the trunk, attitude unit quaternion, linear velocity and angular velocity, etc., and the state of each leg end may include the point position and linear velocity; the embodiment of the present application may use an EKF (Extended Kalman Filter) to estimate the trunk state, and no specific limitation is made to this.

[0068] The state estimation of the embodiment of the present application will transmit corresponding information according to different needs. As shown in Figure 2, for SRB-MPC, only the trunk state is transmitted; for IA-TO, the trunk state, the state of the foot end of each leg, and the joint state are transmitted; for IA-WBC, the trunk state, the state of the foot end of each leg, the joint state, and the ground contact state of each leg are transmitted; for the rough planning module, the trunk state and the state of the foot end of each leg can be transmitted; the above transmission process will be specifically explained in the following embodiments and will not be repeated here.

[0069] In step S102, a reference state sequence of the legged robot and the next moment swing parameters of each leg are generated according to the motion instruction and the motion state.

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

[0071] It can be understood that the embodiments of the present application can use the acquired motion instructions and motion states to generate a reference state sequence of the legged robot and the swing parameters of each leg at the next moment; the embodiments of the present application can use at least one method to generate the reference state sequence and the swing parameters at the next moment, such as using the acquired information for motion planning.

[0072] Specifically, as shown in Figure 2, the coarse planning module performs a rough motion plan for the quadruped robot based on state estimation feedback and user input (e.g., via a remote control) of speed commands (including forward and backward walking speed and left and right lateral translation speed) and direction commands (i.e., yaw angular velocity). The coarse planning module needs to provide the SRB-MPC with a reference state sequence, where the number of reference state sequences is the same as the number of SRB-MPC prediction frames. The coarse planning module also provides the IA-TO with the foothold position, swing duration, and current reference contact state for each leg's next swing.

[0073] In step S103, the foot-end reference state of each leg is determined based on the motion state and the swing parameters of each leg at the next moment. For the leg that starts to swing at the next moment, the swing leg trajectory with collision awareness is planned based on the foot-end dynamics model and the discrete collision model, so that the impact after the early collision is within the target constraint range.

[0074] It can be understood that the embodiments of the present application can determine the foot-end reference state of each leg based on the acquired motion state and the swing parameters of each leg of the leg-type robot at the next moment; when there is a leg that starts to swing at the next moment, the foot-end dynamics model and the discrete collision model are used to plan the swing leg trajectory with collision awareness, so that the impact of the early collision during the swing leg movement is within the target constraint range.

[0075] It should be noted that due to factors such as slight undulations of the ground, errors in state estimation, and errors in control, the foot end of the swing leg of the legged robot comes into contact with the ground before the planned landing time point, which is an early collision; and this unexpected early contact often brings unexpected collision impact. This problem can actually be solved in planning, that is, planning a motion trajectory of the swing leg foot end with collision awareness, so that even if an early collision occurs, its collision impact is within the expected range. Therefore, the embodiment of the present application can construct a trajectory optimization problem that can be applied to the swing leg of a legged robot, wherein the robot dynamics model used is a linearized dynamics model of the swing leg foot end 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 foot end swing time, that is, it is pre-specified. In order to enable the computer on the robot to perform trajectory optimization online, the corresponding optimization problem is designed as a QP (Quadratic Programming) problem that is easy to solve.

[0076] Specifically, as shown in Figure 2, the rough planning module provides the IA-TO with the foothold position, swing duration, and current reference contact state for each leg's next swing. The IA-TO module receives state estimation feedback and reference information provided by the rough planning to generate an appropriate reference state for each leg's foot (including reference point position and linear velocity). The SRB-MPC module solves a model predictive control optimization problem based on the reference information provided by the rough planning, obtaining the optimal trunk reference state and optimal reference plantar force, which are directly transmitted to the IA-WBC. The SRB-MPC solves the optimization problem approximately every frame. When it does not need to solve, its output is not updated, meaning the output data is zero-order held.

[0077] For the stance leg, its reference state is to remain stationary on the ground. For the leg about to swing, a collision-aware trajectory optimization problem is solved. For the leg already in swing, the optimal trajectory optimization solution is interpolated to obtain the reference state at the current moment. The IA-TO module only solves trajectory optimization once before a leg swings, while the computational effort at other times is minimal. The IA-TO module ultimately transmits the reference state of each leg's foot end, along with the reference contact state provided by the rough plan, to the IA-WBC.

[0078] In an embodiment of the present application, a collision-aware swinging leg trajectory is planned based on a foot-end dynamics model and a discrete collision model, including: obtaining predetermined state quantities and target constraints; constructing a continuous-time trajectory optimization problem for the leg that starts swinging at the next moment based on the foot-end dynamics model, the predetermined state quantities 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.

[0079] The pre-specified quantities may include: the initial state of the swing leg foot end, which includes the initial point position and linear velocity; the landing state of the swing leg foot end, which includes the point position and linear velocity when landing; the duration of the swing Expected maximum ground clearance of the foot end of the swing leg

[0080] The pre-established state quantities of the embodiment of the present application include one or more of the initial state of the swing leg foot end, the landing state of the swing leg foot end, the duration of the swing and the expected maximum height of the swing leg foot end from the ground, and the target constraint conditions include one or more of the state equation equality constraint, the driving force inequality constraint, the trajectory height inequality constraint, the initial state equality constraint, the terminal state equality constraint, the collision awareness inequality constraint and the terminal speed direction inequality constraint.

[0081] It is understandable that each leg needs to solve a trajectory optimization problem before it begins to swing to obtain the optimal foot-end swing trajectory. Given the limited computing power of the actual robot, the trajectory optimization problem for the same leg is not repeatedly solved during the foot-end swing process. Since the expressions for the trajectory optimization problems for each leg are essentially the same, for simplicity of expression, the present embodiment omits the subscript l used to represent the leg numbers when there is no ambiguity.

[0082] Specifically, the embodiment of the present application can use the swing leg foot end line acceleration as the control quantity, that is, The state variables are selected as the point position and linear velocity of the foot end, that is, Based on this, the objective function, constraints, and discrete-time trajectory optimization problem of the embodiment of the present application are described below:

[0083] 1. Objective Function

[0084] In the embodiment of the present application, the foot end dynamics model is:

[0085] in, is the linear acceleration of the foot end of the lth leg in the inertial system I, Λ c,lThe mass matrix in the operating space, h c,l is a term that includes the Coriolis force, centripetal force and gravity in the operating space, is the torque τ driving the joint j The foot end driving force after being mapped to the operating space; the process of obtaining the foot end dynamics model in the embodiment of the present application can be specifically as follows:

[0086] In the embodiment of the present application, the point position, linear velocity and linear acceleration of the foot end of the first leg in the inertial system I can be respectively Then the dynamic equation of the foot end in the operating space is:

[0087] in, They are the torque τ driving the joint j The foot-end driving force after mapping to the operating space and the reaction force of the ground on the foot-end (the latter only exists when in contact with the ground); and Λ c,l (q g ) is the mass matrix in the operation space, is a term that includes the Coriolis force, centripetal force, and gravity in the operating space.

[0088] Considering that even when a legged robot performs dynamic motion, its swinging legs are mostly within a certain range below the torso, the embodiment of the present application can be considered that the range of leg configuration changes during a single swing is not particularly large, so Λ c,l (q g ) can be approximately considered to be independent of the configuration and regarded as a constant matrix; experiments have shown that the nonlinear term The changes during leg swing are actually not large, and can also be approximately regarded as a constant vector.

[0089] Thus, the simplified foot end dynamic model can be obtained:

[0090] Considering the sole of the first leg as object 1 and the ground as object 2, and considering that the mass of the ground is much greater than the mass of the robot and the velocity of the ground is zero before and after the collision, the discrete collision model of the embodiment of the present application can be further obtained as follows:

[0091] in, is the projection operator on the collision normal vector, is the unit normal vector of the collision surface; are the velocities of object 1 before and after the collision, are the velocities of object 2 before and after the collision; c r is the restitution coefficient, which is defined as

[0092] In the embodiment of the present application, the objective function of the continuous-time trajectory optimization problem is:

[0093] in, and are weight matrices of different dimensions (mostly diagonal matrices); ‖a‖ Q =a T Qa represents the weighted modulus of vector a under the weight matrix Q; Reflects the task of the swing leg foot end landing state, and are the known terminal position and linear velocity of the foot end of the swing leg respectively; is the control quantity (i.e. linear acceleration) that minimizes the entire swing process, t0=0, t F =T SW , T SW is the duration of the swing; Reflects the maximum height of the swing leg off the ground, t h1 and t h2 The two parameters specify the time period to reach the maximum height, satisfying t0≤t h1 ≤t h2 ≤t F . z c It is the third component of the state quantity, that is, the component in the state quantity that represents the height of the foot from the ground.

[0094] 2. Constraints

[0095] (1) State equation equality constraint: The control variable is chosen to be the foot end linear acceleration, so the state equation is very easy to express; let:

[0096] Then the state equation of the system can be written as:

[0097] (2) Driving force constraint: Since the output torque of the actual actuator has a lower limit and an upper limit, the corresponding foot end driving force lower limit in the operating space and upper limit The driving force constraint can then be written as: min ≤Λ c u(t)+h c ≤f max ,fort∈[t0,t F ].

[0098] (3) Trajectory height constraint: In order to avoid the optimized trajectory being lower than the ground or higher than the desired maximum height above the ground, it is necessary to constrain the trajectory height, i.e., zc,min ≤S z x(t)≤z c,max ,fort∈[t0,t F ],

[0099] in, are the upper and lower limits of the trajectory height; is the selection matrix used to select the third component of the state.

[0100] (4) Initial state constraint: The initial state in the optimization of the swing leg foot trajectory should be the state just before the foot leaves the ground, so the initial state constraint is

[0101] in, They are the latest feedback point position and linear velocity of the swing leg foot before solving the trajectory optimization problem.

[0102] (5) Terminal state constraint: Although the terminal state task has been reflected in the objective function, it cannot guarantee that the height of the end of the optimized foot trajectory from the ground is zero, which may cause the end of the trajectory to hang in the air or penetrate into the ground. Therefore, a terminal state constraint is required, that is,

[0103] in, and are the reference height and vertical reference velocity at the end of the trajectory, respectively, where and is the corresponding selection matrix, i.e.

[0104] It should be noted that the terminal state constraint in the embodiment of the present application does not care about the horizontal component in the state. This is because the desired terminal state in the horizontal direction may not be achieved when there are many constraints. Forcibly implementing the terminal state constraint in the horizontal direction may make the optimization problem unsolvable.

[0105] (6) Collision awareness constraint: In order to allow the swing leg to fully decelerate to a sufficiently safe speed before landing, the embodiment of the present application can consider the collision awareness constraint at the end of the trajectory: Assuming that the collision process starts from t c Starts at time t and lasts until c +δt moment ends, Integrating both sides of the equal sign, we have:

[0106] Since the robot configuration remains almost unchanged immediately before and after the collision, At the same time, the collision process is very short, that is, δt→0. At this time, the pulses from finite forces (such as gravity, Coriolis force, and centripetal force) will disappear in this limit, leaving only the impulse generated by the collision; therefore, the above formula becomes:

[0107] in, represents the impulse of the ground to the foot end; substituting this formula into the early collision model can obtain the collision awareness constraint:

[0108] in, It is the lower and upper limits of the permissible collision impulse selected by humans; imp is the time selected by humans to start the collision awareness constraint, satisfying t0≤t imp ≤t F .

[0109] It is understandable that the collision awareness constraint formula can express the swing leg foot end before landing, that is, [t imp ,t F ] During this time period, if an early collision occurs for some reason, the impulse caused by the collision should be within the constraints of this formula; among them, this constraint cannot be turned on throughout the swing, otherwise the entire swing process will be very slow and normal running motion cannot be achieved.

[0110] (7) Terminal velocity direction constraint: If the velocity direction of the swing leg foot end before landing is close to horizontal, then due to the error in the foot end height control, the sole of the foot may touch the ground prematurely, resulting in the swing leg foot end not landing at the planned landing point, which may affect the stability of the robot's center of mass. Therefore, the embodiment of the present application can design the terminal velocity direction constraint of the foot end so that the velocity of the swing leg foot end before landing is close to horizontal. The angle θ with the unit normal vector -n of the collision surface (the negative sign corresponds to the direction inward) c Within a specified range, that is:

[0111] in, is the maximum angle allowed by humans. The above nonlinear expression can be obtained by scaling to obtain a linear expression. Norm and The relationship between the norms is:

[0112] This can be obtained after rewriting All solutions that satisfy the rewritten formula also satisfy the rewritten formula. Therefore, the rewritten formula can be used as the final velocity direction constraint of TO, which can be further written as:

[0113] in, yes By traversing all the absolute value symbols, the above formula can be further written into 8 equivalent inequalities, namely

[0114] However, there is usually a short period of time before the swing leg lands. The first four lines in this formula are usually redundant, so the final velocity direction constraint can be simplified to:

[0115] in,

[0116] 3. Discrete-time trajectory optimization problem

[0117] It can be understood that in order to facilitate the solution of the above-mentioned continuous-time trajectory optimization problem in a computer, the embodiment of the present application can also be discretized; among them, the embodiment of the present application can use at least one discretization method, such as the integral term in the objective function can be replaced by the sum of finite terms, the state equation constraint can be replaced by the Taylor expansion formula that ignores high-order terms, etc.

[0118] In the embodiment of the present application, it is assumed that the swing process is divided into N segments evenly according to time, that is, there are N frames in total, and the frame interval is Δt=T sw / N. Let t k =t0+k(t F -t0) / N,k=1,2,…,N, then we can simply write x k =x(t k ),u k =u(t k ); the entire discrete-time trajectory optimization problem can be obtained as: f min ≤Λ c u k +h c ≤f max ,k=0,1,…,N-1, z c,min ≤S z x k ≤z c,max ,k=1,2,…,N-1,

[0119] in, is a vector composed of the control quantities of each frame; It is a diagonal weight matrix with different dimensions; the current foot state (including position and speed) feedback Reference terminal foot state (including position and velocity) is the selection matrix of foot end linear velocity; and The start frame and end frame of the swing height are manually specified to meet k imp It is the starting frame for opening the collision awareness constraint and the terminal velocity direction constraint, satisfying 0≤k imp ≤N.

[0120] It is understandable that since many trajectory optimization problems in research are highly nonlinear, they are usually non-convex optimization problems. Although some professional nonlinear solvers can find local optimal solutions to these trajectory optimization problems, they cannot guarantee that a global optimal solution will be found. The trajectory optimization problem in the embodiment of the present application is a QP problem, which is a typical convex optimization problem. Therefore, as long as there is a non-empty solution set, the optimal solution found by the optimizer must be the global optimal solution.

[0121] The objective function of the aforementioned swing leg foot discrete time trajectory optimization problem can be solved using some QP solvers. For example, the embodiment of the present application can use the C++ based open source library qpOASES, which needs to convert the QP problem into a standard form: stlb≤CU≤ub,

[0122] After solving the optimal solution of the above formula, we only get the optimal control quantity U for N frames. * , and it is also necessary to use it to obtain the optimal position and velocity trajectory. The above N-row discrete dynamic equations are written as linear expressions about the initial state and optimization variables, that is,

[0123] in, And A QP and B QP They are:

[0124] Therefore, substituting U* into the above linear expression can obtain the corresponding N-frame optimal position and speed.

[0125] Since the subsequent control program may require a corresponding reference trajectory at any time, it is necessary to convert N discrete points into a continuous curve through interpolation; among them, the embodiment of the present application can use at least one method to implement interpolation; in order to maximize computing efficiency, the embodiment of the present application can use the simplest linear interpolation.

[0126] For example, suppose that from the moment the swing leg leaves the ground, tsw Time, t sw ∈[0,T sw ], and the previous frame closest to this moment is the kth frame, and the corresponding optimal position is Using the linear interpolation method, the optimal reference position at this moment should be:

[0127] Similarly, the corresponding optimal reference speed can be obtained

[0128] It should be noted that this interpolation method cannot guarantee that the time integral of the velocity curve is the position curve. To achieve this, a higher-order interpolation function is required. Here, some interpolation accuracy is sacrificed only to maximize computational efficiency.

[0129] In step S104, the whole body of the legged robot is controlled according to the motion state, the reference state sequence and the foot-end reference state. For the leg that should be the supporting leg in planning and has not touched the ground, the whole body is controlled with collision awareness according to the whole body dynamics model and the discrete collision model, so that the impact after the delayed collision is within the target constraint range.

[0130] It can be understood that, as shown in Figure 2, the embodiment of the present application can solve WQP (Weighted Quadratic Programming) and calculate the optimal joint torque instructions based on the feedback information of state estimation, SRB-MPC and IA-TO modules, perform whole-body control of the legged robot, and deal with the hysteresis collision problem.

[0131] It should be noted that a delayed collision refers to a situation where the foot end has not yet made contact with the ground at the originally planned time point due to certain reasons (such as slight undulations of the ground, errors in state estimation, and errors in control, etc.), and the contact between the foot end and the ground must be later than the originally planned time point; if there is no appropriate control strategy, this unexpected delayed collision can easily lead to unexpected collision impacts. The delayed collision problem is essentially different from the early collision problem, that is, the delayed collision occurs outside the planned swing process, and it is impossible to accurately know the distance between the sole of the foot and the ground (the distance should be zero), nor is it possible to determine when the foot will make contact with the ground (the collision should have occurred on time), and it needs to be effectively solved in the control link. Therefore, based on the core idea of ​​one-step predictive control, the embodiment of the present application can use a collision-aware WBC (Whole Body Control) method.

[0132] Specifically, the following embodiments will use a quadruped robot as an example to specifically illustrate the WBC method of the present application embodiment. In particular, the collision awareness constraint is not always enabled. In different situations, the task objectives and constraint settings of the whole-body control are different. Therefore, the present application embodiment can be expressed in the form of a decision tree as shown in FIG3. The whole-body control of the collision awareness of the present application embodiment is specifically as follows:

[0133] 1. Whole-body control based on weighted quadratic programming

[0134] It can be understood that in order for the whole-body control to handle inequality constraints such as collision awareness constraints, the implementation of the whole-body control should choose an optimization-based method; at the same time, considering that the computing resources on the actual robot are limited and there are many programs that need to run simultaneously (such as communication, control algorithms, etc.), the embodiment of the present application can adopt a whole-body control method based on WQP implementation.

[0135] In the embodiment of the present application, the optimization problem of whole-body control is:

[0136] Among them, A i is the task matrix, b i is the task vector, C j is the constraint matrix, lb j and ub j are the lower and upper bounds of the constraints, W i is the weight matrix, n task is the number of tasks, and nconstraint is the number of constraints.

[0137] It is understandable that in WQP, all tasks that need to be completed are listed in A i χ-b i In the objective function, the weight matrix W i To adjust the relative priority of tasks; all constraints are expressed as lb j ≤C j χ≤ub j For the equality constraint, we can set lb j =ub j In the embodiment of the present application, the optimization variable of WBC is selected as the generalized joint space acceleration The augmented plantar force F is formed by the superposition of the forces acting on the four legs c ,Right now Among them, n χ =n q +n F .

[0138] When the quadruped robot is in a standing position, the whole-body dynamic model of the legged robot in the embodiment of the present application is:

[0139] in, is the generalized mass matrix, is a term that includes the Coriolis force, centripetal force, and gravity. Indicates the output torque of the driving joint; 0 n×m represents a zero matrix of size n×m; and F c They are the augmented Jacobian matrix formed by stacking the contact Jacobian matrix of each supporting leg and the augmented plantar force formed by stacking the reaction force of the ground on the sole of each supporting leg; q g is the generalized joint space position, is the generalized joint space velocity, is the generalized joint space acceleration.

[0140] By solving the QP problem above, we can get the optimal solution χ * ; Combined with the complete dynamic equation, the optimal joint torque index can be easily calculated

[0141] in, is the selection matrix for driving joints, and are the generalized joint space position and velocity feedback provided by the state estimation, respectively.

[0142] 2. Task Settings

[0143] In an embodiment of the present application, the tasks simultaneously processed by the whole-body control include multiple tasks including trunk trajectory tracking tasks, foot trajectory tracking tasks, plantar force tracking tasks, minimizing joint torque change tasks, and minimizing plantar force change tasks; the constraints simultaneously processed by the whole-body control with collision awareness include multiple tasks including: floating basis dynamics equality constraints, plantar force inequality constraints, joint output torque saturation inequality constraints, joint speed saturation inequality constraints, joint output power saturation inequality constraints, and collision awareness constraints.

[0144] Specifically, (1) Trunk tracking task: Trunk trajectory tracking task is the basic task for the quadruped robot to move and is indispensable. In the embodiment of the present application, a trunk reference motion trajectory provided by MPC (Model Predictive Control) can be used, including the trunk optimal reference point position in the inertial system I. Optimal reference line speed Optimal ZYX Euler angles And the optimal reference angular velocity of the trunk under the floating base frame B

[0145] In WBC, PD control is first performed based on the optimal reference trajectory provided by MPC and the feedback state provided by state estimation to obtain the linear acceleration command of the trunk in the inertial system I. and the angular acceleration command in the floating base frame B Then design the corresponding tasks to complete these instructions. Among them, PD control is:

[0146] in, They are PD gain coefficient matrices, both of which are diagonal matrices; Based on the reference ZYX Euler angle and feedback ZYX Euler angle The calculated axis angle representing the attitude error.

[0147] because The first 6 rows of correspond to the linear acceleration and angular acceleration of the floating base, so the corresponding matrices and vectors for the torso trajectory tracking task are written as:

[0148] (2) Foot end trajectory tracking task: The foot end trajectory tracking task is also a basic task of quadruped robot movement and is indispensable. For the foot end of the first leg, according to the kinematics of the acceleration level (excluding rotation), we have:

[0149] Therefore, the corresponding matrices and vectors of the foot trajectory tracking subtask of the lth leg can be written as:

[0150] in, It is the linear acceleration instruction of the foot end of the lth leg in the inertial system I. When this leg is in different states, the instruction is different.

[0151] For example, when this leg is the supporting leg, the foot should be fixed on the ground and not move. When it should be the supporting leg but has not yet touched the ground (delayed touchdown), To track a derivative control of the velocity toward the collision surface:

[0152] in, is the expected collision velocity specified by humans, is the unit normal vector of the collision surface mentioned above, pointing outside the collision surface; when this leg is used as a swinging leg, From PD control:

[0153] in, is the PD gain coefficient matrix, which are all diagonal matrices, The optimal reference point position and linear velocity of the foot end of the swinging leg calculated in the above embodiment.

[0154] It should be noted that the foot-end trajectory tracking subtasks of the four legs in the embodiment of the present application are combined to form a complete foot-end trajectory tracking task. Therefore, the corresponding matrices and vectors can be written as:

[0155] (3) Plantar force tracking task: In addition to being affected by gravity, the center-of-mass linear momentum and center-of-mass angular momentum of the quadruped robot are also affected by the reaction force of the ground on the foot end (i.e., plantar force); when performing high-dynamic movements, appropriate plantar force is very important.

[0156] The embodiment of the present application can propose a method of optimizing the plantar force with reference value using a single rigid body model MPC. In this way, corresponding tasks need to be set in WBC to complete the tracking of plantar force.

[0157] Since the quadruped robot used in the embodiment of the present application does not have a force / torque sensor at the foot end, it is difficult to implement plantar force feedback control in the WBC. The plantar force tracking task can only be set as a simple feedforward force task. The corresponding matrices and vectors can be written as:

[0158] in, When the first leg has contacted the ground and serves as the supporting leg, it should execute the reference plantar force provided by the MPC, so When this leg is the swing leg or is the supporting leg in planning but has not yet touched the ground (i.e., delayed touchdown), the foot end of this leg should not be subject to external force, so

[0159] (4) Task of minimizing the change in joint torque: Since the actual motor force control bandwidth is limited, too drastic output torque changes cannot be achieved in the actual machine. When the actual motor cannot perform actions according to the optimal solution of WBC, the control error is likely to increase. Therefore, the embodiment of the present application can take "reducing the change in joint torque" into consideration in WBC, making it easier for the actual motor to perform actions according to the optimal solution of WBC.

[0160] Assume that the optimal joint torque command calculated by WBC in the previous control cycle is The matrix and vector of the task of minimizing the joint torque change in this control cycle can be written as:

[0161] It should be noted that this task has potential conflicts with the aforementioned trunk trajectory tracking task, foot trajectory tracking task and plantar force tracking task. Therefore, in the embodiment of the present application, the weight of this task cannot be too large, otherwise it will affect the execution effect of the aforementioned tasks.

[0162] (4) Task of minimizing plantar force variation: In order to make the plantar force variation smoother (and thus make the center of mass state variation smoother), the embodiment of the present application may consider the task of “reducing the plantar force variation” in the WBC.

[0163] Assume that the optimal plantar force calculated by WBC in the previous control cycle is The matrix and vector of the task of minimizing the plantar force variation in this control cycle can be written as:

[0164] It should be noted that, similarly, this task has potential conflicts with the aforementioned trunk trajectory tracking task, foot trajectory tracking task, and plantar force tracking task. Therefore, the weight of this task cannot be too large, otherwise it will affect the execution effect of other tasks.

[0165] 3. Constraint Settings

[0166] (1) Floating-basis dynamics equation constraints: The trunk of the quadruped robot does not have direct driving force and torque, so there is no joint output torque in the first six lines of the complete motion equation. These six lines are the floating-basis dynamics equations, which are the equality constraints that must be satisfied. The corresponding matrices and vectors are:

[0167] in, is the selection matrix of the floating basis.

[0168] (2) Plantar force inequality constraint: To avoid relative sliding between the foot of the supporting leg and the ground, the ratio of the plantar force component parallel to the contact surface to the plantar force component perpendicular to the contact surface should be less than the friction coefficient, that is, the plantar force F of the first leg c,l The following constraints should be met to avoid slipping: ||(I 3×3 -nn T )F c,l ||2≤μ|n T F c,l |,

[0169] where μ is the coefficient of friction.

[0170] It can be understood that the above formula describes a conical interval, so this constraint to avoid foot slip is also called friction cone constraint; The norm is a nonlinear expression. In order to facilitate the solution of the WBC optimization problem, the embodiment of the present application can simplify it.

[0171] The embodiment of the present application can first linearize the formula, that is, use a quadrangular pyramid instead of a cone; secondly, it is assumed that the quadruped robot usually walks on a flat ground with a slope close to zero, so F can be c,l The Z-axis component of is considered as the component perpendicular to the contact surface; thus, the above formula can be rewritten as:

[0172] It should be noted that the plantar force is the reaction force of the ground on the sole of the foot, so its Z-axis component is always greater than zero. In order to avoid hardware damage caused by optimizing a particularly large plantar force, the embodiment of the present application can also be used to adjust the F c,l The Z-axis component of the force is restricted; therefore, the plantar force inequality constraint of the lth leg can be written as:

[0173] in, It is the maximum plantar force in the Z-axis direction specified by humans: when this leg is already the supporting leg, is a pre-specified positive number; when the leg is a swing leg or is planned to be a supporting leg but has not yet landed (latent landing), the plantar force of the leg should be zero, so set In the embodiment of the present application, the above formula can also be equivalently expressed in the form of a matrix, that is: 5×1 ≤C F F c,l ≤ub F ,

[0174] in,

[0175] Finally, the plantar force constraints of the four legs are combined to form the complete plantar force inequality constraint. The corresponding matrices and vectors can be written as:

[0176] (3) Joint output torque saturation inequality constraint: The actual motor performance is limited, and even in the stalled state, its output torque has an upper limit. The output torque limit of the actual motor needs to be considered in WBC to better coordinate the various joints to complete multiple desired tasks simultaneously. Therefore, the matrix and vector of the joint output torque saturation inequality constraint can be written as:

[0177] Among them, τ j,min ,τ j,max are the lower and upper limits of the joint output torque respectively.

[0178] (4) Joint speed saturation inequality constraint: Similar to the joint output torque saturation inequality constraint, even in the case of no-load, the actual motor speed has an upper limit. The actual motor speed limit must be considered in WBC to better coordinate the various joints to complete multiple desired tasks simultaneously.

[0179] Perform Taylor expansion on the joint speed at time t and ignore the higher-order terms higher than the acceleration. The embodiment of the present application can be used to Perform linear prediction:

[0180] In the same control cycle, the embodiment of the present application can use the latest feedback joint speed As the current joint speed And use the joint acceleration in the optimization variable as the joint acceleration at the current moment Therefore, the joint speed at time t+δt can be linearly expressed using the optimization variables using the above formula. Therefore, the corresponding matrices and vectors of the joint speed saturation inequality constraints are:

[0181] in, are the lower and upper limits of the joint rotation speed respectively.

[0182] (5) Joint output power saturation inequality constraint: Similar to the joint output torque saturation inequality constraint, the actual motor output power also has an upper limit. The actual motor output power limit must be considered in the WBC to better coordinate the various joints to simultaneously complete multiple desired tasks. Considering that the environment may also perform work on the motor, the embodiment of the present application does not constrain the input power of the joint, and can only constrain the output power of the joint.

[0183] For the αth joint, let its output torque be Output speed is The output power of the joint is Let n j The output power of each joint is Then we have:

[0184] The rotational speed of the joint can be expressed as Substitute in, and the output torque of the joint can be linearly expressed using the optimization variable, so the corresponding matrices and vectors of the joint output power saturation inequality constraints are:

[0185] in, is the upper limit of the joint's output power.

[0186] (6) Collision-aware constraint: The idea of ​​collision-aware constraint in WBC is similar to that of collision-aware constraint in TO, and the discrete collision model used is also the same, namely, the discrete collision dynamics model.

[0187] It is understandable that, in the WBC, assuming that a collision occurs at the next moment (i.e., at time t+δt), the embodiments of the present application can first use the Taylor expansion formula to predict the linear velocity of the foot before the next moment, then predict the sudden change in the foot bottom line velocity caused by the collision, and then predict the impulse caused by the collision, and finally constrain this impulse within the current control cycle. As a result, the joint acceleration optimized by the WBC can make the linear velocity of the foot sufficiently small so that if a collision does occur at the next moment, the impulse caused by the collision is within the collision awareness constraint range of the WBC; if no collision occurs at the next moment, the effect exhibited by the robot is a significant reduction in the movement speed of the foot.

[0188] Specifically, for the lth leg, the robot's kinematics and Taylor expansion formula can be used to linearly predict the linear velocity of the forefoot at time t+δt using the joint acceleration in the optimization variable.

[0189] Substitute the above equations into the discrete collision dynamics model and The joint acceleration in the optimization variable can be used to linearly represent the sudden change in foot bottom line velocity caused by the predicted collision.

[0190] The predicted collision impulse ι is expressed linearly using the joint acceleration in the optimization variable c,l,next :

[0191] Thus, the collision awareness constraint is obtained, and its matrix and vector expressions are:

[0192] in, are the lower and upper limits of the allowable collision impulse specified by humans, and δt expresses the time interval between the current moment and the next moment.

[0193] It is understandable that when this leg is planned to be a swing leg or it has come into contact with the ground and become a supporting leg, this leg does not need a collision awareness constraint; on the contrary, when this leg should be a supporting leg in planning but has not yet landed (i.e., it has landed with a delay), this leg needs to activate the corresponding collision awareness constraint.

[0194] Specifically, the whole body of the legged robot is controlled according to the motion state, the reference state sequence and the foot-end reference state, including: if the planning should be for a swing leg, the foot-end trajectory tracking task is set to track the swing leg trajectory with collision awareness, the target plantar force of the plantar force tracking task is a preset value, the plantar force constraint in the plantar force inequality constraint is a preset value, and the collision awareness constraint is disabled; if the planning should be for a supporting leg, and the supporting leg touches the ground, the target linear acceleration of the foot-end trajectory tracking task is set to a preset value, the target plantar force of the plantar force tracking task is the plantar force provided by the MPC or other modules, the plantar force constraint is a friction cone constraint, and the collision awareness constraint is disabled; if the planning should be for a supporting leg, and the supporting leg does not touch the ground, the goal of the foot-end trajectory tracking task is set to track a velocity pointing to the impact surface, the target plantar force of the plantar force tracking task is a preset value, the plantar force constraint is a preset value, and the collision awareness constraint is enabled.

[0195] Among them, the module that can provide the target plantar force for the plantar force tracking task can include the above-mentioned MPC module, etc.; the preset values ​​of the target plantar force of the plantar force tracking task, the plantar force constraint in the inequality constraint, and the target linear acceleration of the tracking task can be 0; in the embodiment of the present application, the preset value is 0 for explanation.

[0196] It should be noted that embodiments of the present application can further predict the impact of collisions in joint space. For example, the collision impulse can be mapped to each joint to predict the torque effect caused by the collision on the joint. However, generally, as long as the impulse caused by the collision in the operating space is sufficiently small, the impact of the collision in the joint space will not be significant. To maximize the efficiency of WBC calculation, the collision awareness constraints in the embodiments of the present application are limited to the operating space, consistent with the trajectory optimization method in the above embodiments.

[0197] 4. Decision-making on the Implementation of Whole-Body Control

[0198] It can be understood that in the WBC of the embodiment of the present application, there are three unchanging tasks, namely, the trunk trajectory tracking task, the task of minimizing the change in joint torque, and the task of minimizing the change in plantar force; there are four unchanging constraints, namely, the floating basis dynamics equality constraint, the motor output torque saturation inequality constraint, the motor output speed saturation inequality constraint, and the motor output power saturation inequality constraint; the settings of other tasks and constraints are different in different situations, namely, the foot trajectory tracking task, the plantar force tracking task, the plantar force inequality constraint, and the collision awareness constraint.

[0199] It should be noted that, as shown in Figure 3, when a leg is planned as a swing leg, if the WBC activates the corresponding collision awareness constraint when that leg is acting as a swing leg, two situations may occur: First, because the collision awareness constraint is already in place in the TO, the reference linear velocity of the leg's foot is already very low. In this case, the optimal solution optimized by the WBC is not on the corresponding collision awareness constraint boundary, meaning that the corresponding collision awareness constraint in the WBC is ineffective. Second, the optimal solution optimized by the WBC is on the corresponding collision awareness constraint boundary, meaning that the constraint is effective, causing the leg's foot's velocity to be lower than the reference linear velocity given by the TO, preventing the leg's foot from touching down at the TO-specified touchdown time, resulting in a delayed landing. Therefore, when a leg is planned as a swing leg, if the WBC activates the collision awareness constraint for that leg, it either has no effect or has a negative effect. Therefore, the collision awareness constraint in the WBC is only activated when a delayed landing occurs.

[0200] Furthermore, when a leg is planned to be the supporting leg but has not yet been detected to have touched the ground (i.e., it has landed late), the goal of the foot trajectory tracking task is to track a velocity v pointing to the impact surface. imp n, where v imp It should not be set too small, otherwise it will lag for a long time; but there is no need to consider v imp When set too large, the impact velocity is too high. The collision awareness constraint can limit the foot end velocity. Each leg of the legged robot in the embodiment of the present application can be judged according to the decision tree shown in Figure 3 to determine the expressions of all tasks and constraints, that is, to determine the expression of WQP.

[0201] In summary, according to the legged robot planning and control method proposed in the embodiment of the present application, the foot-end dynamics model can be simplified and the trajectory of the discrete collision model can be optimized. The early collision problem can be planned from the swinging process, so that the impact after the early collision is within the target constraint range; at the same time, the whole-body dynamics model and discrete collision model of the footed robot can be controlled, and the delayed collision problem can be processed from the control link, so that the impact after the delayed collision is within the target constraint range; since both early and delayed collisions are taken into consideration at the same time, the embodiment of the present application can reduce the impact generated by the instantaneous collision between the sole of the foot and the ground, reduce the impact of the collision, and thereby improve the stability of the robot's motion state and increase the service life of the robot hardware.

[0202] Next, the legged robot planning and control device proposed in accordance with the embodiments of the present application will be described with reference to the accompanying drawings.

[0203] FIG4 is a block diagram of a legged robot planning and control device according to an embodiment of the present application.

[0204] As shown in FIG4 , the legged robot planning and control device 10 includes: an acquisition module 100 , a generation module 200 , a planning module 300 and a control module 400 .

[0205] Among them, the acquisition module 100 is used to obtain the motion instructions and motion state of the legged robot; the generation module 200 is used to generate the reference state sequence of the legged robot and the swing parameters of each leg at the next moment according to the motion instructions and motion state; the planning module 300 is used to determine the foot-end reference state of each leg according to the motion state and the swing parameters of each leg at the next moment, and for the leg that starts to swing at the next moment, the swing leg trajectory with collision awareness is planned according to the foot-end dynamics model and the discrete collision model, so that the impact after the early collision is within the target constraint range; the control module 400 is used to perform whole-body control of the legged robot according to the motion state, the reference state sequence and the foot-end reference state, wherein, for the support leg that should be planned and the support leg has not touched the ground, the whole-body control with collision awareness is performed according to the whole-body dynamics model and the discrete collision model, so that the impact after the delayed collision is within the target constraint range.

[0206] In the embodiment of the present application, the foot-end dynamics model of the legged robot planning and control device is:

[0207] in, is the linear acceleration of the foot end of the lth leg in the inertial system I, Λ c,l The mass matrix in the operating space, h c,l is the term that includes the Coriolis force, centripetal force and gravity in the operating space, and is the torque τ driving the joint jThe foot-end driving force after mapping to the operating space; the discrete collision model is:

[0208] in, is the projection operator on the collision normal vector, is the unit normal vector of the collision surface; are the velocities of object 1 before and after the collision, are the velocities of object 2 before and after the collision, c r is the coefficient of restitution.

[0209] In an embodiment of the present application, the planning module is further used to: obtain pre-defined state quantities and target constraints; construct a continuous-time trajectory optimization problem for the leg that starts swinging at the next moment based on the foot-end dynamics model, pre-defined state quantities and target constraints; discretize the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot-end of the swinging leg, and solve the problem to obtain a swinging leg trajectory with collision awareness.

[0210] In an embodiment of the present application, the pre-established state quantities of the planning module include one or more of the initial state of the swing leg foot end, the landing state of the swing leg foot end, the duration of the swing, and the expected maximum height of the swing leg foot end from the ground, and the target constraint conditions include one or more of the state equation equality constraint, the driving force inequality constraint, the trajectory height inequality constraint, the initial state equality constraint, the terminal state equality constraint, the collision awareness inequality constraint, and the terminal speed direction inequality constraint.

[0211] In the embodiment of the present application, the objective function of the continuous-time trajectory optimization problem of the planning module is:

[0212] in, and are weight matrices of different dimensions (mostly diagonal matrices); ‖a‖ Q =a T Qa represents the weighted modulus of vector a under the weight matrix Q; Reflects the task of the swing leg foot end landing state, and are the known terminal position and linear velocity of the foot end of the swing leg respectively; is the control quantity (i.e. linear acceleration) that minimizes the entire swing process, t0=0, t F =T SW , T SW is the duration of the swing; Reflects the maximum height of the swing leg off the ground, t h1 and t h2The two parameters specify the time period to reach the maximum height, satisfying t0≤t h1 ≤t h2 ≤t F . z c is the third component of the state quantity, that is, the component of the state quantity that represents the height of the foot end from the ground. The entire discrete time trajectory optimization problem is: f min ≤Λ c u k +h c ≤f max ,k=0,1,…,N-1, z c,min ≤S z x k ≤z c,max ,k=1,2,…,N-1,

[0213] in, is a vector composed of the control quantities of each frame; It is a diagonal weight matrix with different dimensions; the current foot state (including position and speed) feedback Reference terminal foot state (including position and velocity) is the selection matrix of foot end linear velocity; and The start frame and end frame of the swing height are manually specified to meet k imp It is the starting frame for opening the collision awareness constraint and the terminal velocity direction constraint, satisfying 0≤k imp ≤N.

[0214] In the embodiment of the present application, the optimization problem of the whole-body control of the legged robot planning and control device is:

[0215] Among them, A i is the task matrix, b i is the task vector, C j is the constraint matrix, lb j and ub j are the lower and upper bounds of the constraints, W i is the weight matrix, n task is the number of tasks, and nconstraint is the number of constraints.

[0216] In an embodiment of the present application, the tasks simultaneously processed by the whole-body control include multiple tasks including trunk trajectory tracking tasks, foot trajectory tracking tasks, plantar force tracking tasks, minimizing joint torque change tasks, and minimizing plantar force change tasks; the constraints simultaneously processed by the whole-body control with collision awareness include multiple tasks including: floating basis dynamics equality constraints, plantar force inequality constraints, joint output torque saturation inequality constraints, joint speed saturation inequality constraints, joint output power saturation inequality constraints, and collision awareness constraints.

[0217] In an embodiment of the present application, the whole body of the foot-type robot is controlled according to the motion state, the reference state sequence and the foot-end reference state, including: if the planning should be for a swing leg, the foot-end trajectory tracking task is set to track the swing leg trajectory with collision awareness, the target plantar force of the plantar force tracking task is a preset value, the plantar force constraint in the plantar force inequality constraint is a preset value, and the collision awareness constraint is disabled; if the planning should be for a supporting leg, and the supporting leg touches the ground, the target linear acceleration of the foot-end trajectory tracking task is set to a preset value, the target plantar force of the plantar force tracking task is the plantar force provided by the MPC or other modules, the plantar force constraint is a friction cone constraint, and the collision awareness constraint is disabled; if the planning should be for a supporting leg, and the supporting leg does not touch the ground, the target of the foot-end trajectory tracking task is set to track a speed pointing to the impact surface, the target plantar force of the plantar force tracking task is a preset value, the plantar force constraint is a preset value, and the collision awareness constraint is enabled.

[0218] In the embodiment of the present application, the whole-body dynamics model of the legged robot planning and control device is:

[0219] in, is the generalized mass matrix, is a term that includes the Coriolis force, centripetal force, and gravity. Indicates the output torque of the driving joint; 0 n×m represents a zero matrix of size n×m; and F c They are the augmented Jacobian matrix formed by stacking the contact Jacobian matrix of each supporting leg and the augmented plantar force formed by stacking the reaction force of the ground on the sole of each supporting leg; q g is the generalized joint space position, is the generalized joint space velocity, is the generalized joint space acceleration. The collision awareness constraint matrix and vector are expressed as:

[0220] in, are the lower and upper limits of the allowable collision impulse specified by humans, and δt expresses the time interval between the current moment and the next moment.

[0221] It should be noted that the aforementioned explanations of the embodiment of the legged robot planning and control method are also applicable to the legged robot planning and control device of this embodiment, and will not be repeated here.

[0222] According to the legged robot planning and control device proposed in the embodiment of the present application, the foot-end dynamics model can be simplified and the trajectory of the discrete collision model can be optimized. The early collision problem can be planned from the swinging process, so that the impact after the early collision is within the target constraint range; at the same time, the whole-body dynamics model and discrete collision model of the footed robot can be controlled, and the lag problem can be processed from the control link, so that the impact after the lag collision is within the target constraint range; since both early and lagging collisions are taken into consideration at the same time, the embodiment of the present application can reduce the impact generated at the moment of collision between the sole of the foot and the ground, reduce the impact of the collision, and thereby improve the stability of the robot's motion state and increase the service life of the robot hardware.

[0223] Figure 5 is a schematic diagram of the structure of a legged robot provided in an embodiment of the present application. The legged robot may include: a memory 501, a processor 502, and a computer program stored in the memory 501 and executable by the processor 502. When the processor 502 executes the program, it implements the legged robot planning and control method provided in the above embodiment.

[0224] Furthermore, the legged robot also includes: a communication interface 503, used for communication between the memory 501 and the processor 502; the memory 501, used to store computer programs that can be run on the processor 502; the memory 501 may include a high-speed RAM (Random Access Memory) memory, and may also include a non-volatile memory, such as at least one disk memory.

[0225] If the memory 501, processor 502, and communication interface 503 are implemented independently, the communication interface 503, memory 501, and processor 502 can be interconnected 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. Buses can be divided into address buses, data buses, control buses, etc. For ease of illustration, FIG5 shows only one thick line, but this does not mean that there is only one bus or only one type of bus.

[0226] Optionally, in a specific implementation, if the memory 501, the processor 502 and the communication interface 503 are integrated on a chip, the memory 501, the processor 502 and the communication interface 503 can communicate with each other through an internal interface.

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

[0228] An embodiment of the present application also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned legged robot planning and control method.

[0229] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or N embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0230] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of technical features indicated. Thus, a feature specified as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of this application, "N" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0231] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or N executable instructions for implementing a custom logical function or process step, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may be performed in a different order than shown or discussed, including performing functions in a substantially simultaneous manner or in a reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present application pertain.

[0232] It should be understood that various parts of the present application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiment, the N steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array, a field programmable gate array, etc.

[0233] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0234] Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limitations on the present application. Ordinary technicians in this field can change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A method for planning and controlling a legged robot, characterized in that, Including the following steps: Obtain the motion instructions and motion states of the legged robot; Generate the reference state sequence of the legged robot and the swing parameters of each leg at the next moment according to the motion instructions and the motion states; Determine the foot-end reference states of each leg according to the motion states and the swing parameters of each leg at the next moment. For the leg that starts to swing at the next moment, plan a collision-aware swing leg trajectory according to the foot-end dynamics model and the discrete collision model, so that the influence generated after the early collision is within the target constraint range; Perform whole-body control on the legged robot according to the motion states, the reference state sequence, and the foot-end reference states. Among them, for the leg that should be a support leg in the plan and the support leg does not touch the ground, perform collision-aware whole-body control according to the whole-body dynamics model and the discrete collision model, so that the influence generated after the lagged collision is within the target constraint range.

2. The foot-type robot planning and control method according to claim 1, characterized in that The foot-end dynamics model is as follows: Among them, is the linear acceleration of the end of the l-th leg in the inertial frame I, Λ c,l is the mass matrix in the operational space, h c,l is the term that includes the Coriolis force, centripetal force, and gravity in the operational space, is the torque τ driving the joint j the driving force at the foot end after mapping to the operational space; The discrete collision model is as follows: Among them, is the projection operator on the collision normal vector, is the unit normal vector of the collision surface; are the velocities of object 1 before and after the collision, respectively, are the velocities of the object 2 before and after the collision, respectively, and c r is the coefficient of restitution.

3. The foot-type robot planning and control method according to claim 1 or 2, characterized in that, The planning of the collision-aware swing leg trajectory according to the foot-end dynamics model and the discrete collision model includes: Obtain the pre-established state variables and target constraint conditions; Construct a continuous-time trajectory optimization problem for the leg that starts to swing at the next moment according to the foot-end dynamics model, the pre-established state variables, and the target constraint conditions; Discretize the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot-end of the swing leg, and solve this problem to obtain a collision-aware swing leg trajectory.

4. The foot-type robot planning and control method according to claim 3, wherein, The pre-established state variables include one or more of the initial state of the foot-end of the swing leg, the landing state of the foot-end of the swing leg, the duration of the swing, and the expected maximum lift-off height of the foot-end of the swing leg. The target constraint conditions include one or more of the state equation equality constraints, the driving force inequality constraints, the trajectory height inequality constraints, the initial state equality constraints, the terminal state equality constraints, the collision-awareness inequality constraints, and the end-segment velocity direction inequality constraints.

5. The foot-type robot planning and control method according to claim 3, characterized in that The objective function of the continuous-time trajectory optimization problem is as follows: Among them, And is a weight matrix with different dimensions (mostly diagonal matrices); ‖a‖ Q = a T Qa represents the weighted norm of vector a under the weight matrix Q; Tasks reflecting the landing state of the foot tip of the swinging leg, And are the known positions and linear velocities of the end points of the swinging leg respectively; Minimize the control quantity (i.e., linear acceleration) throughout the entire swinging process, where t0 = 0, t F = T SW , T SW is the duration of the swing; Reflects the swing leg leaving Maximum height task, t h1 and t h2 Two parameters specify the time period to reach the maximum height, satisfying t0 ≤ t h1 ≤ t h2 ≤ t F ; z c Is the third component of the state quantity, that is, the component in the state quantity representing the height of the foot end off the ground; The entire discrete-time trajectory optimization problem is as follows: f min ≤ Λ c u k + h c ≤ f max , k = 0, 1, …, N - 1, z c,min ≤S z x k ≤z c,max , k = 1, 2, …, N - 1, Among them, is a vector composed of the control amounts of each frame; is a diagonal weight matrix with different dimensions; the feedback of the end-effector state (including position and velocity) at the current moment Reference terminal foot-end state (including position and velocity) It is the selection matrix of the foot-end linear velocity; And The starting frame and the ending frame for artificially specifying the maintained swing height satisfy k imp is the starting frame for enabling the collision awareness constraint and the end velocity direction constraint, satisfying 0 ≤ k imp ≤ N.

6. The foot-type robot planning and control method according to claim 1, characterized in that The optimization problem of the overall control is as follows: s.t.lb j ≤C j χ≤ub j , j = 1, 2, …, nconstraint Among them, A i is the task matrix, b i is the task vector, C j is the constraint matrix, lb j and ub j are the lower and upper bounds of the constraints, W i is the weight matrix, n task is the number of tasks, and nconstraint is the number of constraints.

7. The foot-type robot planning and control method according to claim 1, characterized in that, The tasks simultaneously processed by the whole-body control include multiple tasks such as the trunk trajectory tracking task, the foot-end trajectory tracking task, the sole force tracking task, the task of minimizing the change in joint torque, and the task of minimizing the change in sole force. The constraints simultaneously processed by the collision-aware whole-body control include multiple constraints such as the floating-base dynamics equality constraint, the sole force inequality constraint, the joint output torque saturation inequality constraint, the joint rotational speed saturation inequality constraint, the joint output power saturation inequality constraint, and the collision-awareness constraint.

8. The foot-type robot planning and control method according to claim 7, characterized in that The performing whole-body control on the legged robot according to the motion states, the reference state sequence, and the foot-end reference states includes: If it should be a swing leg in the plan, set the foot-end trajectory tracking task to track the collision-aware swing leg trajectory, the target sole force of the sole force tracking task to a preset value, the sole force constraint in the sole force inequality constraint to a preset value, and disable the collision-awareness constraint; If it is planned to be a support leg and the support leg touches the ground, set the target linear acceleration of the foot-end trajectory tracking task to a preset value, the target sole force of the sole force tracking task to the sole force provided by MPC or other modules, the sole force constraint to the friction cone constraint, and disable the collision awareness constraint; If it is planned to be a support leg and the support leg does not touch the ground, set the target of the foot-end trajectory tracking task to track a velocity pointing to the impact surface, the target sole force of the sole force tracking task to a preset value, the sole force constraint to a preset value, and enable the collision awareness constraint.

9. The foot-type robot planning and control method according to claim 1, characterized in that The whole-body dynamics model is as follows: Among them, is the generalized mass matrix, is a term that includes the Coriolis force, centripetal force, and gravity, Represents the output torque of the driving joint; 0 n×m Represents a zero matrix of size n×m; and F c are the augmented Jacobian matrix stacked by the contact Jacobian matrices of each support leg and the augmented sole force stacked by the ground reaction forces on the soles of each support leg; q g is the generalized joint space position, is the generalized joint space velocity, is the generalized joint space acceleration; The expressions of the collision awareness constraint matrix and vector are as follows: Among them, are respectively the lower and upper limits of the allowable collision impulse specified by humans, and δt represents the time interval between the current moment and the next moment.

10. A legged robot planning and control device, characterized in that, including: An acquisition module for acquiring the motion instructions and motion states of the legged robot; A generation module for generating a reference state sequence of the legged robot and the swing parameters of each leg at the next moment according to the motion instructions and the motion states; A planning module for determining the foot-end reference state of each leg according to the motion states and the swing parameters of each leg at the next moment. For the leg that starts to swing at the next moment, plan a collision-aware swing leg trajectory according to the foot-end dynamics model and the discrete collision model, so that the influence generated after the early collision is within the target constraint range; A control module for performing whole-body control on the legged robot according to the motion states, the reference state sequence, and the foot-end reference state. Among them, for the leg that is planned to be a support leg and does not touch the ground, perform collision-aware whole-body control according to the whole-body dynamics model and the discrete collision model, so that the influence generated after the late collision is within the target constraint range.

11. A legged robot, characterized in that, including: A memory, a processor, and a computer program stored on the memory and executable on the processor. The processor executes the program to implement the legged robot planning and control method according to any one of claims 1-9.

12. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to be used to implement the legged robot planning and control method according to any one of claims 1-9.

Images