A quadruped robot center of mass trajectory planning method, device and storage medium

Abstract

The present application discloses a method, device and storage medium for planning the center of mass trajectory of a quadruped robot, which are used to improve the stability of the robot during movement. The method of the present application includes: obtaining the center of mass reference trajectory of the robot; obtaining the state parameter set of the current control cycle of the robot according to the sensor; obtaining the first control input according to the center of mass reference trajectory, cost function, target constraint condition and the state parameter set of the current control cycle; obtaining the target state parameter set of at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input; obtaining the target control input corresponding to at least one subsequent control cycle according to the center of mass reference trajectory, cost function and target constraint condition and the target state parameter set of at least one subsequent control cycle; controlling the robot to move along the optimized trajectory according to the target control input corresponding to at least one subsequent control cycle.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to a method, device and storage medium for center of mass trajectory planning of a quadruped robot. Background Art

[0002] Compared with traditional wheeled robots and tracked robots, legged robots have the advantage of good obstacle crossing performance. The multi-degree-of-freedom leg and foot structure of legged robots enables them to cope with some more complex terrains. As a bionic robot of common mammals, quadruped robots have good application prospects and potential.

[0003] In the prior art, when a speed instruction is given to a quadruped robot, the robot will walk in a straight line according to the specified speed and the planned trajectory. However, when encountering stairs or other scenes with uneven roads, since the footholds in the trajectory are set according to flat roads, these footholds are not applicable in non-flat road environments. If the robot still walks in a straight line according to the previous trajectory, it may lose balance or even fall, resulting in poor stability of the quadruped robot when performing stair climbing tasks or walking on rough terrain. Summary of the invention

[0004] The present application provides a method, device and storage medium for planning the center of mass trajectory of a quadruped robot, which are used to improve the stability of the quadruped robot during its movement.

[0005] The first aspect of the present application provides a method for planning the center of mass trajectory of a quadruped robot, comprising:

[0006] Get the robot's center of mass reference trajectory;

[0007] Acquire a set of state parameters of the current control cycle of the robot according to the sensor;

[0008] Acquire a first control input according to the mass center reference trajectory, the cost function, the target constraint condition and the state parameter set of the current control cycle;

[0009] Acquire a target state parameter set for at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input;

[0010] Obtaining a target control input corresponding to at least one subsequent control cycle according to the mass center reference trajectory, the cost function, the target constraint condition, and the target state parameter set of at least one subsequent control cycle;

[0011] The robot is controlled to move along the optimized trajectory according to the target control input corresponding to the subsequent at least one control cycle.

[0012] Optionally, the acquiring the first control input according to the mass center reference trajectory, the cost function, the target constraint condition and the state parameter set of the current control cycle includes:

[0013] Determining a reference state parameter set in the center of mass reference trajectory;

[0014] A first control input is acquired according to the reference state parameter set, the cost function, the target constraint condition and the state parameter set of the current control cycle.

[0015] Optionally, the target constraint condition includes:

[0016] Initial state constraints, foot end force constraints, support force constraints and dynamic constraints;

[0017] The initial state constraints are the initial position and initial posture of the robot;

[0018] The foot-end force constraint is that the foot-end force of the robot is within the friction cone;

[0019] The support force constraint is that the speed of the supporting foot of the robot is 0;

[0020] The dynamic constraint is that the difference between the states of two adjacent points on the trajectory is equal to the integral of the dynamic equation between two moments.

[0021] Optionally, the cost function is a relationship function between the center of mass velocity of the robot and the control input.

[0022] Optionally, the state parameter set includes the position of the robot's center of mass, attitude angle, speed, angular velocity and foot-end position, and the control input includes the robot's foot-end force and foot-end velocity.

[0023] A second aspect of the present application provides a quadruped robot center of mass trajectory planning device, comprising:

[0024] A first acquisition unit, used to acquire a center of mass reference trajectory of the robot;

[0025] A second acquisition unit, used for acquiring a state parameter set of the robot in a current control cycle according to a sensor;

[0026] A third acquisition unit, configured to acquire a first control input according to the mass center reference trajectory, the cost function, the target constraint condition and the state parameter set of the current control cycle;

[0027] A fourth acquisition unit, configured to acquire a target state parameter set of at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input;

[0028] A fifth acquisition unit, configured to acquire a target control input corresponding to at least one subsequent control cycle according to the center of mass reference trajectory, the cost function, the target constraint condition, and the target state parameter set of at least one subsequent control cycle;

[0029] A control unit is used to control the robot to move along an optimized trajectory according to the target control input corresponding to the subsequent at least one control cycle.

[0030] Optionally, the third acquiring unit is specifically used for:

