Methods, apparatus, robots, and storage media for planning and controlling legged robots.

Abstract

This application relates to the field of robotics technology, and more particularly to a planning and control method, apparatus, robot, and storage medium for a legged robot. The method includes the steps of: determining the foot end reference state of each leg based on the motion state of the legged robot and the next time swing parameter of each leg; planning a swing leg trajectory with collision awareness based on a foot end dynamics model and a discrete collision model for the leg that will start swinging in the next time; and controlling the entire legged robot based on the motion state, reference state sequence, and foot end reference state. In this method, if a leg is planned to be a support leg and the support leg is not in contact with the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model. This solves the problem in related technologies where passive flexible cushioning of collisions in legged robots is limited, the calculation of the control algorithm is large, the control accuracy is low, the degree of wear on the robot hardware is relatively high, the service life is short, and it is not possible to meet actual demands.

Description

Technical Field

[0001] This application relates to the field of robot technology, and particularly to a planning and control method, device, robot, and storage medium for a legged robot.

Background Art

[0002] Legged robots are developing rapidly, and many legged robots are not limited to static walking but are evolving in directions such as faster, more sensitive, more robust, safer, and with more functions. When a legged robot performs high-dynamic motion, the collision problem between its sole and the ground cannot be ignored.

[0003] In related technologies, a legged robot can be adjusted from the hardware aspect and the control algorithm aspect. In terms of the hardware aspect, a foot pad with a certain flexibility can be attached to the sole of the robot, or a buffer device such as a spring damper can be attached to the driving joint part of the robot. Alternatively, VIA (Variable Impedance Actuators) or VSA (Variable Stiffness Actuators) can be adopted to provide a desired passive mechanical impedance and achieve the effect of changing the joint impedance at the mechanical level. Thereby, the buffer effect can be adjusted according to actual needs. In terms of the control algorithm aspect, the contact speed between the sole of the foot and the ground can be made close to 0 in the motion plan.

[0004] However, there are certain shortcomings in the methods of the related technologies. In terms of hardware, the introduction of materials affects the response frequency to the robot system, and since the stiffness, damping, and inertial properties of the materials are constant, the cushioning effect cannot be adjusted according to actual needs. When using VIA or VSA, the overall structure is too complex, and the volume and weight are large, making it difficult to apply such actuators to legged robots that require high dynamic motion. In terms of control algorithms, when legged robots perform high dynamic motion, there may be a clear error between the actual motion trajectory and the planned trajectory of the swinging leg. This can cause premature or delayed ground contact of the sole, generate a huge plantar force impact, cause zero-position drift of the joints, and accelerate hardware aging, etc. [Overview of the project] [Problems that the invention aims to solve]

[0005] This invention provides a planning and control method, apparatus, robot, and storage medium for a legged robot to solve problems such as the limited passive flexible cushioning in related technologies for legged robot collisions, the large computational complexity of control algorithms, low control accuracy, high wear on robot hardware, short service life, and inability to meet actual application needs. [Means for solving the problem]

[0006] An embodiment of the first aspect of the present invention provides a method for planning and controlling a legged robot, comprising the steps of: acquiring motion commands and motion states of the legged robot; generating a reference state sequence and next-time swing parameters for each leg of the legged robot based on the motion commands and motion states; determining the foot end reference state of each leg based on the motion states and next-time swing parameters for each leg, and for the leg that starts swinging at the next time, planning a collision-aware swing leg trajectory based on a foot end dynamics model and a discrete collision model such that the effects that occur after an early collision are within the target constraint range; and performing whole-body control of the legged robot based on the motion states, the reference state sequence and the foot end reference state, wherein, in the case of a planned support leg that is not in contact with the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model such that the effects that occur after a delayed collision are within the target constraint range.

[0007] JPEG2026514522000002.jpg93170

[0008] The step of selectively planning a swing leg trajectory with collision awareness based on the foot-end dynamics model and the discrete collision model includes the steps of: obtaining a preset state variable and a target constraint; constructing a continuous-time trajectory optimization problem for the leg that will start swinging at the next time step based on the foot-end dynamics model, the preset state variable, and the target constraint; and discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the swing leg foot end, and solving the problem to obtain a swing leg trajectory with collision awareness.

[0009] Selectively, the preset state quantities include one or more of the initial state of the swing leg tip, the landing state of the swing leg tip, the duration of the swing, and the desired maximum ground clearance of the swing leg tip, and the target constraints include one or more of the state equation constraint, the driving force inequality constraint, the trajectory altitude inequality constraint, the initial state equation constraint, the terminal state equation constraint, the collision awareness inequality constraint, and the terminal velocity direction inequality constraint.

[0010] JPEG2026514522000003.jpg218170JPEG2026514522000004.jpg103170

[0011] JPEG2026514522000005.jpg50170

[0012] Selectively, the tasks that the whole-body control processes simultaneously include a torso trajectory tracking task, a foot trajectory tracking task, a plantar force tracking task, a task to minimize joint moment change, and a task to minimize plantar force change. The constraints that the collision-aware whole-body control processes simultaneously include a floating-based dynamics equation constraint, a plantar force inequality constraint, a joint output moment saturation inequality constraint, a joint rotation speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0013] The step of controlling the entire leg robot based on the motion state, the reference state sequence, and the foot tip reference state, selectably, sets a foot tip trajectory following task that follows the collision-aware swing leg trajectory if it is planned to be a swing leg, sets the target plantar force of the plantar force following task to a preset value, sets the plantar force constraint in the plantar force inequality constraint to a preset value, and disables the collision-aware constraint; and, if it is planned to be a support leg and the support leg is in contact with the ground, sets the target linear acceleration of the foot tip trajectory following task to a preset value, and sets the target plantar force of the plantar force following task to an MPC (Model Predictive) The steps include: setting the plantar force to be provided by Control (model predictive control) or another module, setting the plantar force constraint to a friction cone constraint, and disabling the collision awareness constraint; and setting the target of the foot trajectory tracking task to track a velocity toward the collision surface when the support leg is planned to be a support leg and the support leg is not in contact with the ground, setting the target plantar force of the plantar force tracking task to a preset value, setting the plantar force constraint to a preset value, and enabling the collision awareness constraint.

