Whole-body control method for foot-type robot and foot-type robot

Abstract

The invention provides a full-body control method for a foot-type robot and the foot-type robot, and the method comprises the steps: obtaining a planned path and a current state of the foot-type robot; based on the planned path and the current state, constructing constraint conditions by using the first task, and constructing a quadratic programming problem by using the second task and the weight coefficient thereof; the quadratic programming problem is solved, and a control instruction of the foot type robot is obtained; and performing whole-body control on the foot-type robot based on the control instruction. According to the scheme, the task priority can be dynamically adjusted through weight distribution, and the flexibility of the whole-body control method for the foot-type robot can be improved.

Description

Technical Field

[0001] This application relates to the technical field of robot control. Specifically, it relates to a full-body control method for a legged robot and a legged robot. Background Art

[0002] Robot whole-body control technology (Whole-Body Control, WBC) is a comprehensive control strategy for robots, aiming to simultaneously control multiple degrees of freedom of the robot to achieve complex motions and tasks.

[0003] Currently, most robot whole-body control technologies are implemented based on the null space. Its core idea is to decompose control tasks into different priorities. High-priority tasks are executed in the main space, while low-priority tasks are executed in the null space. This can ensure that the robot will not be interfered by secondary tasks when completing the main tasks. However, the whole-body control technology based on the null space needs to strictly follow the priority order of each task, and the priorities of each task cannot be flexibly adjusted, resulting in poor adaptability of the whole-body control scheme to different scenarios. Summary of the Invention

[0004] The purpose of the embodiments of this application is to provide a full-body control method for a legged robot and a legged robot, so as to improve the adaptability of the whole-body control scheme to different scenarios.

[0005] In a first aspect, the embodiments of this application provide a full-body control method for a legged robot. The method includes: obtaining the planned path and the current state of the legged robot; based on the planned path and the current state, using the first task to construct a constraint condition, and using the second task and its weight coefficient to construct a quadratic programming problem; wherein, the priority of the first task is higher than the priority of the second task; the weight coefficient of the second task is positively correlated with the priority of the second task; solving the quadratic programming problem to obtain the control instruction of the legged robot; performing full-body control on the legged robot based on the control instruction.

[0006] In the implementation process of the above solution, on the one hand, the above solution can achieve dynamic adjustment of task priorities through weight allocation, which is beneficial to improving the flexibility of the above full-body control method for legged robots; on the other hand, compared with the whole-body control scheme based on the null space in the related art, the above solution can add or subtract tasks by simple linear algebra addition, which is beneficial to improving the scalability and adaptability of the above full-body control method for legged robots.

[0007] In one implementation manner of the first aspect, the first task includes: a limb extension limit task for constraining the limb extension length of the legged robot;

[0008] Constructing the constraint conditions by using the limb stretching limit task includes: in the body coordinate system of the legged robot, determining the first coordinate of the hip joint relative to the center of mass; performing coordinate transformation on the first coordinate to obtain the second coordinate of the hip joint in the world coordinate system; obtaining the hip joint height component of the second coordinate; based on the foot end height of the legged robot and the hip joint height component, obtaining the hip-foot distance between the hip joint and the foot end; based on the hip-foot distance, constructing a foot end position constraint; and using second-order Taylor expansion, the Jacobian matrix, and the derivative of the Jacobian matrix to convert the foot end position constraint into a joint acceleration constraint.

[0009] In the implementation process of the above solution, on the one hand, by restricting the joint angle from the acceleration level, the above solution can effectively prevent the joint angle from exceeding the limit, which is beneficial to improving the control effect of the above legged robot whole-body control method; on the other hand, by directly restricting the joint angle from the acceleration level, the above solution is beneficial to improving the real-time performance of the above legged robot whole-body control method, enabling the above legged robot whole-body control method to more directly control the dynamic behavior of the joints.

[0010] In an implementation manner of the first aspect, the method further includes: obtaining the Jacobian matrix of the hip joint height component based on the hip joint height component; and solving the derivative of the Jacobian matrix with respect to time to obtain the derivative of the Jacobian matrix.

[0011] In the implementation process of the above solution, on the one hand, by directly solving the Jacobian matrix through the hip joint height component, the limb stretching limit task can be accurately mapped from the acceleration level. The Jacobian matrix maps the velocity of the robot's center of mass coordinates to the three-dimensional linear velocity of the hip coordinates, so that the movement of the robot in complex terrains can be controlled more precisely, ensuring that the limb stretching is within a safe range; on the other hand, by solving the derivative of the Jacobian matrix with respect to time, the control strategy can be dynamically adjusted to the current motion state. The derivative of the Jacobian matrix reflects the change of the robot's kinematic parameters over time, enabling the control strategy to respond more quickly to the change of the motion state and improving the dynamic performance and adaptability of the control.

[0012] In an implementation manner of the first aspect, the constructing the foot end position constraint based on the hip-foot distance includes: correcting the maximum and minimum values of the hip-foot distance in the foot end position constraint based on the hip-foot distance and the projection value of the hip-foot distance in the z-axis direction; and constructing the foot end position constraint based on the hip-foot distance, the maximum value of the hip-foot distance, and the minimum value of the hip-foot distance.

[0013] In the implementation process of the above solution, the maximum and minimum hip-foot distances are corrected by the projection value of the hip-foot distance in the z-axis direction, so as to limit the hip-foot distance within a reasonable threshold range, which is beneficial to improving the control effect of the above-mentioned whole-body control method for legged robots.

[0014] In one implementation manner of the first aspect, the first task further includes: a floating base kinematic equation task, a motor torque limit task, a friction cone constraint task, and a foot-end contact point no-motion task, where: the floating base kinematic equation task is used to constrain the base of the legged robot to satisfy its kinematic equation; the motor torque limit task is used to constrain the output torque of the joint motors of the legged robot; the friction cone constraint task is used to constrain the foot-end force of the legged robot within the friction cone range; the foot-end contact point no-motion task is used to constrain the foot-end to be in a stationary state when the foot-end of the legged robot is in the contact phase.