[0031] Determining a reference state parameter set in the center of mass reference trajectory;

[0032] A first control input is acquired according to the reference state parameter set, the cost function, the target constraint condition and the state parameter set of the current control cycle.

[0033] Optionally, the target constraint condition includes:

[0034] Initial state constraints, foot end force constraints, support force constraints and dynamic constraints;

[0035] The initial state constraints are the initial position and initial posture of the robot;

[0036] The foot-end force constraint is that the foot-end force of the robot is within the friction cone;

[0037] The support force constraint is that the speed of the supporting foot of the robot is 0;

[0038] The dynamic constraint is that the difference between the states of two adjacent points on the trajectory is equal to the integral of the dynamic equation between two moments.

[0039] Optionally, the cost function is a relationship function between the center of mass velocity of the robot and the control input.

[0040] Optionally, the state parameter set includes the position of the robot's center of mass, attitude angle, speed, angular velocity and foot-end position, and the control input includes the robot's foot-end force and foot-end velocity.

[0041] A third aspect of the present application provides a quadruped robot center of mass trajectory planning device, the device comprising:

[0042] Processor, memory, input-output unit, and bus;

[0043] The processor is connected to the memory, the input and output unit, and the bus;

[0044] The memory stores a program, and the processor calls the program to execute the first aspect and any optional quadruped robot center of mass trajectory planning method in the first aspect.

[0045] A fourth aspect of the present application provides a computer-readable storage medium having a program stored thereon, and when the program is executed on a computer, the program executes the first aspect and any optional quadruped robot center of mass trajectory planning method in the first aspect.

[0046] It can be seen from the above technical solutions that this application has the following advantages:

[0047] By obtaining the robot's center of mass reference trajectory and current center of mass state, and then iterating the robot's center of mass state and control input in the next few control cycles according to the current center of mass state, cost function and related constraints, the robot movement is controlled with the calculated control input.

[0048] Through the method provided by the present application, the trajectory within the next few control cycles can be planned according to the current center of mass state of the robot, that is, the trajectory will be updated once every few control cycles according to the real-time center of mass state of the robot, so that the center of mass reference trajectory of the robot is optimized, and the optimized center of mass trajectory is close to the reference trajectory, and the speed of the robot is close to the preset speed. In this way, the dynamic adjustment of the robot's posture during the movement is realized, the foot end is prevented from slipping, and the stability of the robot during the movement is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the technical solution in the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0050] Figure 1 A schematic diagram of the hardware structure of the quadruped robot provided for this application;

[0051] Figure 2 A schematic diagram of the mechanical structure of the quadruped robot provided for this application;

[0052] Figure 3 A schematic diagram of a flow chart of an embodiment of a method for planning the center of mass trajectory of a quadruped robot provided in this application;

[0053] Figure 4 A schematic diagram of the calculation process of the target control input quantity in the quadruped robot center of mass trajectory planning method provided in this application;

[0054] Figure 5A schematic diagram of a center of mass reference trajectory and an optimized center of mass trajectory in the quadruped robot center of mass trajectory planning method provided in this application;

[0055] Figure 6 Another schematic diagram of the center of mass reference trajectory and the optimized center of mass trajectory in the quadruped robot center of mass trajectory planning method provided in this application;

[0056] Figure 7 A schematic diagram of the structure of an embodiment of a quadruped robot center of mass trajectory planning device provided in this application;

[0057] Figure 8 This is a schematic structural diagram of another embodiment of the quadruped robot center of mass trajectory planning device provided in this application. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0059] It should be noted that the quadruped robot center of mass trajectory planning method provided in the present application is applied to the field of robot control technology. The hardware structure and mechanical structure of the quadruped robot provided in the present application will be described below. In the subsequent description, the use of suffixes such as "module", "component" or "unit" used to represent components is only to facilitate the description of the present invention, and has no specific meaning in itself. Therefore, "module", "component" or "unit" can be used in a mixed manner.

[0060] See also Figure 1 , Figure 1 FIG. 1 is a schematic diagram of the hardware structure of a quadruped robot 100 according to one embodiment of the present invention. Figure 1 In the illustrated embodiment, the quadruped robot 100 includes a mechanical unit 101, a communication unit 102, a sensor unit 103, an interface unit 104, a storage unit 105, a control module 110, and a power supply 111. The various components of the quadruped robot 100 can be connected in any manner, including wired or wireless connections. Those skilled in the art will appreciate that Figure 1 The specific structure of the quadruped robot 100 shown in the figure does not constitute a limitation of the quadruped robot 100. The quadruped robot 100 may include more or fewer components than shown in the figure. Some components are not essential components of the quadruped robot 100 and may be omitted or combined as needed without changing the essence of the invention.