[0014] JPEG2026514522000006.jpg115170

[0015] An embodiment of a second aspect of the present invention provides a planning and control device for a legged robot, comprising: an acquisition module for acquiring motion commands and motion states of the legged robot; a generation module for generating a reference state sequence and next-time swing parameters for each leg of the legged robot based on the motion commands and motion states; a planning module for determining the foot end reference state of each leg based on the motion states and next-time swing parameters for each leg, and for the leg that starts swinging at the next time, planning a collision-aware swing leg trajectory based on a foot end dynamics model and a discrete collision model so that the effects occurring after an early collision are within the target constraint range; and a control module for whole-body control of the legged robot based on the motion states, the reference state sequence, and the foot end reference state, wherein, in the case of a planned support leg that is not in contact with the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model so that the effects occurring after a delayed collision are within the target constraint range.

[0016] JPEG2026514522000007.jpg87170

[0017] The planning module may be further used to obtain pre-set state variables and target constraints, construct a continuous-time trajectory optimization problem for the leg that will start swinging at the next time step based on the foot-end dynamics model, the pre-set state variables, and the target constraints, and discretize the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the swinging leg foot-end, and solve this problem to obtain a swinging leg trajectory that is aware of collisions.

[0018] Selectively, the preset state quantities of the planning module include one or more of the initial state of the swing leg tip, the landing state of the swing leg tip, the duration of the swing, and the desired maximum ground clearance of the swing leg tip, and the target constraints include one or more of the state equation constraint, the driving force inequality constraint, the trajectory altitude inequality constraint, the initial state equation constraint, the terminal state equation constraint, the collision awareness inequality constraint, and the terminal velocity direction inequality constraint.

[0019] JPEG2026514522000008.jpg214170JPEG2026514522000009.jpg106170

[0020] JPEG2026514522000010.jpg51170

[0021] Selectively, the tasks simultaneously processed by the whole-body control include a torso trajectory tracking task, a foot trajectory tracking task, a plantar force tracking task, a task to minimize joint moment change, and a task to minimize plantar force change. The constraints simultaneously processed by the collision-aware whole-body control include a floating-based dynamics equation constraint, a plantar force inequality constraint, a joint output moment saturation inequality constraint, a joint rotation speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0022] The steps of selectively controlling the entire leg robot based on the motion state, the reference state sequence, and the foot end reference state include setting a foot end trajectory following task that follows the collision-aware swing leg trajectory, setting the target plantar force of the plantar force following task to a preset value, setting the plantar force constraint in the plantar force inequality constraint to a preset value, and disabling the collision-aware constraint, and setting the target line of the foot end trajectory following task when the support leg is to be used as a support leg and the support leg is in contact with the ground. The steps include setting the acceleration to a preset value, setting the target plantar force for the plantar force tracking task to a plantar force provided by the MPC or another module, setting the plantar force constraint to a friction cone constraint, and disabling the collision awareness constraint; and setting the target of the foot trajectory tracking task to track a velocity directed toward the collision surface when the support leg is planned to be a support leg and the support leg is not in contact with the ground, setting the target plantar force for the plantar force tracking task to a preset value, setting the plantar force constraint to a preset value, and enabling the collision awareness constraint.

[0023] JPEG2026514522000011.jpg114170

[0024] An embodiment of 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 by the processor. By executing the program, the processor realizes the motion planning and control method of the legged robot described in the above embodiment.

[0025] An embodiment of the fourth aspect of the present application provides a computer-readable storage medium, in which a computer program is stored. By executing the program by a processor, the motion planning and control method of the legged robot described in the above embodiment is realized.

Advantages of the Invention

[0026] Thus, the present application has at least the following beneficial effects: The embodiments of the present application can simplify the foot-end dynamics model and optimize the trajectory of the discrete collision model. The early collision problem is planned from the swing process so that the influence generated after the early collision is within the target constraint range. At the same time, the whole-body dynamics model and the discrete collision model of the legged robot are controlled as a whole, and the delay problem is processed from the control stage so that the influence generated after the delayed collision is within the target constraint range. Considering both early and delayed collisions, the embodiments of the present application reduce the impact generated at the moment when the sole of the foot collides with the ground, reduce the influence of the collision, further improve the stability of the robot's motion state, and improve the service life of the robot hardware.

[0027] Additional aspects and advantages of the present application will be partially shown in the following description, will become apparent from the following description in part, or will be understood by the practice of the present application.

Brief Description of the Drawings

[0028] The above and / or additional aspects and advantages of the present application will become apparent and be readily understood from the description of the following embodiments with reference to the accompanying drawings. [Figure 1] It is a flowchart of the motion planning and control method of the legged robot according to the embodiment of the present application. [Figure 2] This is a schematic diagram of a collision-aware quadruped robot walking control system according to an embodiment of the present invention. [Figure 3] This is a schematic diagram of a decision-making tree for whole-body control with collision awareness according to an embodiment of the present invention. [Figure 4] This figure shows an example of a planning and control device for a legged robot according to an embodiment of the present invention. [Figure 5] This is a schematic diagram of the structure of a legged robot according to an embodiment of the present invention. [Modes for carrying out the invention]

[0029] The embodiments of the present application shown in the drawings will be described in detail below. In all drawings, the same or similar reference numerals indicate the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the drawings are illustrative and for illustrative purposes only, and should not be understood as limitations thereon.

[0030] In recent years, the field of legged robotics has developed rapidly, with an increasing number of mature and stable methods being applied to the motion planning and control of legged robots. Legged robots from many research institutions are evolving beyond static walking to become faster, more sensitive, more robust, safer, and more functional. When legged robots perform high-dynamic motion, problems that are negligible in low-dynamic motion become significant. For example, when legged robots run at high speeds, the problem of collision between the soles of the feet and the ground becomes a concern. The movement of a legged robot is achieved by the continuous switching of the supporting legs, and the soles of the feet must come into contact with the ground with each step. This is a deliberate collision with the environment and is a common collision problem when legged robots interact with the environment. If the controlled collision is inappropriate, the soles of the feet may collide violently with the ground, affecting the subsequent motion state of the robot, causing it to lose balance, fall over, and potentially shortening the lifespan or damaging the hardware.