[0015] In the implementation process of the above solution, by setting a limb extension limit task, a floating base kinematic equation task, a motor torque limit task, a friction cone constraint task, and a foot-end contact point no-motion task in the first task, the legged robot can strictly meet the constraint conditions that need to be strictly observed during the motion process, which is beneficial to improving the control effect of the legged robot.

[0016] In one implementation manner of the first aspect, the second task includes: a centroid x and y-axis direction acceleration tracking task, a centroid z-axis direction acceleration tracking task, a centroid yaw angular acceleration tracking task, and a centroid roll and pitch angular acceleration tracking task, where: the centroid x and y-axis direction acceleration tracking task is used to track the acceleration of the centroid of the legged robot in the horizontal direction; the centroid z-axis direction acceleration tracking task is used to track the acceleration of the centroid of the legged robot in the vertical direction; the centroid yaw angular acceleration tracking task is used to track the yaw acceleration of the centroid of the legged robot; the centroid roll and pitch angular acceleration tracking task is used to track the roll and pitch accelerations of the centroid of the legged robot.

[0017] In the implementation process of the above solution, by setting a centroid x and y-axis direction acceleration tracking task, a centroid z-axis direction acceleration tracking task, a centroid yaw angular acceleration tracking task, and a centroid roll and pitch angular acceleration tracking task in the second task, the above-mentioned whole-body control method for legged robots can control the legged robot to move stably from the centroid x and y-axis directions, z-axis direction, yaw angle, roll angle, and pitch angle, which is beneficial to improving the control effect of the above-mentioned whole-body control method for legged robots.

[0018] In an implementation of the first aspect, the construction of the quadratic programming problem by using the second task and its weight coefficients includes: constructing the centroid dynamics equation of the legged robot based on momentum; obtaining the centroid acceleration based on the centroid dynamics equation of the legged robot; where the centroid acceleration includes: centroid acceleration in the x direction, centroid acceleration in the y direction, centroid acceleration in the z-axis direction, centroid yaw angular acceleration, centroid roll angular acceleration, and centroid pitch angular acceleration; obtaining the tracking errors of the centroid acceleration tracking tasks in the x and y axis directions, the centroid acceleration tracking task in the z-axis direction, the centroid yaw angular acceleration tracking task, and the centroid roll and pitch angular acceleration tracking tasks based on the centroid acceleration; and constructing a quadratic programming problem based on the tracking errors and the weight coefficients of the second task.

[0019] In the implementation process of the above solution, on the one hand, the centroid acceleration of the legged robot is quickly obtained through the centroid dynamics equation of the legged robot, so as to realize the efficient control of the centroid acceleration of the legged robot, which is beneficial to improving the control real-time performance of the above-mentioned whole-body control method for the legged robot; on the other hand, the centroid acceleration of the legged robot is quickly obtained through the centroid dynamics equation of the legged robot, and then the data requirements of the centroid acceleration tracking task are met, which is beneficial to improving the control efficiency of the above-mentioned whole-body control method for the legged robot.

[0020] In an implementation of the first aspect, the second task further includes: a swing leg foot-end trajectory tracking task and a foot-end force tracking task, where: the swing leg foot-end trajectory tracking task is used to track the trajectory of the swing leg of the legged robot; the foot-end force tracking task is used to track the foot-end force of the legged robot.

[0021] In the implementation process of the above solution, by setting the swing leg foot-end trajectory tracking task and the foot-end force tracking task in the second task, the above-mentioned whole-body control method for the legged robot can control the smoothness of the legged robot during walking and the contact force between the legged robot and the ground, which is beneficial to improving the control effect of the above-mentioned whole-body control method for the legged robot.

[0022] In an implementation of the first aspect, the method further includes: in response to a motion mode switching instruction of the legged robot, updating the weight coefficients of the second task to the weight coefficients corresponding to the current motion mode.

[0023] In the implementation process of the above solution, the weight coefficients of the second task can be updated based on the motion mode switching instruction of the legged robot, so that the legged robot can be applied to more application scenarios, which is beneficial to improving the adaptability of the above-mentioned whole-body control method for the legged robot.

[0024] In an implementation of the first aspect, solving the quadratic programming problem to obtain the control instruction of the legged robot includes: solving the quadratic programming problem to obtain the acceleration control instruction, the foot-end force control instruction, and the joint torque control instruction of the legged robot.

[0025] In the implementation process of the above solution, on the one hand, compared with the solution of directly performing whole-body control on the legged robot from the speed and position levels in the related art, the control instruction of the legged robot in the above solution includes an acceleration control instruction, enabling the whole-body control method of the legged robot to perform whole-body control on the legged robot from the acceleration level, which is beneficial to improving the real-time performance and control effect of the whole-body control method of the legged robot; on the other hand, the foot-end force and joint torque of the legged robot can also be controlled, enabling the whole-body control method of the legged robot to take into account the coordinated control tasks of the body, feet, and legs, which is beneficial to further improving the control effect of the whole-body control method of the legged robot.

[0026] In a second aspect, an embodiment of the present application provides a legged robot, which includes a processor and a memory. At least one computer program is stored in the memory, and the at least one computer program is loaded and executed by the processor to implement the method provided by the first aspect or any possible implementation manner of the first aspect.

[0027] In a third aspect, an embodiment of the present application provides a computer-readable storage medium, on which computer program instructions are stored. When the computer program instructions are read and run by a processor, the method provided by the first aspect or any possible implementation manner of the first aspect is executed.

[0028] In a fourth aspect, an embodiment of the present application provides a computer program product, which includes a computer program. When the computer program is executed by a processor, the method provided by the first aspect or any possible implementation manner of the first aspect is implemented.

[0029] Other features and advantages of the present application will be described in the subsequent description, and some of them will become obvious from the description, or can be understood by implementing the embodiments of the present application. The objectives and other advantages of the present application can be achieved and obtained through the structures specifically pointed out in the written description, claims, and drawings. Description of the Drawings

[0030] To more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings required for use in the embodiments of the present application. It should be understood that the following drawings only show certain embodiments of the present application and should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other relevant drawings can also be obtained based on these drawings.

[0031] Figure 1 Schematic flowchart of the full-body control method for the legged robot provided by the embodiment of the present application;