[0061] Combine the following Figure 1 The various components of the quadruped robot 100 are introduced in detail:

[0062] The mechanical unit 101 is the hardware of the quadruped robot 100. Figure 1 As shown, the mechanical unit 101 may include a driving plate 1011, a motor 1012, and a mechanical structure 1013. Figure 2 As shown, the mechanical structure 1013 may include a body 1014, extendable legs 1015, and a foot 1016. In other embodiments, the mechanical structure 1013 may also include an extendable mechanical arm, a rotatable head structure, a swingable tail structure, a load-bearing structure, a saddle structure, a camera structure, etc. It should be noted that each component module of the mechanical unit 101 may be one or more, and may be arranged according to specific circumstances. For example, there may be four legs 1015, and each leg 1015 may be configured with three motors 1012, and the corresponding motors 1012 are 12.

[0063] The communication unit 102 can be used to receive and send signals, and can also communicate with the network and other devices, for example, after receiving the instruction information sent by the remote control or other quadruped robot 100 to move in a specific direction at a specific speed according to a specific gait, the communication unit 102 is transmitted to the control module 110 for processing. The communication unit 102 includes a WiFi module, a 4G module, a 5G module, a Bluetooth module, an infrared module, etc.

[0064] The sensing unit 103 is used to obtain information data about the surrounding environment of the quadruped robot 100 and to monitor parameter data of each component inside the quadruped robot 100, and send it to the control module 110. The sensing unit 103 includes a variety of sensors, such as sensors for obtaining information about the surrounding environment: laser radar (for long-range object detection, distance determination and / or speed value determination), millimeter wave radar (for short-range object detection, distance determination and / or speed value determination), camera, infrared camera, global navigation satellite system (GNSS, Global Navigation Satellite System), etc. For example, sensors for monitoring each component inside the quadruped robot 100: inertial measurement unit (IMU, Inertial Measurement Unit) (for measuring the value of velocity value, acceleration value and angular velocity value), plantar sensor (for monitoring the position of the plantar force point, plantar posture, ground contact force size and direction), temperature sensor (for detecting component temperature). As for other sensors such as load sensors, touch sensors, motor angle sensors, torque sensors, etc. that can also be configured for the quadruped robot 100, they will not be repeated here.

[0065] The interface unit 104 may be used to receive input (e.g., data information, power, etc.) from an external device and transmit the received input to one or more components within the quadruped robot 100, or may be used to output (e.g., data information, power, etc.) to an external device. The interface unit 104 may include a power port, a data port (such as a USB port), a memory card port, a port for connecting a device having an identification module, an audio input / output (I / O) port, a video I / O port, etc.

[0066] The storage unit 105 is used to store software programs and various data. The storage unit 105 may mainly include a program storage area and a data storage area, wherein the program storage area may store operating system programs, motion control programs, application programs (such as text editors), etc.; the data storage area may store data generated by the quadruped robot 100 during use (such as various sensor data obtained by the sensor unit 103, log file data), etc. In addition, the storage unit 105 may include a high-speed random access memory, and may also include a non-volatile memory, such as a disk storage, a flash memory, or other volatile solid-state memory.

[0067] The display unit 106 is used to display information input by the user or information provided to the user. The display unit 106 may include a display panel 1061, which may be configured in the form of a liquid crystal display (LCD), an organic light-emitting diode (OLED), or the like.

[0068] The input unit 107 can be used to receive input digital or character information. Specifically, the input unit 107 may include a touch panel 1071 and other input devices 1072. The touch panel 1071, also known as a touch screen, can collect the user's touch operation (such as the user's operation on the touch panel 1071 or near the touch panel 1071 using the palm, finger or suitable accessory), and drive the corresponding connection device according to a pre-set program. The touch panel 1071 may include two parts: a touch detection device 1073 and a touch controller 1074. Among them, the touch detection device 1073 detects the user's touch orientation, detects the signal brought by the touch operation, and transmits the signal to the touch controller 1074; the touch controller 1074 receives the touch information from the touch detection device 1073, converts it into contact coordinates, and then sends it to the control module 110, and can receive and execute commands sent by the control module 110. In addition to the touch panel 1071, the input unit 107 may also include other input devices 1072. Specifically, other input devices 1072 may include but are not limited to one or more of a remote control operating handle, etc., which are not specifically limited here.

[0069] Furthermore, the touch panel 1071 may cover the display panel 1061. When the touch panel 1071 detects a touch operation on or near it, it transmits the information to the control module 110 to determine the type of the touch event. Then, the control module 110 provides corresponding visual output on the display panel 1061 according to the type of the touch event. Figure 1 In the embodiment, the touch panel 1071 and the display panel 1061 are used as two independent components to respectively realize the input and output functions. However, in some embodiments, the touch panel 1071 and the display panel 1061 can be integrated to realize the input and output functions, which is not limited here.