[0031] In related technologies, solutions to the legged robot collision problem can be proposed from two perspectives: hardware and control algorithms.

[0032] In terms of hardware, foot pads with a certain degree of flexibility can be attached to the soles of the robot's feet, or shock absorbers such as spring dampers can be attached to the robot's drive joints, and VIA or VSA can also be employed. VIA and VSA can provide the desired passive mechanical impedance, achieving the effect of changing joint impedance at the mechanical level, thereby allowing the shock absorber effect to be adjusted according to actual needs. Here, the method of attaching shock absorbers to the robot is usually called the passive adaptive method.

[0033] In terms of control algorithms, to minimize the adverse effects of foot-to-ground collisions on the robot's state during walking, the contact velocity between the foot and the ground can be brought close to zero in the motion plan. When a small or lightweight foot-to-ground robot walks slowly on flat terrain, this method is effective because the controller's performance can usually ensure that the actual motion trajectory of the swinging leg essentially matches the planned trajectory. If the terrain is uneven, the terrain can be estimated using visual sensors such as cameras or laser radar, and by planning a foot motion trajectory that brings the contact velocity between the foot and the ground close to zero based on the estimated terrain, collision impacts due to premature or delayed ground contact can be avoided.

[0034] However, there are certain limitations to the methods used in related technologies.

[0035] From a hardware perspective, passive adaptive methods can reduce the effects of collisions to some extent, but they also have several problems. First, the introduction of these passive flexible materials affects the response frequency of the robot system, and if the control algorithm does not take this effect into account, the robot is prone to generating unstable vibrations during motion. Second, the damping effect of passive adaptive methods is limited, and since the stiffness, damping, and inertia characteristics are determined after the material of the damping device is decided, it is not possible to adjust the damping effect according to actual needs. Third, when using the VIA or VSA method, two motors can be used, one motor independently controlling the mechanical stiffness of the joint and the other motor generating torque, but the overall structure is too complex, and both the volume and weight are large, making it difficult to apply such actuators to legged robots that require high dynamic motion.

[0036] In terms of control algorithms, when a legged robot performs high-dynamic movements such as high-speed locomotion, the actual trajectory of the swinging leg has a significant error compared to the planned trajectory. This can cause premature or delayed ground contact of the sole. In the case of delayed ground contact, the sole of the foot has not yet touched the ground, but it is planned to support the torso. This can cause the leg to quickly stomp on the ground, resulting in a violent collision. At the moment of impact, a huge impact force from the sole of the foot is generated, and in the case of heavy legged robots, the impact force from such collisions is often greater. When a legged robot travels at high speed, violent impacts can cause zero-position drift of the joints, and prolonged and frequent collisions of the sole of the foot can accelerate hardware aging.

[0037] Based on the above, the control collisions in related technologies were treated as disturbances, and the focus was on the balance control problem of the robot after the collision, while ignoring the impact of such frequent, purposeful collisions on the robot hardware lifespan.

[0038] In response to the problems mentioned in the background technology above, this application provides a planning and control method for a legged robot. Hereinafter, an embodiment of the planning and control method, apparatus, robot, and storage medium of this application will be described with reference to the drawings.

[0039] Specifically, Figure 1 is a flowchart of the planning and control method for a legged robot according to an embodiment of the present invention. As shown in Figure 1, the planning and control method for the legged robot includes the following steps.

[0040] In step S101, the motion commands and motion state of the legged robot are acquired.

[0041] Here, the drive command includes the motion speed command and direction command entered by the user, and the motion state may include the state of the robot's torso, the state of the foot ends of each leg, and the state of each leg's contact with the ground.

[0042] To ensure understanding, the embodiments of the present invention can acquire motion commands and motion states of a legged robot in at least one manner, all actuators of the legged robots in the embodiments of the present invention are motors with force control capabilities, and the sensors provided include an IMU (Inertial Measurement Unit), a motor angle code disk, a motor moment sensor, etc., and the following embodiments will be described based on this.

[0043] For example, Figure 2 is a block diagram of a walking control system for a collision-aware quadruped robot according to an embodiment of the present invention, and this block diagram is applicable to both simulations and actual robots. The overall control framework mainly consists of five modules: 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 high system loads (e.g., 500 Hz, 1 kHz), IA-WBC operates at the same frequency, SRB-MPC operates at around 40 Hz, and the optimization problem in IA-TO is calculated only once before a leg swings. The following embodiments will describe the above system as an example.

[0044] Specifically, as shown in Figure 2, the embodiment of the present invention can estimate the torso state of a legged robot based on read IMU data and motor state information, where the IMU data may include 3D Euler angles, 3D angular velocity, and 3D linear acceleration, the motor state information may include motor rotation angle, rotation speed, and feedback moment, the torso state of the legged robot may include the point position of the torso, attitude unit quaternion, linear velocity, and angular velocity, and the state of the foot end of each leg may include the point position and linear velocity. The embodiment of the present invention can estimate the torso state using an EKF (Extended Kalman Filter), but is not specifically limited to this.

[0045] The state estimation in the embodiments of this application transmits information corresponding to different needs. As shown in Figure 2, for SRB-MPC, only the torso state is transmitted; for IA-TO, the torso state, the state of the foot ends of each leg, and the joint state are transmitted; for IA-WBC, the torso state, the state of the foot ends of each leg, the joint state, and the ground contact state of each leg are transmitted; and for rough planning modules, the torso state and the state of the foot ends of each leg can be transmitted. The above transmission process will be specifically explained in the following embodiments and will not be repeated here.

[0046] In step S102, a reference state sequence for the legged robot and the next time swing parameters for each leg are generated based on the motion command and motion state.