[0032] Figure 2 Schematic architecture diagram of the full-body control model provided by the embodiment of the present application;

[0033] Figure 3 Schematic structure diagram of the quadruped robot in a certain scenario provided by the embodiment of the present application;

[0034] Figure 4 Schematic diagram of the hip-foot distance of a certain limb of the quadruped robot in a certain scenario provided by the embodiment of the present application. Detailed implementation manners

[0035] The following will describe the technical solutions in the embodiments of the present application in combination with the drawings in the embodiments of the present application. The following embodiments are only used to more clearly illustrate the technical solutions of the present application and are only examples, and thus cannot be used to limit the protection scope of the present application.

[0036] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present application belongs; the terms used herein are only for the purpose of describing specific embodiments and are not intended to limit the present application; the terms "including" and "having" and any variations thereof in the specification and claims of the present application and the above drawings are intended to cover non-exclusive inclusion.

[0037] In the description of the embodiments of the present application, technical terms such as "first" and "second" are only used to distinguish different objects and cannot be understood as indicating or implying relative importance or implicitly indicating the quantity, specific order or primary-secondary relationship of the indicated technical features. In the description of the embodiments of the present application, "a plurality" means two or more unless otherwise specifically defined.

[0038] References herein to "embodiments" mean that a particular feature, structure, or characteristic described in connection with the embodiments can be included in at least one embodiment of the present application. The phrase appears in various places in the specification and does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art will explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0039] In the whole-body control WBC theory, the null space is used to represent the degrees of freedom or redundant space for certain tasks. Specifically: when a task is defined as high-priority, its corresponding control variables (such as joint torques or velocities) are strictly constrained to ensure the execution of the task. Other tasks can only be adjusted within the null space of the high-priority task, which means that the execution of low-priority tasks must not interfere with high-priority tasks. This null-space-based control method is essentially a hierarchical control strategy, where the constraints for high-priority tasks are rigid, and low-priority tasks can only be optimized within the remaining degrees of freedom of high-priority tasks. Therefore, the order of task priorities is fixed, and once set, it is difficult to dynamically adjust according to the scenario.

[0040] Based on this, the embodiments of the present application provide a whole-body control method for a legged robot. This method uses a task layering combined with weight allocation to achieve the whole-body control of the legged robot. The tasks are divided into two layers based on the priority of the tasks, namely the first task and the second task, where the priority of the first task is higher than that of the second task. The first task is used to construct constraint conditions, and the second task differentiates different task priorities according to the weight coefficients. On the one hand, the above solution can achieve dynamic adjustment of task priorities through weight allocation, which is beneficial to improving the flexibility of the whole-body control method for the legged robot; on the other hand, compared with the null-space-based whole-body control solution in the related art, the above solution can add or subtract tasks by simple linear algebra addition, which is beneficial to improving the scalability and adaptability of the whole-body control method for the legged robot.

[0041] Please refer to Figure 1 The flow schematic diagram of the whole-body control method for the legged robot provided by the embodiments of the present application shown. The whole-body control method for the legged robot provided by the embodiments of the present application can be applied to an electronic device. The electronic device can include physical devices such as a server, a PC, a tablet computer, or a smart phone, or can also be a virtual device such as a virtual machine or a container. The electronic device can be a single device, or a combination of multiple devices or a cluster of a large number of devices. The above whole-body control method for the legged robot can include:

[0042] Step S110: Obtain the planned path and the current state of the legged robot;

[0043] Step S120: Based on the planned path and the current state, construct constraint conditions using the first task, and construct a quadratic programming problem using the second task and its weight coefficients; wherein, the priority of the first task is higher than that of the second task; the weight coefficient of the second task is positively correlated with the priority of the second task;

[0044] Step S130: Solve the quadratic programming problem to obtain the control instructions for the legged robot;

[0045] Step S140: Perform whole-body control on the legged robot based on the control instructions.

[0046] The above-mentioned legged robot refers to: a robot that moves by simulating the walking postures of animals or humans. A legged robot is usually configured with one or more legs, and each leg is configured with one or more joints. The robot moves by controlling the lifting or lowering of the legs.

[0047] The planned path in the above-mentioned step S110 refers to the centroid of the legged robot and the limb planned path. The planned path can be obtained based on relatively mature path acquisition methods in the field such as the method of trajectory planners, the method of graph search, the method of machine learning, etc. For specific acquisition methods, please refer to related technologies, and the embodiments of the present application will not elaborate.

[0048] The current state in the above-mentioned step S110 refers to the current state of the legged robot, which may include information such as the position and motion state of the centroid and limbs of the legged robot. The current state can be obtained based on relatively mature state acquisition methods in the field such as the Kalman filtering method based on sensor fusion, the method based on particle filtering, the method based on state estimators, etc. The embodiments of the present application will not elaborate.

[0049] It can be understood that the above-mentioned step S120 and step S130 can be implemented by a whole-body control model (or a whole-body controller). Please refer to Figure 2 , in the embodiments of the present application, the input of the whole-body control model can be the planned path and the current state of the legged robot, and the output can be the control instructions for the legged robot. The whole-body control model defines two layers of tasks, where:

[0050] The first layer of tasks, that is, the first task, is a high-priority task, which can also be understood as a task that needs to be strictly satisfied during the whole-body control process. The priority levels of the first tasks are the same, so the weight coefficients can be not used to distinguish the priorities of the tasks;

[0051] The second layer of tasks, that is, the second task, is a low-priority task, which can also be understood as a task that does not need to be strictly satisfied during the whole-body control process. There are high and low priorities among the tasks.

[0052] It can be understood that the above first task is mainly used for the legged robot to achieve the basic kinematic relationship, while the second task is mainly used to improve the motion performance and adaptability of the legged robot on the basis of achieving the basic kinematic relationship.

[0053] The above second task may include a state tracking task of the legged robot, such as a centroid state tracking task, a joint state tracking task, etc.

[0054] The whole-body control model can configure the priority levels of each task by configuring the weight coefficients of each task in the second task. The weight coefficient of the second task is positively correlated with the priority of the second task. The higher the priority of the second task, the greater its weight coefficient.