[0070] The control module 110 is the control center of the quadruped robot 100. It uses various interfaces and lines to connect the various components of the entire quadruped robot 100, and controls the quadruped robot 100 as a whole by running or executing software programs stored in the storage unit 105 and calling data stored in the storage unit 105.

[0071] The power supply 111 is used to supply power to each component. The power supply 111 may include a battery and a power control board. The power control board is used to control functions such as battery charging, discharging, and power consumption management. Figure 1 In the illustrated embodiment, the power supply 111 is electrically connected to the control module 110. In other embodiments, the power supply 111 may also be electrically connected to the sensor unit 103 (such as a camera, a radar, a speaker, etc.) and the motor 1012. It should be noted that each component may be connected to a different power supply 111 or powered by the same power supply 111.

[0072] On the basis of the above embodiments, specifically, in some embodiments, the terminal device can be used to communicate with the quadruped robot 100. When the terminal device communicates with the quadruped robot 100, the terminal device can send instruction information to the quadruped robot 100. The quadruped robot 100 can receive the instruction information through the communication unit 102, and can transmit the instruction information to the control module 110 after receiving the instruction information, so that the control module 110 can process the target speed value according to the instruction information. The terminal device includes but is not limited to: a mobile phone with an image capture function, a tablet computer, a server, a personal computer, a wearable smart device, and other electrical devices.

[0073] The instruction information can be determined according to preset conditions. In one embodiment, the quadruped robot 100 may include a sensor unit 103, and the sensor unit 103 may generate instruction information according to the current environment where the quadruped robot 100 is located. The control module 110 may determine whether the current speed value of the quadruped robot 100 meets the corresponding preset conditions according to the instruction information. If satisfied, the current speed value and current gait of the quadruped robot 100 will be maintained; if not satisfied, the target speed value and the corresponding target gait will be determined according to the corresponding preset conditions, so that the quadruped robot 100 can be controlled to move with the target speed value and the corresponding target gait. The environmental sensor may include a temperature sensor, an air pressure sensor, a visual sensor, and a sound sensor. The instruction information may include temperature information, air pressure information, image information, and sound information. The communication method between the environmental sensor and the control module 110 may be wired communication or wireless communication. The wireless communication method includes but is not limited to: wireless network, mobile communication network (3G, 4G, 5G, etc.), Bluetooth, infrared.

[0074] The above is a description of the hardware structure and mechanical structure of the quadruped robot provided in the present application. The following is a description of the center of mass trajectory planning method of the quadruped robot provided in the present application.

[0075] In the prior art, when a speed instruction is given to a quadruped robot, the robot will walk in a straight line according to the specified speed and the planned trajectory. However, when encountering stairs or other scenes with uneven roads, since the footholds in the trajectory are set according to flat roads, these footholds are not applicable in non-flat road environments. If the robot still walks in a straight line according to the previous trajectory, it may lose balance or even fall, resulting in poor stability of the quadruped robot when performing stair climbing tasks or walking on rough terrain.

[0076] Based on the above problems, the present application provides a method, device and storage medium for planning the center of mass trajectory of a quadruped robot, which are used to improve the stability of the robot during movement. For the convenience of description, the present embodiment takes the control unit as the execution subject for example, and the quadruped robot is referred to as the robot hereinafter.

[0077] See also Figure 3 , Figure 3 An embodiment of a quadruped robot center of mass trajectory planning method provided in the present application includes:

[0078] 301. Obtain the center of mass reference trajectory of the robot;

[0079] When the control unit of the robot receives a speed command, the control unit will integrate the speed command to obtain a straight line trajectory, which is the center of mass reference trajectory in this application. If the speed command is a fixed speed value, that is, the robot moves at a constant speed, the control unit can calculate the center of mass reference trajectory according to the speed formula. If the speed is constantly changing, for example, after the robot obtains a target point, it will move at different speeds at different times, then an integral operation is required to calculate the center of mass reference trajectory. The integral is to cope with the calculation of different speed values ​​in the trajectory.

[0080] It should be noted that the goal of the quadruped robot center of mass trajectory planning method provided in this application is to optimize the center of mass reference trajectory according to the real-time state of the robot to improve the stability of the robot moving in different terrains, and make the optimized trajectory close to the center of mass reference trajectory, and the speed of the robot is close to the preset speed.

[0081] Without any intervention, the control unit will control the movement of the robot according to the center of mass reference trajectory. It should be noted that the center of mass reference trajectory is composed of multiple discrete points, and each point on the trajectory contains position and time data.