[0047] Here, the reference state sequence may be a series of reference quantities from the current time to a certain time in the future, and the reference state sequence may include a torso reference state sequence, a reference position sequence of the support leg's foot, a reference contact state sequence, and an MPC reference frame interval sequence, and the swing parameters include the initial state of the swing leg's foot (initial point position and linear velocity), the landing state (point position and linear velocity at landing), the duration of the swing, and the desired maximum ground clearance of the swing leg's foot.

[0048] To make it clear, embodiments of the present invention can generate a reference state sequence and next-time swing parameters for each leg of a leg-based robot using acquired motion commands and motion states, and embodiments of the present invention can generate the reference state sequence and next-time swing parameters in at least one way, for example, by performing motion planning using acquired information.

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

[0050] In step S103, the foot end reference state of each leg is determined based on the motion state and the next time swing parameters for each leg. For the leg that will start swinging in the next time step, a swing leg trajectory with collision awareness is planned based on the foot end dynamics model and the discrete collision model, such that the effects that occur after the early collision are within the target constraint range.

[0051] To make it easier to understand, the embodiment of this application can determine the foot end reference state of each leg based on the acquired motion state and the next time swing parameters of each leg of the leg-type robot. If there is a leg that starts swinging in the next time, the foot end dynamics model and discrete collision model are used to plan the swing leg trajectory with collision awareness so that the effects that occur after an early collision when the swinging leg moves are within the target constraint range.

[0052] Furthermore, slight undulations of the ground, errors in state estimation, and control errors can cause the foot of a legged robot's swing leg to contact the ground before the planned landing time. This is an early collision, and such unexpected early contacts often result in unexpected impact. This problem can be solved through planning; that is, the swing leg's foot end motion trajectory is planned with a collision awareness of one segment so that even if an early collision occurs, the impact is within the expected range. Thus, the embodiment of the present invention can construct a trajectory optimization problem applicable to a legged robot's swing leg, where the robot dynamics model used is a linearized dynamics model of the foot end of the swing leg in operating space, and the discrete collision model is a discrete collision dynamics model. In this trajectory optimization problem, the prediction time window is determined by the planned, i.e., pre-set foot end swing time. To enable the robot's computer to perform trajectory optimization online, the corresponding optimization problem is designed as an easily solvable QP (Quadratic Programming) problem.

[0053] Specifically, as shown in Figure 2, the rough planning module can provide the IA-TO with the landing position of the next swing for each leg, the swing duration, and the current reference contact state. The IA-TO module receives feedback information for state estimation and the reference information provided by the rough planning module, and generates an appropriate reference state (including the reference point position and linear velocity) for the foot end of each leg. The SRB-MPC module can solve an optimization problem in a single model predictive control based on the reference information provided by the rough planning module, obtaining the optimal torso reference state and the optimal reference plantar force. These are transmitted directly to the IA-WBC, and the SRB-MPC can solve the optimization problem once in about one frame. If there is no need to solve it, its output is not updated, meaning the output data is zero-order retained.

[0054] For the support leg, its reference state is to remain motionless on the ground. For the leg that will soon begin to swing, a collision-aware trajectory optimization problem is solved. For the swinging leg, the optimal solution of the trajectory optimization is interpolated to obtain the reference state at the current time. Here, the IA-TO module solves the trajectory optimization only once, before a leg swings, with very little computation at other times. The IA-TO module finally transmits the reference state of the foot end of each leg, along with the reference contact state provided by the rough plan, to the IA-WBC.

[0055] In the embodiment of the present invention, the step of planning a swing leg trajectory with collision awareness based on a foot-end dynamics model and a discrete collision model includes the steps of: obtaining a predetermined state variable and a target constraint; constructing a continuous-time trajectory optimization problem for the leg that will start swinging at the next time step based on the foot-end dynamics model, the predetermined state variable and the target constraint; discretizing the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot end of the swing leg, and solving the problem to obtain a swing leg trajectory with collision awareness.

[0056] JPEG2026514522000012.jpg19170

[0057] The preset state quantities in the embodiment of the present invention include one or more of the initial state of the swing leg tip, the landing state of the swing leg tip, the duration of the swing, and the desired maximum ground clearance of the swing leg tip, and the target constraints include one or more of the state equation constraint, the driving force inequality constraint, the trajectory altitude inequality constraint, the initial state equation constraint, the terminal state equation constraint, the collision awareness inequality constraint, and the terminal velocity direction inequality constraint.

[0058] To make it understandable, each leg needs to solve a trajectory optimization problem to obtain the optimal foot-end swing trajectory before starting its swing. Considering the limited computing power of the computer on an actual robot, the same leg's trajectory optimization problem is not solved repeatedly during the foot-end swing. Here, since there is no essential difference in the expression of the trajectory optimization problem for each leg, for the sake of simplicity of expression, in the embodiments of this application, the subscript l indicating the leg number is omitted in all cases, only when it does not cause misunderstanding.

[0059] JPEG2026514522000013.jpg30170

[0060] JPEG2026514522000014.jpg57170

[0061] JPEG2026514522000015.jpg58170

[0062] JPEG2026514522000016.jpg39170

[0063] JPEG2026514522000017.jpg79170

[0064] JPEG2026514522000018.jpg131170

[0065] JPEG2026514522000019.jpg43170

[0066] JPEG2026514522000020.jpg29170

[0067] JPEG2026514522000021.jpg44170

[0068] JPEG2026514522000022.jpg45170

[0069] JPEG2026514522000023.jpg87170

[0070] JPEG2026514522000024.jpg126170

[0071] JPEG2026514522000025.jpg32170

[0072] JPEG2026514522000026.jpg214170JPEG2026514522000027.jpg28170

[0073] III. Discrete-Time Orbit Optimization Problem To facilitate computer solving of the above continuous-time orbit optimization problem, embodiments of the present invention may be further discretized, where embodiments of the present invention may use at least one type of discretization method, for example, by replacing the integral term in the objective function with a sum of finite terms, or by replacing the state equation constraint with a Taylor expansion that ignores higher-order terms.