[0055] The quadratic programming problem (QP) in the above step S120 is a mathematical optimization problem. Its objective function is a quadratic function of the optimization vector x, and the constraint conditions are linear equalities or inequalities. Generally speaking, the quadratic term of the quadratic function can be designed as a positive number, so the quadratic function opens upward and has a minimum value. Therefore, minimizing the above objective function can obtain the optimal solution of the optimization vector x. The above weight coefficients can be applied to the objective function of the quadratic programming problem. Taking a certain scenario as an example, the second task in this scenario includes an acceleration A tracking task and an acceleration B tracking task, and the cost function includes the error term of the acceleration A tracking task and the error term of the acceleration B tracking task. The error term can be used to represent the difference between the actual acceleration value and the target acceleration value. In the objective function, the error term of the acceleration A tracking task is multiplied by its weight coefficient to form a weighted error term, and the error term of the acceleration B tracking task is multiplied by its weight coefficient to form a weighted error term. The two weighted error terms constitute the objective function of the above quadratic programming problem. By solving this quadratic programming problem, the control instruction of the legged robot can be obtained.

[0056] The whole-body control methods in the related technologies generally directly output commands for the parameters at the position level and the speed level. For the task of avoiding joint over-limit, the current main method is to limit the joint angle and angular velocity. However, the joint angle and angular velocity commands still need to be further converted into joint acceleration commands and torque commands, which is difficult to ensure real-time performance, and it is impossible to prevent joint over-limit in time when the legged robot encounters situations such as stepping on empty, slipping, or even falling. Based on this, the embodiments of the present application provide the following solutions:

[0057] Optionally, the above first task may include:

[0058] A limb extension limit task for constraining the limb extension length of the legged robot.

[0059] The following introduces the solution for constructing constraint conditions by using the above limb extension limit task:

[0060] Construct constraint conditions by using limb extension limit tasks, including: in the body coordinate system of the legged robot, determine the first coordinate of the hip joint relative to the center of mass; perform coordinate transformation on the first coordinate to obtain the second coordinate of the hip joint in the world coordinate system; obtain the hip joint height component of the second coordinate; based on the foot tip height and the hip joint height component of the legged robot, obtain the hip-foot distance between the hip joint and the foot tip; construct a foot tip position constraint based on the hip-foot distance; use second-order Taylor expansion, the Jacobian matrix, and the derivative of the Jacobian matrix to convert the foot tip position constraint into a joint acceleration constraint. For example, this implementation method:

[0061] Please refer to Figure 3 , assuming that the torso length of the quadruped robot is l x , and the torso width is l y , in the body coordinate system of the robot, the first coordinate of the k-th (k = 1, 2, 3, 4) hip joint relative to the center of mass is:

[0062]

[0063] In the world coordinate system, the center of mass coordinates of the quadruped robot are set as:

[0064] O = [o x1 o y1 o z1 T

[0065] The torso postures are roll angle roll 1 , pitch angle pitch 1 , and yaw angle yaw 1 ;

[0066] In the world coordinate system, the second coordinate of the k-th hip joint is set as:

[0067]

[0068] According to the coordinate transformation principle, it can be obtained that:

[0069]

[0070] Among them, R k is the coordinate of the k-th hip joint in the world coordinate system; O is the center of mass coordinate of the robot in the world coordinate system; R(yaw 1 ) represents the rotation matrix for rotating yaw 1 angle around the yaw axis; R(pitch 1 ) represents the rotation matrix for rotating pitch 1 angle around the pitch axis; R(roll 1 ​) Represents the rotation matrix for rotating roll by roll around the roll axis 1 ; or k Is the vector coordinate of the k-th hip joint coordinate relative to the center of mass in the body coordinate system of the robot; c yaw1 Represents the yaw angle yaw 1 Cosine value of; s yaw1 Represents the yaw angle yaw 1 Sine value of; c pitch1 Represents the pitch angle pitch 1 Cosine value of; s pitch1 Represents the pitch angle pitch 1 Sine value of; c roll1 Represents the roll angle roll 1 Cosine value of; s roll1 Represents the roll angle roll 1 Sine value of;

[0071] The Hz component of the height of the k-th hip joint is the third row component of the above second coordinate R k That is:

[0072]

[0073] It can be seen that the height component of the hip joint is only related to o z1 (The height component of the center of mass coordinate of the robot in the world coordinate system), the roll angle roll 1 , the pitch angle pitch 1 These three variables are related;

[0074] Perform a second-order Taylor expansion on the z height component of the position of the k-th hip joint in the world coordinate system:

[0075]

[0076] Among them, Represents the current moment; t represents the future moment; Represents the time interval; Represents the moment The z height component of the position of the k-th hip joint in the world coordinate system at time; Represents the moment The first derivative of the z height component of the position of the k-th hip joint in the world coordinate system with respect to time at time, that is, the velocity; Represents the moment The second derivative of the z height component of the position of the k-th hip joint in the world coordinate system with respect to time at time, that is, the acceleration;

[0077] The second-order Taylor expansion uses the Taylor series expansion to approximate the change of the hip joint height over time. Specifically, it considers the current time The height, speed, and acceleration, and use the time step δt to predict the hip joint height at time t;

[0078] The Jacobian matrix is a matrix of partial derivatives of a multivariable function. In robot whole-body control technology, the Jacobian matrix is usually used to describe the relationship between the speed of the robot's end effector (such as the foot tip of a legged robot) and the joint speeds of the robot;

[0079] Introduce the Jacobian matrix mapping expression at the level of the hip joint coordinate velocity of the robot:

[0080]

[0081] where is the three-dimensional linear velocity vector of the k-th hip joint coordinate; u is the six-dimensional linear / angular velocity vector of the robot's center-of-mass coordinate;

[0082] Extract the 1×nu-dimensional Jacobian vector from the Jacobian matrix J Then transform the second-order Taylor expansion of the z-height component of the k-th hip joint into:

[0083]

[0084] Define as the z-direction height of the k-th foot tip, and constrain the z-direction hip-foot distance between h min and h max :

[0085]

[0086] where h min and h max represent the minimum and maximum values of the z-direction hip-foot distance, respectively.

[0087] Convert the above constraints into matrix form for calculation:

[0088]

[0089] Considering that the optimization vector x contains Therefore, the above constraints can be converted into:

[0090]

[0091] So far, the joint acceleration constraints corresponding to the limb extension limit task are obtained.

[0092] On the one hand, by restricting the joint angle from the acceleration aspect, the above solution can effectively prevent the joint angle from exceeding the limit, which is beneficial to improving the control effect of the above full-body control method for the legged robot; on the other hand, by directly restricting the joint angle from the acceleration aspect, the above solution is beneficial to improving the real-time performance of the above full-body control method for the legged robot, enabling the above full-body control method for the legged robot to more directly control the dynamic behavior of the joints.

[0093] Optionally, the above full-body control method for the legged robot further includes: obtaining the Jacobian matrix of the hip joint height component based on the hip joint height component; solving the derivative of the Jacobian matrix with respect to time to obtain the derivative of the Jacobian matrix. This implementation is as follows:

[0094] Taking the derivative of the hip joint height component can obtain the Jacobian matrix of the hip joint height component

[0095]

[0096] It can be obtained that:

[0097]

[0098] Then solve the derivative of the Jacobian matrix with respect to time For the Jacobian matrix Taking the derivative of each element respectively and following the chain rule of differentiation, it can be obtained that:

[0099]

[0100] On the one hand, by directly solving the Jacobian matrix through the hip joint height component, the limb extension limit task can be accurately mapped from the acceleration aspect. The Jacobian matrix maps the velocity of the robot's center of mass coordinates to the three-dimensional linear velocity of the hip coordinates, so that the movement of the robot in complex terrains can be controlled more precisely, ensuring that the limb extension is within a safe range; on the other hand, by solving the derivative of the Jacobian matrix with respect to time, the control strategy can be dynamically adjusted to the current motion state. The derivative of the Jacobian matrix reflects the change of the robot's kinematic parameters over time, enabling the control strategy to respond more quickly to the change of the motion state and improving the dynamic performance and adaptability of the control.

[0101] Please refer to Figure 4 , since the foot end coordinates of the robot are not always directly below the hip joint coordinates along the z-axis direction. For example Figure 4 the foot end F shown in min is not directly below the hip joint R. At this time, if a fixed h max, the hip-foot distance will be limited within an unreasonable threshold range, thus affecting the control effect. Based on this, the embodiments of the present application provide the following solutions:

[0102] Optionally, the above-mentioned construction of the foot-end position constraint based on the hip-foot distance includes: correcting the maximum hip-foot distance and the minimum hip-foot distance in the foot-end position constraint based on the hip-foot distance and the projection value of the hip-foot distance in the z-axis direction; constructing the foot-end position constraint based on the hip-foot distance, the maximum hip-foot distance, and the minimum hip-foot distance. For example, this implementation method:

[0103] Use the following formula to correct the maximum hip-foot distance h max and the minimum hip-foot distance h min :

[0104]

[0105] where, l min and l max are respectively the physical minimum distance and the physical maximum distance of the hip-foot distance, which are generally determined by the lengths of the thigh and calf of the limb and are fixed values; l is the actual value of the hip-foot distance to the ground; h is the projection value of the hip-foot distance in the z-axis direction;

[0106] It can be understood that if the foot-end of the robot falls directly below the hip joint, that is, when the point F (foot-end) coincides with the point P (the point directly below the hip joint), h is equal to l; if the foot-end of the robot does not fall directly below the hip joint, that is, when the point F does not coincide with the point P, h is less than l. That is:

[0107]

[0108] The above solution corrects the maximum hip-foot distance and the minimum hip-foot distance through the projection value of the hip-foot distance in the z-axis direction, thereby restricting the hip-foot distance within a reasonable threshold range, which is beneficial to improving the control effect of the above-mentioned whole-body control method of the legged robot.

[0109] Next, other tasks that the above first task may include are introduced:

[0110] Optionally, the above first task may further include: a floating base kinematic equation task, a motor torque limit task, a friction cone constraint task, and a foot-end contact point no-motion task, where:

[0111] The floating base kinematic equation task is used to constrain the base of the legged robot to satisfy its kinematic equation to maintain the overall kinematic relationship of the legged robot;

[0112] The motor torque limit task is used to constrain the output torque of the joint motors of the legged robot to ensure that the motors operate within a safe range;

[0113] The friction cone constraint task is used to constrain the foot-end force of the legged robot within the friction cone range to prevent the robot from slipping.

[0114] The foot-end contact point no-motion task is used to constrain the foot-end to be stationary when the foot-end of the legged robot is in the contact phase to avoid unnecessary motion of the corresponding foot-end.

[0115] By setting the limb extension limit task, floating base kinematic equation task, motor torque limit task, friction cone constraint task, and foot-end contact point no-motion task in the first task, the above solution enables the legged robot to strictly meet the constraint conditions that need to be strictly observed during the motion process, which is beneficial to improving the control effect of the legged robot.

[0116] The following introduces the relevant implementation methods of the second task:

[0117] Optionally, the above second task includes: the centroid x and y-axis direction acceleration tracking task, the centroid z-axis direction acceleration tracking task, the centroid yaw angle acceleration tracking task, and the centroid roll and pitch angle acceleration tracking task, where:

[0118] The centroid x and y-axis direction acceleration tracking task is used to track the acceleration of the centroid of the legged robot in the horizontal direction to control the stability of the legged robot in the horizontal direction.

[0119] The centroid z-axis direction acceleration tracking task is used to track the acceleration of the centroid of the legged robot in the vertical direction to control the stability of the legged robot in the vertical direction.

[0120] The centroid yaw angle acceleration tracking task is used to track the yaw acceleration of the centroid of the legged robot to control the steering stability of the legged robot.

[0121] The centroid roll and pitch angle acceleration tracking task is used to track the roll and pitch accelerations of the centroid of the legged robot to control the attitude of the legged robot.

[0122] By setting the centroid x and y-axis direction acceleration tracking task, the centroid z-axis direction acceleration tracking task, the centroid yaw angle acceleration tracking task, and the centroid roll and pitch angle acceleration tracking task in the second task, the above legged robot whole-body control method can effectively control the translational and rotational stabilities of the legged robot from the aspects of the centroid x and y-axis directions, z-axis direction, yaw angle, roll angle, and pitch angle, which is beneficial to improving the control effect of the above legged robot whole-body control method.

[0123] For the above-mentioned centroid acceleration tracking tasks in the x and y axis directions, centroid acceleration tracking task in the z axis direction, centroid yaw angular acceleration tracking task, and centroid roll and pitch angular acceleration tracking tasks, most of the related technologies control the translation and rotation of the legged robot through centroid pose and centroid velocity commands. This method is difficult to ensure real-time execution. Based on this, the embodiments of the present application provide the following technical solutions:

[0124] Optionally, a quadratic programming problem is constructed using the second task and its weight coefficient, including: constructing the centroid dynamics equation of the legged robot based on momentum; obtaining the centroid acceleration based on the centroid dynamics equation of the legged robot; where the centroid acceleration includes: centroid acceleration in the x direction, centroid acceleration in the y direction, centroid acceleration in the z axis direction, centroid yaw angular acceleration, centroid roll angular acceleration, and centroid pitch angular acceleration; obtaining the tracking errors of the centroid acceleration tracking tasks in the x and y axis directions, centroid acceleration tracking task in the z axis direction, centroid yaw angular acceleration tracking task, and centroid roll and pitch angular acceleration tracking tasks based on the centroid acceleration; constructing a quadratic programming problem based on the tracking errors and the weight coefficient of the second task.

[0125] The implementation manner of obtaining the centroid acceleration based on the centroid dynamics equation of the legged robot in the above solution is, for example:

[0126] The centroid dynamics equation of the legged robot based on momentum can be described as:

[0127]

[0128] where h com is the centroid momentum vector; is the differential of the generalized coordinate vector q; A(q) is defined as the momentum matrix, representing the mapping from the differential of the generalized coordinate to the centroid momentum;

[0129] To describe the centroid dynamics of the robot in detail, the centroid dynamics equation can be expanded as:

[0130]

[0131] where hp com is the centroid linear momentum (three-dimensional vector), hθ com is the centroid angular momentum (three-dimensional vector), following the ZYX Euler angles; is the centroid coordinate velocity, A b (q) is the momentum matrix of the centroid coordinate velocity; is the driving joint coordinate angular velocity, A j (q) is the momentum matrix of the driving joint coordinate angular velocity;

[0132] To solve the centroid acceleration at the acceleration level Therefore, differentiating both sides of the above equation gives:

[0133]

[0134] Extracting the centroid acceleration command from the above equation gives:

[0135]

[0136] Since the six variables of the centroid coordinate velocity are independent of each other, the momentum matrix A b (6×6 dimension) is invertible.

[0137] At this point, the centroid acceleration can be obtained.

[0138] The above scheme is based on the construction method of the constraint equation for the relevant tasks of the centroid acceleration. For example:

[0139] According to the scenario requirements, each task related to the centroid acceleration command is weighted. The constraint equation for the centroid x and y acceleration tracking tasks can be expressed as:

[0140]

[0141] where ω xy_accel is the weight coefficient for the centroid x and y axis direction acceleration tracking tasks, corresponding to the first element of the centroid acceleration command; corresponding to the second element of the centroid acceleration command;

[0142] Similarly, the constraint equation for the centroid yaw angular acceleration tracking task can be expressed as:

[0143]

[0144] where ω yaw_accel is the weight coefficient for the centroid yaw angular acceleration tracking task; corresponding to the fourth element of the centroid acceleration command;

[0145] The constraint equation for the centroid z axis direction acceleration tracking task can be expressed as:

[0146]

[0147] where ω z_accel is the weight coefficient for the centroid z axis direction acceleration tracking task; corresponding to the third element of the centroid acceleration command;

[0148] The constraint equations for the centroid roll angle and pitch angle acceleration tracking tasks can be expressed as:

[0149]

[0150] where ω roll_pitch_accel is the weight coefficient for the centroid roll angle and pitch angle acceleration tracking tasks; corresponds to the fifth element of the centroid acceleration command; corresponds to the sixth element of the centroid acceleration command;

[0151] Finally, input the above constraint equations as the cost function into the quadratic programming solver to obtain the optimal solution of the control command.

[0152] On the one hand, the above scheme quickly obtains the centroid acceleration of the legged robot through the centroid dynamics equation of the legged robot, thereby realizing the efficient control of the centroid acceleration of the legged robot, which is beneficial to improving the control real-time performance of the above-mentioned whole-body control method of the legged robot; on the other hand, the above scheme quickly obtains the centroid acceleration of the legged robot through the centroid dynamics equation of the legged robot, and then meets the data requirements of the centroid acceleration tracking task, which is beneficial to improving the control efficiency of the above-mentioned whole-body control method of the legged robot.

[0153] Optionally, the above second task further includes: a swing leg foot end trajectory tracking task and a foot end force tracking task, where:

[0154] The swing leg foot end trajectory tracking task is used to track the trajectory of the swing leg of the legged robot to control the smoothness of the legged robot during walking;

[0155] The foot end force tracking task is used to track the foot end force of the legged robot to control the contact force between the legged robot and the ground to meet the expectations.

[0156] By setting the swing leg foot end trajectory tracking task and the foot end force tracking task in the second task, the above-mentioned whole-body control method of the legged robot can control the smoothness of the legged robot during walking and the contact force between the legged robot and the ground, which is beneficial to improving the control effect of the above-mentioned whole-body control method of the legged robot.

[0157] Optionally, the above-mentioned whole-body control method of the legged robot further includes: in response to the motion mode switching instruction of the legged robot, updating the weight coefficient of the second task to the weight coefficient corresponding to the current motion mode. This implementation method is as follows:

[0158] The legged robot can respond to the motion mode switching instruction input by the user, or automatically generate a motion switching instruction based on different terrains through methods such as semantic map recognition. For example, when it is recognized that the legged robot moves onto an ice surface, an ice surface mode switching instruction can be automatically generated. Since the ice surface is relatively smooth and has a low friction coefficient, the legged robot is prone to slipping. Therefore, the weight coefficient of the centroid yaw angular acceleration tracking task can be appropriately increased to reduce sliding during turning, and the weight coefficient of the foot end force tracking task can be appropriately decreased, thereby reducing the applied foot end force. Another example: In the terrain environment of climbing stairs, since the legged robot requires a high foot end tracking accuracy, the weight coefficients of the swing leg foot end trajectory tracking task and the foot end force tracking task can be appropriately increased, and the weight coefficients of the centroid z-axis direction acceleration tracking task and the centroid roll and pitch angular acceleration tracking tasks can be appropriately decreased.

[0159] The above solution can update the weight coefficient of the second task based on the motion mode switching instruction of the legged robot, enabling the legged robot to be applicable to more application scenarios, which is beneficial to improving the adaptability of the above legged robot whole-body control method.

[0160] Optionally, the above step S130 includes: solving a quadratic programming problem to obtain the acceleration control instruction, foot end force control instruction, and joint torque control instruction of the legged robot.

[0161] It can be understood that for the legged robot, the optimization vector of the quadratic programming problem can include the acceleration control instruction of the generalized coordinates, the foot end force control instruction, and the joint torque control instruction, and the optimization vector can be expressed as:

[0162]

[0163] where f is the foot end force control instruction; τ is the joint torque control instruction; is the acceleration control instruction of the generalized coordinates, including the accelerations corresponding to the position (x, y, z), attitude (roll, pitch, yaw) of the centroid of the legged robot, and the joint angles.

[0164] On the one hand, compared with the solution in the related technology that directly performs whole-body control on the legged robot from the velocity and position levels, the control instruction of the legged robot in the above solution includes the acceleration control instruction, enabling the above legged robot whole-body control method to perform whole-body control on the legged robot from the acceleration level, which is beneficial to improving the real-time performance and control effect of the above legged robot whole-body control method; on the other hand, the foot end force and joint torque of the legged robot can also be controlled, enabling the above legged robot whole-body control method to take into account the coordinated control tasks of the body, feet, and legs, which is beneficial to further improving the control effect of the above legged robot whole-body control method.

[0165] For ease of understanding, the following uses a certain scenario as an example to introduce the setting methods of the first task and the second task:

[0166] In a certain scenario, the first task and the second task adopt the setting method shown in Table 1:

[0167]

[0168]

[0169] The general expression formula for the above equation type tasks can be:

[0170] Ax - b = W

[0171] The general expression formula for the inequality type tasks can be:

[0172] Dx - e ≤ v

[0173] Where x is the optimization vector.

[0174] The above floating base kinematic equation task is based on the contact dynamics equation of the legged robot, and puts the acceleration control instruction, the foot end force control instruction and the joint torque control instruction into the same optimization vector x.

[0175] The above motor torque limit task is an inequality constraint task, and it is necessary to limit the joint torque control instruction in the optimization vector x within the range of [reverse torque threshold, forward torque threshold]. The reverse torque threshold is negative, representing the maximum value of the reverse torque, and the forward torque threshold is positive, representing the maximum value of the forward torque.

[0176] The above friction cone constraint task is an inequality constraint task, and it is necessary to limit the foot end force control instruction in the optimization vector x within the friction cone range according to the preset foot end friction coefficient.

[0177] The above foot end contact point no - motion task is used to keep the joint acceleration in the optimization vector x as 0 when a certain foot end is in the contact phase.

[0178] The above swing leg foot end trajectory tracking task is used to control the tracking error of the joint acceleration control instruction in the optimization vector x within a reasonable range based on the weight coefficient of the swing leg foot end trajectory tracking task when a certain foot end is in the swing phase.

[0179] The above foot end force tracking task is used to control the tracking error of the foot end force control instruction in the optimization vector x within a reasonable range based on the weight coefficient of the foot end force tracking task.

[0180] It can be understood that in the second task, according to the different weight coefficients of the six parameters of position (x, y, z) and attitude (roll, pitch, yaw), the tracking accuracy of the task with a high weight coefficient can be preferentially achieved according to the requirements of different scenarios, and the task with a low weight coefficient can have better adaptability.

[0181] Based on the same inventive concept, an embodiment of the present application further provides a legged robot, which includes a processor and a memory. At least one computer program is stored in the memory, and the at least one computer program is loaded and executed by the processor to implement the above-mentioned whole-body control method of the legged robot.

[0182] The above-mentioned memory includes one or more (only one is shown in the figure), which can be, but is not limited to, a random access memory (Random Access Memory, abbreviated as RAM), a read-only memory (Read Only Memory, abbreviated as ROM), a programmable read-only memory (Programmable Read-Only Memory, abbreviated as PROM), an erasable programmable read-only memory (Erasable Programmable Read-Only Memory, abbreviated as EPROM), an electrically erasable programmable read-only memory (Electric Erasable Programmable Read-Only Memory, abbreviated as EEPROM), etc. The processor and other possible components can access the memory, read and / or write the data therein.

[0183] The processor includes one or more (only one is shown in the figure), which can be an integrated circuit chip with signal processing capabilities. The above-mentioned processor can be a general-purpose processor, including a central processing unit (Central Processing Unit, abbreviated as CPU), a microcontroller unit (Micro Controller Unit, abbreviated as MCU), a network processor (Network Processor, abbreviated as NP), or other conventional processors; it can also be a dedicated processor, including a digital signal processor (Digital Signal Processor, abbreviated as DSP), an application-specific integrated circuit (Application Specific Integrated Circuits, abbreviated as ASIC), a field programmable gate array (Field Programmable Gate Array, abbreviated as FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0184] One or more computer programs can be stored in a memory, and a processor can read and run these computer programs to implement the full-body control method of the legged robot provided by the embodiments of the present application and other desired functions.

[0185] The embodiments of the present application further provide a computer-readable storage medium, on which computer program instructions are stored. When the computer program instructions are read and run by a processor of a computer, the full-body control method of the legged robot provided by the embodiments of the present application is executed.

[0186] The embodiments of the present application further provide a computer program product, which includes a computer program. When the computer program is executed by a processor, the full-body control method of the legged robot provided by the embodiments of the present application is implemented.

[0187] In the embodiments provided by the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. The device embodiments described above are only illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For another example, 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 displayed or discussed mutual coupling or direct coupling or communication connection can be through some communication interfaces. The indirect coupling or communication connection of the devices or units can be in an electrical, mechanical or other form. In addition, the units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment. Furthermore, in each embodiment of the present application, the functional modules can be integrated together to form an independent part, or each module can exist alone, or two or more modules can be integrated to form an independent part.

[0188] It should be noted that if a function is implemented in the form of a software functional module 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, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present application. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM), random access memories (RAM), magnetic disks, or optical discs that can store program codes.