[0082] The center of mass reference trajectory also includes the state parameter set and control input quantity of each control cycle during the robot's movement. The control cycle refers to the robot's motion control cycle, which is a very short period of time, generally between 0.001 seconds and 0.003 seconds, preferably 0.002 seconds. During the movement of the robot, each step of the movement is realized according to the preset control logic, and the movement of multiple control cycles forms the robot's motion trajectory.

[0083] The state parameter set refers to the set of parameters such as the position, attitude angle, speed, angular velocity and foot position of the robot's center of mass. Among them, the attitude angle refers to the angle of rotation of the body posture, and the center of mass will generate angular velocity as the body posture rotates. Specifically, the control unit can detect parameters such as the moving distance (position) of the center of mass, the angle, speed and angular velocity of the body posture rotation through an inertial measurement unit (IMU).

[0084] The state parameter set Xn of the Nth control cycle is expressed as:

[0085] Xn=[p(n)θ(n)v(n)ω(n)pfoot1(n)pfoot2(n)pfoot3(n)pfoot4(n)];

[0086] Among them, p(n) is the position of the robot's center of mass, θ(n) is the robot's attitude angle, v(n) is the robot's center of mass velocity, ω(n) is the angular velocity of the robot's center of mass, and pfoot1(n), pfoot2(n), pfoot3(n), and pfoot4(n) are the foot positions of the robot's four legs respectively.

[0087] The control input refers to the foot-end force and foot-end speed of the robot. After knowing that the robot (center of mass) needs to move at a certain speed, the control unit will calculate how much foot-end force and foot-end speed it needs to output based on the speed so that the center of mass reaches that speed.

[0088] The control input quantity Un of the Nth control cycle is expressed as:

[0089] Un=[F1(n)F2(n)F3(n)F4(n)qd1(n)qd2(n)qd3(n)qd4(n)];

[0090] Among them, F1(n), F2(n), F3(n), and F4(n) are the foot-end forces of the robot’s four legs, and qd1(n), qd2(n), qd3(n), and qd4(n) are the foot-end velocities of the robot’s four legs.

[0091] 302. Obtain a state parameter set of the robot's current control cycle according to the sensor;

[0092] Since the center of mass reference trajectory is a straight line trajectory obtained by only considering the velocity integral, without considering whether the robot can reach each point on the trajectory, whether there is a suitable landing point, etc., the problem is not obvious when the robot walks on flat ground. Once it performs stair climbing tasks or walks on rugged terrain, the shortcomings of the original trajectory planning method will be exposed. At this time, it is necessary to dynamically adjust the center of mass reference trajectory according to the specific situation to improve the stability of the robot during the movement.

[0093] The control unit obtains the real-time state parameter set of the robot according to the velocity sensor, acceleration sensor and other sensors in the IMU or other related sensors, that is, the state parameter set of the current control cycle in this application. The state parameter set of the current control cycle refers to the set of parameters such as the position, attitude angle, speed, angular velocity and foot end position of the center of mass of the robot in the current control cycle.

[0094] 303. Obtain a first control input according to a center of mass reference trajectory, a cost function, a target constraint condition, and a state parameter set of a current control cycle;

[0095] Specifically, the step of obtaining the first control input according to the mass center reference trajectory, the cost function, the target constraint condition and the state parameter set of the current control cycle includes:

[0096] Determining a reference state parameter set in a center of mass reference trajectory;

[0097] A first control input is obtained according to a reference state parameter set, a cost function, a target constraint condition and a state parameter set of the current control cycle.

[0098] The center of mass reference trajectory is a set of states and times, that is, each control cycle on the center of mass reference trajectory has a center of mass state parameter set. The control unit uses the state parameter set of the current control cycle in the center of mass reference trajectory as the desired state, and calculates the corresponding control input according to the deviation between the desired state and the current state. This is a typical process of feedback control. In the calculation process, it is also necessary to combine the cost function and the target constraints to calculate. The cost function ensures that the center of mass velocity generated by the control input is as small as possible and meets all target constraints. The input of the cost function is the calculated control input, and the output is the center of mass velocity. This cost function ensures that the output center of mass velocity is as small as possible. If there is no restriction on the cost function, the calculated control input will not necessarily be the optimal solution.

[0099] Specifically, the target constraints and cost functions in this application are described below:

[0100] 1. Initial state constraints:

[0101] The starting position and posture of the quadruped robot can be obtained by the underlying motion controller or by the center of mass reference trajectory. During the calculation process, the trajectory must satisfy X1=X(0), Xn=X(T), where X1 is the state at the current moment, X(0) is the state at the starting point of the trajectory, Xn is the state at the Nth moment, and X(T) is the state at the end of the trajectory.