[0074] JPEG2026514522000028.jpg218170JPEG2026514522000029.jpg8170

[0075] As can be understood, many orbital optimization problems under study are all highly nonlinear and are usually all nonconvex optimization problems. Therefore, while it is possible to find local optima of these orbital optimization problems using some specialized nonlinear solvers, finding a global optimum is not guaranteed. The orbital optimization problem in the embodiment of this application is a QP problem and belongs to the typical convex optimization problem. Therefore, as long as a set of nonempty solutions exists for it, the optimum found by the optimizer is necessarily a global optimum.

[0076] JPEG2026514522000030.jpg102170

[0077] Since subsequent control programs require corresponding reference trajectories at any given time, the interpolation method converts N discrete points into a continuous curve. Here, the embodiment of the present invention can achieve interpolation using at least one method, and in order to maximize computational efficiency, the embodiment of the present invention can use the simplest linear interpolation.

[0078] JPEG2026514522000031.jpg37170

[0079] However, this type of 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, which involves sacrificing some interpolation accuracy to maximize computational efficiency.

[0080] In step S104, the legged robot is controlled as a whole based on the motion state, reference state sequence, and foot end reference state. Here, if a support leg is planned to be a support leg but is not in contact with the ground, collision-aware whole-body control is performed based on the whole-body dynamics model and discrete collision model so that the effects that occur after a delayed collision remain within the target constraint range.

[0081] To make it easier to understand, as shown in Figure 2, the embodiment of the present invention solves WQP (Weighted Quadratic Programming) based on state estimation feedback information and reference information from the SRB-MPC and IA-TO modules to calculate the optimal joint moment command, thereby controlling the entire legged robot and handling the delayed collision problem.

[0082] Delayed collision refers to a situation where, due to certain causes (such as slight unevenness in the ground, errors in state estimation, and control errors), the foot does not make contact with the ground at the initially planned time, inevitably resulting in contact with the ground occurring later than initially planned. Without appropriate control collision, such unexpected delayed collisions are likely to cause unexpected impact. The delayed collision problem and the early collision problem are fundamentally different; that is, a delayed collision occurs outside the planned swing process, making it impossible to accurately know the distance between the sole of the foot and the ground (which should ideally be zero), and it is also impossible to determine when contact will occur with the ground (which should ideally occur on time). Therefore, it is necessary to effectively resolve this at the control stage. Based on the core concept of one-step predictive control, the embodiment of this application can use the WBC (Whole Body Control) method of collision awareness.

[0083] Specifically, in the following embodiment, the WBC method according to the embodiment of the present invention will be explained in detail using a quadruped robot as an example. Here, the collision awareness constraint is not always on, and the task objectives and constraint settings for whole-body control differ depending on the situation. Therefore, the embodiment of the present invention can be expressed in the form of a decision tree as shown in Figure 3, and the whole-body control of collision awareness according to the embodiment of the present invention is specifically as follows.

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

[0085] To make it understandable, since whole-body control handles inequality constraints such as collision awareness constraints, the implementation of whole-body control should be based on optimization. At the same time, considering that the computational resources on an actual robot are limited and there are many programs (e.g., communication, control algorithms, etc.) that need to run simultaneously, the embodiment of this application can employ a whole-body control method implemented based on WQP.

[0086] JPEG2026514522000032.jpg53170

[0087] JPEG2026514522000033.jpg38170

[0088] JPEG2026514522000034.jpg75170

[0089] JPEG2026514522000035.jpg45170

[0090] 2. Task Setting In the embodiment of the present invention, the tasks simultaneously processed by whole-body control include a torso trajectory tracking task, a foot trajectory tracking task, a plantar force tracking task, a task to minimize joint moment change, and a task to minimize plantar force change. The constraints simultaneously processed by whole-body control with collision awareness include a floating-based dynamics equation constraint, a plantar force inequality constraint, a joint output moment saturation inequality constraint, a joint rotation speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0091] JPEG2026514522000036.jpg44170

[0092] JPEG2026514522000037.jpg79170

[0093] JPEG2026514522000038.jpg114170

[0094] JPEG2026514522000039.jpg95170

[0095] JPEG2026514522000040.jpg73170

[0096] JPEG2026514522000041.jpg18170

[0097] JPEG2026514522000042.jpg101170

[0098] (4) Task of minimizing the amount of joint moment change: Because the actual motor force control bandwidth is limited, excessively large changes in output moment cannot be realized in the actual machine, and if the actual motor cannot operate according to the optimal solution of WBC, control errors tend to become large. For this reason, the embodiment of the present invention takes into consideration "reduction of the amount of joint moment change" in WBC, so that the actual motor can operate according to the optimal solution of WBC more easily.

[0099] JPEG2026514522000043.jpg54170

[0100] (4) Task of minimizing the amount of change in plantar force: In order to make the change in plantar force smoother (to make the change in the state of the core smoother), the embodiment of the present invention may consider the task of "reducing the amount of change in plantar force" in WBC.

[0101] JPEG2026514522000044.jpg49170

[0102] III. Setting Constraints

[0103] JPEG2026514522000045.jpg50170

[0104] JPEG2026514522000046.jpg42170

[0105] JPEG2026514522000047.jpg26170

[0106] JPEG2026514522000048.jpg166170

[0107] JPEG2026514522000049.jpg60170

[0108] JPEG2026514522000050.jpg63170

[0109] (4) Joint rotation speed saturation inequality constraint: Similar to the joint output moment saturation inequality constraint, even under no load conditions, there is an upper limit to the actual motor rotation speed. In WBC, this limitation of the actual motor rotation speed must be taken into consideration, allowing each joint to coordinate better to achieve multiple desired tasks simultaneously.

[0110] JPEG2026514522000051.jpg90170

[0111] (5) Joint output power saturation inequality constraint: Similar to the joint output moment saturation inequality constraint, there is an upper limit to the rotational speed of the actual motor, and the WBC must take into account the limitations of the actual motor rotational speed, which allows each joint to be better coordinated to achieve multiple desired tasks simultaneously. Considering the possibility that the environment may do work on the motor, in the embodiments of this application, it is possible to restrict only the output power of the joints and not the input power of the joints.