[0189] In this text, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations.

[0190] The above are only embodiments of the present application and are not used to limit the protection scope of the present application. For those skilled in the art, the present application can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for controlling the whole body of a legged robot, characterized in that: The method comprises: Get the planned path and current state of the legged robot; Based on the planned path and the current state, a constraint condition is constructed using the first task, and a quadratic programming problem is constructed using the second task and its weight coefficient; wherein the priority of the first task is higher than the priority of the second task; and the weight coefficient of the second task is positively correlated with the priority of the second task; Solving the quadratic programming problem to obtain control instructions for the legged robot; The legged robot is fully controlled based on the control instructions.

2. The whole body control method of a legged robot according to claim 1, characterized in that: The first task includes: A limb extension restriction task, used to restrict the limb extension length of the legged robot; The constraint condition is constructed using the limb extension restriction task, including: In the body coordinate system of the legged robot, determining a first coordinate of the hip joint relative to the center of mass; Performing coordinate transformation on the first coordinate to obtain a second coordinate of the hip joint in a world coordinate system; Obtaining a hip joint height component of the second coordinate; Based on the foot end height of the foot robot and the hip joint height component, obtaining the hip-foot distance between the hip joint and the foot end; Based on the hip-to-foot distance, construct a foot end position constraint; The foot end position constraint is converted into a joint acceleration constraint using a second-order Taylor expansion, a Jacobian matrix and a derivative of the Jacobian matrix.