[0102] 2. Foot end force constraint:

[0103] C i，n F i，n ≤d i，n , where i=1...4 refers to the four feet of the robot, C i，n refers to the friction cone of the ith foot at the nth moment, F i，n is the ground support reaction force of the ith foot at the nth moment, d i，n The maximum static friction force of the ground on the ith foot at the nth moment. This constraint is used to constrain the robot's foot forces within the friction cone to prevent the supporting foot from slipping.

[0104] 3. Support force constraints:

[0105] During the walking process of a quadruped robot, for example, in the walk gait, the supporting foot is the foot on the ground, and the swinging foot is the foot in the air. Since the supporting foot is fixed to the ground and does not move, with the foot tip on the ground, the speed of the supporting foot is zero; while the swinging foot is the foot in the air, with the foot tip of the swinging foot suspended in the air and no reaction force, so the force at the foot tip of the swinging foot is zero.

[0106] 4. Dynamic constraints:

[0107] For two adjacent points on the trajectory, the state difference is equal to the integral of the dynamic equation between the two moments, that is, it satisfies X n+1 =X n +Xd n Δt, where n = 1...N-1, X n+1 is the state at time N+1, X n is the state at time N, Xd n Δt is the dynamic integral, and the state includes position, velocity, angular velocity, attitude angle, etc.

[0108] 5. Cost function:

[0109]

[0110] Where Y refers to the control input, and J refers to the center of mass speed. The goal of optimal control is to find a suitable control input Y so that the output center of mass speed J is as small as possible. When calculating the control input Y, the purpose of the cost function is to achieve the optimal control performance under all the above constraints, that is, to ensure that the center of mass speed J generated by the calculated control input Y is as small as possible under the premise of satisfying all the above constraints. The smaller the center of mass speed J, the better, in order to save the robot energy and reduce the robot's motion amplitude as much as possible while ensuring the minimum following error, which is conducive to increasing the endurance and life while completing the trajectory following task.

[0111] 304. Acquire a target state parameter set of at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input;

[0112] x[n+1]=Ax[n]+Bu[n];

[0113] Among them, x is the state parameter set, u is the control input, and parameters A and B are determined by the selected dynamic model, that is, the state parameter set at time n+1 can be calculated by the dynamic model based on the state parameter set and control input at time n.

[0114] It should be noted that there are many ways to model the above-mentioned dynamic model, such as a dynamic model established by applying Lagrangian dynamics theory, a dynamic model established by applying Euler theory, etc. Different dynamic models produce different control effects, which are not specifically limited here.

[0115] 305. Obtain a target control input corresponding to at least one subsequent control cycle according to the center of mass reference trajectory, the cost function, the target constraint condition, and the target state parameter set of at least one subsequent control cycle;

[0116] Step 304 and step 305 are the processes of iteratively calculating the target state parameter set and the target control input in at least one subsequent control cycle based on the state parameter set of the current control cycle of the robot, that is, the state parameter set at time t can be used to calculate the state parameter set at time t+1 according to dynamics, and the control input at time t+1 can be used to calculate the control input at time t+1 based on the state parameter set at time t+1 and the expected state at time t+1.

[0117] For example, see Figure 4 , the control unit calculates the control input u1 from the state parameter set x1 (the state parameter set of the current control cycle) at time t (the current time) according to the center of mass reference trajectory, the cost function and the target constraint conditions;

[0118] The state parameter set x1 at time t and the control input u1 are used to calculate the state parameter set x2 at time t+1 according to the dynamic model;

[0119] The state parameter set x2 at time t+1 and the center of mass reference trajectory at time t+1 can calculate the control input u2 at time t+1...

[0120] Thus, the target state parameter set and the corresponding target control input within multiple control cycles can be iterated, and the cost function and the target constraint conditions must also be considered during the calculation process. It should be noted that the control unit can only calculate the target state parameter set within a subsequent control cycle to adjust the center of mass reference trajectory in real time, or it can continuously calculate the target state parameter set within several control cycles (preferably 5-10 control cycles) to adjust the trajectory every multiple control cycles, which is not limited here.

[0121] 306. Control the robot to move along the optimized trajectory according to the target control input corresponding to at least one subsequent control cycle.

[0122] The control unit controls the movement of the robot according to the calculated target control input. Since the real-time state of the robot, the center of mass reference trajectory, the cost function and the target constraint are combined when calculating the target control input, the target control input can be updated in time according to the actual situation compared to the control input in the original center of mass reference trajectory, thereby preventing the robot from losing balance during movement.

[0123] In this embodiment, by obtaining the robot's center of mass reference trajectory and current center of mass state, the robot's center of mass state and control input in the next few control cycles are iterated according to the current center of mass state, cost function and related constraints, and the robot movement is controlled by the calculated control input.