[0112] JPEG2026514522000052.jpg86170

[0113] (6) Collision awareness constraint: The concept of collision awareness constraint in WBC and in TO is similar, and the discrete collision model used, i.e., the discrete collision dynamics model, is also the same.

[0114] JPEG2026514522000053.jpg46170

[0115] JPEG2026514522000054.jpg165170

[0116] To make it clear, if this leg is planned to be a swing leg or is already in contact with the ground and acting as a support leg, then this leg does not require a collision awareness constraint. Conversely, if this leg should be a support leg in the plan but is not yet landing (i.e., delayed landing), then this leg needs to have the corresponding collision awareness constraint turned on.

[0117] Specifically, the steps for controlling the entire legged robot based on motion state, reference state sequence and foot end reference state include: if the planned leg should be a swinging leg, setting the foot end trajectory tracking task to tracking a swinging leg trajectory with collision awareness, setting the target plantar force for the plantar force tracking task to a preset value, setting the plantar force constraint in the plantar force inequality constraint to a preset value, and disabling the collision awareness constraint; if the planned leg should be a support leg and the support leg is in contact with the ground, setting the target linear acceleration for the foot end trajectory tracking task to a preset value, setting the target plantar force for the plantar force tracking task to a plantar force provided by the MPC or other module, setting the plantar force constraint to a friction cone constraint, and disabling the collision awareness constraint; and if the planned leg should be a support leg and the support leg is not in contact with the ground, setting the target of the foot end trajectory tracking task to tracking a velocity directed toward the impact surface, setting the target plantar force for the plantar force tracking task to a preset value, setting the plantar force constraint to a preset value, and enabling the collision awareness constraint.

[0118] Here, the module that can provide the target plantar force for the plantar force tracking task may be the MPC module mentioned above, and the preset values ​​for the target plantar force for the plantar force tracking task, the plantar force constraint in the inequality constraint, and the target linear acceleration for the tracking task may be 0. In the embodiment of this application, the preset values ​​will be described as 0.

[0119] Furthermore, the embodiments of this application may also predict the effects of collisions in the joint space, for example, by mapping the collision impact force to each joint and predicting the moment effect that the collision has on the joint. However, if the impulse caused by the collision in the operating space is sufficiently small, the effects of the collision in the joint space will not be significant. In order to maximize the WBC calculation efficiency, the collision awareness constraint in the embodiments of this application is limited to the operating space and is consistent with the trajectory optimization method in the above embodiments.

[0120] IV. Strategies for achieving whole-body control

[0121] To make it easier to understand, in the WBC of the embodiment of the present invention, three tasks are always set: namely, the torso trajectory tracking task, the task of minimizing the change in joint moment, and the task of minimizing the change in plantar force. Four constraints are always set: namely, the floating-based dynamic equation constraint, the motor output moment saturation inequality constraint, the motor output rotational speed saturation inequality constraint, and the motor output power saturation inequality constraint. Other tasks and constraints vary depending on the situation and are the foot end trajectory tracking task, the plantar force tracking task, the plantar force inequality constraint, and the collision awareness constraint, respectively.

[0122] As shown in Figure 3, if this leg is a swing leg in the plan, when WBC turns on the corresponding collision awareness constraint when designating that leg as a swing leg, there are two cases: one is that TO already has a collision awareness constraint, and the reference linear velocity of the leg's foot is small, in which case the optimal solution optimized by WBC is not on the corresponding collision awareness constraint boundary, i.e., the corresponding collision awareness constraint in WBC does not function; the other is that the optimal solution optimized by WBC is on the corresponding collision awareness constraint boundary, i.e., the constraint functions, and the motion velocity of the leg's foot becomes lower than the reference linear velocity given by TO, as a result the leg's foot cannot complete contact at the contact time point specified by TO, i.e., a delayed landing occurs. Thus, when a leg is a swing leg in the plan, turning on the collision awareness constraint for that leg in WBC results in either no function or a negative function, so the collision awareness constraint in WBC is turned on only when a delayed landing occurs.

[0123] JPEG2026514522000055.jpg55170

[0124] As described above, the planning and control method for a legged robot according to the embodiment of the present invention can simplify the foot-end dynamics model and optimize the trajectory of the discrete collision model. The early collision problem is planned from the swing process so that the effects that occur after the early collision are within the target constraint range, and at the same time, the whole-body dynamics model and discrete collision model of the legged robot are controlled as a whole. The delayed collision problem is handled from the control stage so that the effects that occur after the delayed collision are within the target constraint range. Because both early and delayed collisions are considered simultaneously, the embodiment of the present invention reduces the impact that occurs at the moment the sole of the foot collides with the ground, reduces the effects of the collision, further improves the stability of the robot's motion state, and extends the service life of the robot hardware.

[0125] Next, with reference to the drawings, a planning and control device for a legged robot according to an embodiment of the present invention will be described.

[0126] Figure 4 is a schematic block diagram of a planning and control device for a legged robot according to an embodiment of the present invention.

[0127] As shown in Figure 4, the planning and control device 10 of the legged robot comprises an acquisition module 100, a generation module 200, a planning module 300, and a control module 400.

[0128] Here, the acquisition module 100 is used to acquire the motion commands and motion state of the legged robot; the generation module 200 is used to generate the reference state sequence and next time swing parameters for each leg of the legged robot based on the motion commands and motion state; the planning module 300 determines the foot end reference state of each leg based on the motion state and next time swing parameters for each leg, and for the leg that will start swinging in the next time, it is used to plan a collision-aware swing leg trajectory based on a foot end dynamics model and a discrete collision model so that the effects that occur after an early collision are within the target constraint range; and the control module 400 is used to control the entire legged robot based on the motion state, reference state sequence and foot end reference state, and here, if a leg that should be a support leg in the plan and that support leg is not in contact with the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model so that the effects that occur after a delayed collision are within the target constraint range.

[0129] JPEG2026514522000056.jpg94170