3. The whole body control method of a legged robot according to claim 2, characterized in that: The method further comprises: Based on the hip joint height component, obtaining a Jacobian matrix of the hip joint height component; Solve the derivative of the Jacobian matrix with respect to time to obtain the derivative of the Jacobian matrix.

4. The whole body control method of a legged robot according to claim 2, characterized in that: The step of constructing a foot end position constraint based on the hip-foot distance comprises: Based on the hip-foot distance and the projection value of the hip-foot distance in the z-axis direction, the maximum value of the hip-foot distance and the minimum value of the hip-foot distance in the foot end position constraint are corrected; A foot end position constraint is constructed based on the hip-foot distance, the hip-foot distance maximum value and the hip-foot distance minimum value.

5. The whole body control method of a legged robot according to claim 2, characterized in that: The first task also includes: a floating base kinematic equation task, a motor torque limit task, a friction cone constraint task and a foot end contact point no motion task, wherein: The floating base kinematics equation task is used to constrain the base of the legged robot to satisfy its kinematics equation; The motor torque limiting task is used to constrain the output torque of the joint motor of the foot-type robot; The friction cone constraint task is used to constrain the foot end force of the foot-type robot to be within the range of the friction cone; The foot end contact point has no motion task and is used to constrain the foot end to be in a static state when the foot end of the foot-type robot is in the contact phase.