[0124] Through the method provided by the present application, the trajectory within the next few control cycles can be planned according to the center of mass state of the robot at the current moment, that is, the trajectory will be updated once every few control cycles according to the real-time center of mass state of the robot, so that the center of mass reference trajectory of the robot is optimized, and the optimized center of mass trajectory is close to the reference trajectory, and the speed of the robot is close to the preset speed. In this way, the dynamic adjustment of the posture of the robot during the movement is achieved, the foot end is prevented from slipping, and the stability of the robot during the movement is improved. Especially when the robot is traveling on non-planar complex terrain (special terrain such as stairs, steep slopes or rugged terrain), the robot can optimize the center of mass reference trajectory in time according to the real-time center of mass state, and control the robot to move with the optimized trajectory, which can effectively prevent the robot from losing balance or falling when traveling on non-planar complex terrain.

[0125] In addition, in this application, the trajectory is represented as a piecewise polynomial to reduce the computational dimension and ensure the smoothness of the motion. This is because the trajectory is composed of the position angle and time of the particle. Any trajectory curve can be described by a set of points on the curve. The denser the points are, the more it can describe the real curve, but too many points will burden the calculation. Therefore, the trajectory curve can be described as a polynomial. As long as there are coefficients of the polynomial, the curve can be described. For example: the straight line y=kx can be represented by a set of n points on this line, or it can be represented by only a first-order polynomial with a coefficient of k. The latter only needs to record a coefficient k to represent the curve, which is more convenient and makes the calculation faster.

[0126] In another possible implementation, the control unit may also analyze the environmental information collected by the visual sensor, and only dynamically adjust the center of mass reference trajectory when it is determined that there is non-planar complex terrain on the center of mass reference trajectory. For example, the control unit analyzes whether there is non-planar complex terrain on the center of mass reference trajectory through video image information captured by a camera. Specifically, the non-planar complex terrain refers to special terrain such as stairs, steep slopes, or rugged terrain. When the control unit determines that there is non-planar complex terrain on the center of mass reference trajectory, the above method is executed to dynamically adjust the center of mass reference trajectory.

[0127] See also Figure 5 and Figure 6 , Figure 5 and Figure 6 Schematic diagram of the center of mass reference trajectory and the optimized center of mass trajectory in the quadruped robot center of mass trajectory planning method provided in this application.

[0128] The above-mentioned embodiments have elaborated on the quadruped robot center of mass trajectory planning method provided in the present application. The quadruped robot center of mass trajectory planning device and computer-readable storage medium provided in the present application will be elaborated on in detail below in conjunction with the accompanying drawings.

[0129] See also Figure 7 , Figure 7 An embodiment of a quadruped robot center of mass trajectory planning device provided in the present application includes:

[0130] A first acquisition unit 701 is used to acquire a reference trajectory of the center of mass of the robot;

[0131] The second acquisition unit 702 is used to acquire a state parameter set of the current control cycle of the robot according to the sensor;

[0132] The third acquisition unit 703 is used to acquire the first control input according to the mass center reference trajectory, the cost function, the target constraint condition and the state parameter set of the current control cycle;

[0133] A fourth acquisition unit 704, configured to acquire a target state parameter set of at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input;

[0134] A fifth acquisition unit 705 is used to acquire a target control input corresponding to at least one subsequent control cycle according to the mass center reference trajectory, the cost function, the target constraint condition and the target state parameter set of at least one subsequent control cycle;

[0135] The control unit 706 is used to control the robot to move along the optimized trajectory according to the target control input corresponding to at least one subsequent control cycle.

[0136] Optionally, the third acquiring unit 703 is specifically configured to:

[0137] Determining a reference state parameter set in a center of mass reference trajectory;

[0138] A first control input is obtained according to a reference state parameter set, a cost function, a target constraint condition, and a state parameter set of a current control cycle.

[0139] Optionally, target constraints include:

[0140] Initial state constraints, foot end force constraints, support force constraints and dynamic constraints;

[0141] The initial state constraints are the robot’s initial position and initial posture;

[0142] The foot-end force constraint is that the robot’s foot-end force is within the friction cone;

[0143] The support force constraint is that the speed of the robot’s supporting foot is 0;

[0144] The dynamic constraint is that the difference between the states of two adjacent points on the trajectory is equal to the integral of the dynamic equation between the two times.

[0145] Optionally, the cost function is a relationship function between the robot's center of mass velocity and the control input.

[0146] Optionally, the state parameter set includes the position of the robot's center of mass, attitude angle, speed, angular velocity and foot position, and the control input includes the robot's foot force and foot speed.