[0130] In the embodiment of the present invention, the planning module is further used to obtain a set state quantity and a target constraint, construct a continuous-time trajectory optimization problem for the leg that will start swinging at the next time step based on the set state quantity and target constraint, discretize the continuous-time trajectory optimization problem to obtain a discrete-time trajectory optimization problem for the foot of the swinging leg, and solve the problem to obtain a swinging leg trajectory that is aware of collisions.

[0131] In the embodiment of the present invention, the preset state quantities of the planning module include one or more of the initial state of the swing leg tip, the landing state of the swing leg tip, the duration of the swing, and the desired maximum ground clearance of the swing leg tip, and the target constraints include one or more of the state equation constraint, the driving force inequality constraint, the trajectory altitude inequality constraint, the initial state equation constraint, the terminal state equation constraint, the collision awareness inequality constraint, and the terminal velocity direction inequality constraint.

[0132] JPEG2026514522000057.jpg224170JPEG2026514522000058.jpg105170

[0133] JPEG2026514522000059.jpg58170

[0134] In the embodiment of the present invention, the tasks simultaneously processed by whole-body control include a torso trajectory tracking task, a foot trajectory tracking task, a plantar force tracking task, a task to minimize joint moment change, and a task to minimize plantar force change. The constraints simultaneously processed by whole-body control with collision awareness include a floating-based dynamics equation constraint, a plantar force inequality constraint, a joint output moment saturation inequality constraint, a joint rotation speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

[0135] In the embodiment of the present invention, the steps of controlling the entire leg robot based on motion state, reference state sequence and foot end reference state include: if the planned leg should be a swinging leg, setting the foot end trajectory tracking task to tracking a swinging leg trajectory with collision awareness, setting the target plantar force of the plantar force tracking task to a preset value, setting the plantar force constraint in the plantar force inequality constraint to a preset value, and disabling the collision awareness constraint; if the planned leg should be a support leg and the support leg is in contact with the ground, setting the target linear acceleration of the foot end trajectory tracking task to a preset value, setting the target plantar force of the plantar force tracking task to a plantar force provided by the MPC or other module, setting the plantar force constraint to a friction cone constraint, and disabling the collision awareness constraint; and if the planned leg should be a support leg and the support leg is not in contact with the ground, setting the target of the foot end trajectory tracking task to tracking a velocity directed toward the impact surface, setting the target plantar force of the plantar force tracking task to a preset value, setting the plantar force constraint to a preset value, and enabling the collision awareness constraint.

[0136] JPEG2026514522000060.jpg119170

[0137] Furthermore, the interpretation and explanation of the aforementioned embodiment of the planning and control method for the legged robot also apply to the planning and control device of the legged robot in that embodiment, and will not be repeated here.

[0138] The planning and control method for a legged robot according to the embodiment of the present invention can simplify the foot-end dynamics model and optimize the trajectory of the discrete collision model. It plans the early collision problem from the swing process so that the effects that occur after the early collision are within the target constraint range, and at the same time controls the whole body dynamics model and discrete collision model of the legged robot. It also processes the delayed collision problem from the control stage so that the effects that occur after the delayed collision are within the target constraint range. Because it considers both early and delayed collisions simultaneously, the embodiment of the present invention reduces the impact that occurs at the moment the sole of the foot collides with the ground, reduces the impact of the collision, further improves the stability of the robot's motion state, and extends the service life of the robot hardware.

[0139] Figure 5 is a schematic diagram of the structure of a legged robot according to an embodiment of the present invention. The legged robot includes a memory 501, a processor 502, and a computer program stored in the memory 501 that can be run on the processor 502. When the processor 502 executes the program, it implements the planning and control method for the legged robot provided in the above embodiment.

[0140] Furthermore, the legged robot further includes a communication interface 503 used for communication between the memory 501 and the processor 502, and a memory 501 for storing computer programs that can run on the processor 502. The memory 501 may include high-speed RAM (Random Access Memory) memory or non-volatile memory, such as at least one magnetic disk memory.

[0141] If the memory 501, processor 502, and communication interface 503 are implemented independently, the communication interface 503, memory 501, and processor 502 can be connected via a bus to complete communication with each other. The bus may be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, or an EISA (Extended Industry Standard Architecture) bus. Buses can be classified into address buses, data buses, control buses, etc. For illustrative purposes, Figure 5 shows only one thick line, but this does not indicate that there is only one bus or only one type of bus.

[0142] In a more concrete implementation, if the memory 501, processor 502, and communication interface 503 are integrated onto a single chip, the memory 501, processor 502, and communication interface 503 can complete communication with each other via an internal interface.

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

[0144] The embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, the above-described method for planning and controlling a legged robot is realized.

[0145] In this specification, any reference to terms such as “one embodiment,” “several embodiments,” “example,” “specific example,” or “several examples” means that a particular feature, structure, material, or property described with reference to such embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the general expressions of the above terms do not necessarily apply to the same embodiment or example. In addition, any particular feature, structure, material, or property described may be incorporated in an appropriate manner in any one or more embodiments or examples. Furthermore, those skilled in the art can combine and combine the various embodiments or examples and the features relating to the various embodiments or examples described herein without contradiction.

[0146] Furthermore, terms such as "first" and "second" are merely for the purpose of explaining the purpose and cannot be considered to indicate or implicitly suggest relative importance, or to implicitly indicate a number that specifies a technical feature. Therefore, a feature limited as "first" or "second" may be explicitly or implicitly indicated to include one such feature, and in the description of this application, unless otherwise specifically and clearly defined, the concept of "N items" refers to at least two, for example, two or three.

[0147] Any process or method description in a flowchart or otherwise described herein can be understood as indicating one or more modules, fragments, or parts containing code for executable instructions to implement a custom logical function or step of a process, and the scope of preferred embodiments of this application includes other implementations, and the functions do not have to be performed in the order shown or discussed, including performing the functions essentially concurrently or in reverse order based on the relevant functions, as should be understood by those skilled in the art.