6. The whole body control method of a legged robot according to claim 1, characterized in that: The second task includes: a task of tracking acceleration in the x- and y-axis directions of the center of mass, a task of tracking acceleration in the z-axis direction of the center of mass, a task of tracking acceleration in the yaw angle of the center of mass, and a task of tracking acceleration in the roll angle and pitch angle of the center of mass, wherein: The center of mass x- and y-axis acceleration tracking task is used to track the acceleration of the center of mass of the legged robot in the horizontal direction; The centroid z-axis acceleration tracking task is used to track the acceleration of the centroid of the footed robot in the vertical direction; The center of mass yaw acceleration tracking task is used to track the yaw acceleration of the center of mass of the legged robot; The center of mass roll angle and pitch angle acceleration tracking task is used to track the roll and pitch acceleration of the center of mass of the legged robot.

7. The whole body control method of a legged robot according to claim 6, characterized in that: The method of constructing a quadratic programming problem using the second task and its weight coefficient includes: Construct the momentum-based center-of-mass dynamics equations for the legged robot; Based on the center-of-mass dynamics equation of the legged robot, the center-of-mass acceleration is obtained; wherein the center-of-mass acceleration includes: center-of-mass x-direction acceleration, center-of-mass y-direction acceleration, center-of-mass z-axis direction acceleration, center-of-mass yaw angular acceleration, center-of-mass roll angular acceleration and center-of-mass pitch angular acceleration; Based on the center of mass acceleration, obtaining tracking errors of the center of mass x- and y-axis acceleration tracking tasks, the center of mass z-axis acceleration tracking task, the center of mass yaw angle acceleration tracking task, and the center of mass roll angle and pitch angle acceleration tracking task; A quadratic programming problem is constructed based on the tracking error and the weight coefficient of the second task.

8. The whole body control method of a legged robot according to claim 6, characterized in that: The second task also includes: a swing leg foot end trajectory tracking task and a foot end force tracking task, wherein: The swing leg foot end trajectory tracking task is used to track the swing leg trajectory of the foot-type robot; The foot-end force tracking task is used to track the foot-end force of the foot-type robot.

9. The whole body control method of a legged robot according to any one of claims 1 to 8, characterized in that: The whole-body control method of the legged robot also includes updating the weight coefficient of the second task to the weight coefficient corresponding to the current motion mode in response to a motion mode switching instruction of the legged robot; or, solving the quadratic programming problem to obtain the control instruction of the legged robot includes solving the quadratic programming problem to obtain the acceleration control instruction, foot-end force control instruction and joint torque control instruction of the legged robot.

10. A legged robot, characterized in that: The legged robot comprises a processor and a memory, wherein at least one computer program is stored in the memory, and the at least one computer program is loaded and executed by the processor to implement the method according to any one of claims 1 to 9.

Images