[0147] This application also provides a quadruped robot center of mass trajectory planning device, see Figure 8 , Figure 8 An embodiment of a quadruped robot center of mass trajectory planning device provided in this application includes:

[0148] Processor 801, memory 802, input and output unit 803, bus 804;

[0149] The processor 801 is connected to the memory 802, the input and output unit 803 and the bus 804;

[0150] The memory 802 stores a program, and the processor 801 calls the program to execute any of the above quadruped robot center of mass trajectory planning methods.

[0151] The present application also relates to a computer-readable storage medium on which a program is stored, wherein when the program is run on a computer, the computer executes any of the above quadruped robot center of mass trajectory planning methods.

[0152] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0153] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0154] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0155] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0156] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, read-only memory), random access memory (RAM, random access memory), disk or optical disk and other media that can store program code.

Claims

1. A method for planning the center of mass trajectory of a quadruped robot, characterized in that: The method comprises: Get the robot's center of mass reference trajectory; Acquire a state parameter set of the robot in the current control cycle according to the sensor, wherein the state parameter set includes the position of the robot's center of mass, attitude angle, speed, angular velocity and foot end position; Determining a reference state parameter set in the center of mass reference trajectory; Acquire a first control input according to the reference state parameter set, the cost function, the target constraint condition and the state parameter set of the current control cycle, wherein the first control input comprises a foot end force and a foot end velocity of the robot, the cost function is a relationship function between the center of mass velocity of the robot and the control input, and the cost function is used to ensure that the center of mass velocity generated by the target control input is as small as possible and meets all the target constraint conditions, wherein the target constraint conditions include a starting state constraint, a foot end force constraint, a support force constraint and a dynamic constraint; Acquire a target state parameter set for at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input; Obtaining a target control input corresponding to at least one subsequent control cycle according to the mass center reference trajectory, the cost function, the target constraint condition, and the target state parameter set of at least one subsequent control cycle; The robot is controlled to move along the optimized trajectory according to the target control input corresponding to the subsequent at least one control cycle.

2. The method according to claim 1, characterized in that The initial state constraints are the initial position and initial posture of the robot; The foot-end force constraint is that the foot-end force of the robot is within the friction cone; The support force constraint is that the speed of the supporting foot of the robot is 0; The dynamic constraint is that the difference between the states of two adjacent points on the trajectory is equal to the integral of the dynamic equation between two moments.

3. A quadruped robot center of mass trajectory planning device, characterized in that: The device comprises: A first acquisition unit, used to acquire a center of mass reference trajectory of the robot; A second acquisition unit, used to acquire a state parameter set of the robot in the current control cycle according to the sensor, wherein the state parameter set includes the position of the robot's center of mass, attitude angle, speed, angular velocity and foot end position; A third acquisition unit, configured to acquire a first control input according to the mass center reference trajectory, the cost function, the target constraint condition and the state parameter set of the current control cycle; A fourth acquisition unit, configured to acquire a target state parameter set of at least one subsequent control cycle according to the state parameter set of the current control cycle and the first control input; A fifth acquisition unit, configured to acquire a target control input corresponding to at least one subsequent control cycle according to the center of mass reference trajectory, the cost function, the target constraint condition, and the target state parameter set of at least one subsequent control cycle; A control unit, configured to control the robot to move along an optimized trajectory according to a target control input corresponding to the at least one subsequent control cycle; The third acquisition unit is specifically used for: Determining a reference state parameter set in the center of mass reference trajectory; A first control input is obtained according to the reference state parameter set, the cost function, the target constraint conditions and the state parameter set of the current control cycle. The first control input includes the foot end force and foot end velocity of the robot. The cost function is a relationship function between the center of mass velocity of the robot and the control input. The cost function is used to ensure that the center of mass velocity generated by the target control input is as small as possible and meets all the target constraint conditions. The target constraint conditions include the starting state constraint, the foot end force constraint, the support force constraint and the dynamic constraint.

4. The device according to claim 3, characterized in that The initial state constraints are the initial position and initial posture of the robot; The foot-end force constraint is that the foot-end force of the robot is within the friction cone; The support force constraint is that the speed of the supporting foot of the robot is 0; The dynamic constraint is that the difference between the states of two adjacent points on the trajectory is equal to the integral of the dynamic equation between two moments.

5. A quadruped robot center of mass trajectory planning device, characterized in that: The device comprises: Processor, memory, input-output unit, and bus; The processor is connected to the memory, the input and output unit, and the bus; The memory stores a program, and the processor calls the program to execute the method according to any one of claims 1 to 2.

6. A computer-readable storage medium having a program stored thereon, wherein the program, when executed on a computer, performs the method according to any one of claims 1 to 2.

Images