[0148] It should be understood that each part of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by an appropriate instruction execution system. If implemented in hardware, as in other embodiments, it can be implemented in any or a combination of techniques known in the art, such as discrete logic circuits having logic gate circuits for implementing logic functions for data signals, dedicated integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays, and field-programmable gate arrays.

[0149] Those skilled in the art will understand that the implementation of all or some of the steps described in the above embodiment of the method can be completed by instructing the relevant hardware by program, the program being stored on a computer-readable storage medium, and when the program is executed, it includes one or a combination of the steps of the embodiment of the method.

[0150] Although embodiments of the present application have been described above, it should be understood that these embodiments are illustrative and not intended to be understood as limitations of the present application, and that those skilled in the art may make changes, modifications, substitutions and variations to the embodiments within the scope of the present application. Cross-reference to related applications

[0151] This application is proposed based on a Chinese patent application with patent application number 202311511514.9, filed on November 13, 2023, claiming priority and interest of said Chinese patent application, and the entire contents of said Chinese patent application are incorporated herein by reference.

Claims

1. A method for planning and controlling a legged robot, Steps to acquire motion commands and motion status of a legged robot, The steps include generating a reference state sequence for the legged robot and the next time swing parameter for each leg based on the motion command and the motion state, Based on the aforementioned motion state and the next time swing parameters for each leg, the foot end reference state of each leg is determined, and for the leg that starts swinging at the next time, a swing leg trajectory with collision awareness is planned based on the foot end dynamics model and the discrete collision model so that the effects that occur after the early collision are within the target constraint range. A method for planning and controlling a legged robot, comprising the steps of controlling the entire legged robot based on the motion state, the reference state sequence, and the foot end reference state, wherein, when a support leg is planned to be a support leg and the support leg is not in contact with the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model so that the effects that occur after a delayed collision are within the target constraint range.

2. The method for planning and controlling a legged robot according to Claim 1.

3. The step of planning a swing leg trajectory with collision awareness based on the foot-end dynamics model and the discrete collision model is: Steps include obtaining pre-set state variables and target constraints, The steps include constructing a continuous-time trajectory optimization problem for the leg that starts swinging at the next time step, based on the foot-end dynamics model, the preset state variables, and the target constraints, A method for planning and controlling a legged robot according to claim 1 or 2, characterized by comprising the step of discretizing the continuous-time trajectory optimization problem, obtaining a discrete-time trajectory optimization problem for the swing leg foot, and solving the problem to obtain a swing leg trajectory that is collision-aware.

4. The method for planning and controlling a legged robot according to claim 3, characterized in that the preset state quantities include one or more of the initial state of the swing leg's foot tip, the landing state of the swing leg's foot tip, the duration of the swing, and a desired maximum ground clearance of the swing leg's foot tip, and the target constraint conditions include one or more of the state equation constraint, driving force inequality constraint, trajectory altitude inequality constraint, initial state equation constraint, terminal state equation constraint, collision awareness inequality constraint, and terminal velocity direction inequality constraint.

5. The method for planning and controlling a legged robot according to claim 3.

6. The planning and control method for a legged robot according to claim 1.

7. The planning and control method for a legged robot according to claim 1, characterized in that the tasks simultaneously processed by the whole-body control include a torso trajectory tracking task, a foot trajectory tracking task, a plantar force tracking task, a task to minimize joint moment change, and a task to minimize plantar force change, and the constraints simultaneously processed by the collision-aware whole-body control include a floating-based dynamic equation constraint, a plantar force inequality constraint, a joint output moment saturation inequality constraint, a joint rotation speed saturation inequality constraint, a joint output power saturation inequality constraint, and a collision awareness constraint.

8. The step of controlling the entire body of the foot-type robot based on the aforementioned motion state, the aforementioned reference state sequence, and the aforementioned foot end reference state is: The steps include setting a foot-end trajectory tracking task that follows the swing leg trajectory with collision awareness, determining that the swing leg should be the swing leg in the plan, setting the target plantar force of the plantar force tracking task to a predetermined value, setting the plantar force constraint in the plantar force inequality constraint to a predetermined value, and disabling the collision awareness constraint, If a support leg is to be used in the plan and the support leg is in contact with the ground, the steps include setting the target linear acceleration of the foot trajectory tracking task to a predetermined value, setting the target plantar force of the plantar force tracking task to a plantar force provided by the MPC or other module, setting the plantar force constraint to a friction cone constraint, and disabling the collision awareness constraint, A method for planning and controlling a legged robot according to claim 7, comprising the steps of: setting the target of the foot trajectory tracking task to track a speed directed toward the collision surface when the support leg is planned to be a support leg and is not in contact with the ground; setting the target plantar force of the plantar force tracking task to a preset value; setting the plantar force constraint to a preset value; and enabling the collision awareness constraint.

9. The method for planning and controlling a legged robot according to claim 1.

10. A planning and control system for a legged robot, An acquisition module for acquiring motion commands and motion status of a legged robot, A generation module for generating a reference state sequence and next time swing parameters for each leg of the leg-type robot based on the motion command and the motion state, Based on the aforementioned motion state and the next time swing parameters for each leg, the foot end reference state of each leg is determined, and for the leg that starts swinging at the next time, a planning module is provided for planning a swing leg trajectory with collision awareness based on a foot end dynamics model and a discrete collision model, such that the effects that occur after the early collision are within the target constraint range. A planning and control device for a legged robot, comprising: a control module for whole-body control of the legged robot based on the motion state, the reference state sequence, and the foot end reference state, wherein, when a support leg is planned to be a support leg and is not in contact with the ground, collision-aware whole-body control is performed based on a whole-body dynamics model and a discrete collision model so that the effects occurring after a delayed collision are within the target constraint range.

11. A legged robot comprising a memory, a processor, and a computer program stored in the memory and operable on the processor, wherein the processor executes the program to realize the planning and control method for the legged robot described in any one of claims 1 to 9.

12. A computer-readable storage medium in which a computer program is stored, characterized in that the program is executed by a processor to realize a planning and control method for a legged robot described in any one of claims 1 to 9.

Images