A method, apparatus, device, and storage medium for whole-body robot control

Abstract

This application discloses a method, device, equipment, and storage medium for whole-body robot control. The method includes constructing a dynamic model of the robot's floating base and an acceleration constraint model between task joints; introducing continuous state phase variables to generate task activation weights; classifying physical constraints by relaxation, introducing relaxation variables, assigning penalty function forms to various constraints, and determining corresponding penalty coefficients to obtain constraint penalty terms; constructing an optimization objective function by combining the task tracking error, task activation weights, and constraint penalty terms obtained from the acceleration constraint model, using the floating base dynamic model and acceleration constraint model as equality constraints, and the physical constraints after introducing relaxation variables as inequality constraints; and solving the optimization objective function to drive the robot's joint motors. This application can solve the torque jump caused by task switching and the unsolvable and singular problems under constraint conflicts in whole-body robot control in related technologies.

Description

Technical Field

[0001] This application relates to the field of robot motion control technology, and in particular to a method, device, equipment and storage medium for whole-body robot control. Background Technology

[0002] Whole-Body Control (WBC) combined with Model Predictive Control (MPC) is currently an efficient framework for handling the multi-degree-of-freedom, strongly coupled dynamics of legged robots. In existing technologies, whole-body control is usually modeled as a rigorous hierarchical optimization problem based on null-space projection, or a quadratic programming problem based on fixed weights.

[0003] However, existing technologies have the following three shortcomings: Strictly Prioritized Torque Abrupt Changes: When performing highly dynamic movements (such as foot landing, take-off, or gait transition), null-space projection-based methods can cause pulse-like jumps in the desired joint torque due to abrupt changes in the null-space dimension of the projection matrix. This discontinuity can easily lead to high-frequency joint oscillations, damaging the hardware.

[0004] Unsolvable and singular problems under constraint conflicts: When in a singular configuration or when environmental friction is limited, strict inequality constraints (such as friction cone constraints and joint limits) may conflict with high priority tasks, resulting in the QP (quadratic programming) solver having no solution or outputting extreme torques.

[0005] High computational complexity: The massive multi-task WBC solution takes a long time, making it difficult to meet the real-time requirements of low-level control at high frequencies (such as above 1kHz). Summary of the Invention

[0006] This application provides a method, apparatus, device, and storage medium for whole-body robot control, in order to solve the problems of torque jumps caused by task switching and unsolvable and singular problems under constraint conflicts in whole-body robot control in related technologies.

[0007] In a first aspect, embodiments of this application provide a method for whole-body robot control, including: Construct a dynamic model of the robot's floating base and an acceleration constraint model between the task joints; Introduce continuous state phase variables to generate task activation weights; Physical constraints are classified into relaxation categories, relaxation variables are introduced, penalty function forms are assigned to each type of constraint, and corresponding penalty coefficients are determined to obtain constraint penalty terms. An optimization objective function is constructed by combining the task tracking error, task activation weight, and constraint penalty term obtained from the acceleration constraint model. The floating base dynamics model and acceleration constraint model are used as equality constraints, and the physical constraints after introducing relaxation variables are used as inequality constraints. The optimization objective function is solved to drive the robot's joint motors.

[0008] In conjunction with the first aspect, in one implementation, the introduction of continuous state phase variables includes mapping state changes during robot task switching to continuous values ​​between 0 and 1.

[0009] In conjunction with the first aspect, in one implementation, the generation task activation weight includes: Based on the continuous state phase variable, the preset smoothing factor, the maximum activation weight of the task, and the critical phase point of task switching, the task activation weight is calculated using a smooth transition function.

[0010] In conjunction with the first aspect, in one implementation, the relaxation classification of physical constraints includes: dividing the physical constraints into absolutely non-relaxable constraints, finitely relaxable constraints, and fully relaxable constraints. The method of assigning penalty functions to various constraints includes: assigning an exponential barrier function to the absolutely non-relaxable constraint, assigning a logarithmic barrier function to the finitely relaxable constraint, and assigning a quadratic penalty function to the fully relaxable constraint; and / or, the method of introducing relaxation variables includes: introducing a zero relaxation variable to the absolutely non-relaxable constraint; and introducing non-negative relaxation variables to the finitely relaxable constraint and the fully relaxable constraint.

[0011] In conjunction with the first aspect, in one embodiment, the method further includes: setting a maximum permissible relaxation amount for the relaxation variable of the finitely relaxed constraint; wherein the maximum permissible relaxation amount is determined based on a physical safety threshold.

[0012] In conjunction with the first aspect, in one implementation, obtaining the constraint penalty term includes: Substituting the slack variables into the corresponding penalty function form, we obtain the penalty function value; The constraint penalty term is obtained based on the penalty function value and the corresponding penalty coefficient.

[0013] In conjunction with the first aspect, in one implementation, determining the corresponding penalty coefficient includes: Calculate the degree of constraint violation based on the actual number of violations and the maximum permissible number of violations; The basic penalty coefficient is dynamically adjusted according to the degree of constraint violation to obtain an adaptive penalty coefficient, which is then used as the penalty coefficient.

[0014] In conjunction with the first aspect, in one implementation, the task tracking error obtained from the acceleration constraint model is calculated through the following steps: Obtain the robot's expected acceleration for the task; Based on the acceleration constraint model, the actual acceleration of the task is obtained; The difference between the actual acceleration of the task and the expected acceleration of the task is calculated as the task tracking error.

[0015] In conjunction with the first aspect, in one implementation, the step of constructing an optimization objective function based on the task tracking error, task activation weights, and constraint penalty terms obtained from the acceleration constraint model includes: The task tracking error is calculated based on the weighted quadratic norm of the task activation weights; The weighted quadratic norm is summed with the constraint penalty term to obtain the optimization objective function.

[0016] In conjunction with the first aspect, in one implementation, solving the optimization objective function to drive the robot joint motors includes: The optimization objective function is reconstructed into a standard quadratic programming problem and solved to obtain the optimal decision variables; Drive the robot joint motors according to the optimal decision variables.

[0017] In conjunction with the first aspect, in one implementation, the physical constraints are divided into absolutely non-relaxable constraints, finitely relaxable constraints, and fully relaxable constraints; the absolutely non-relaxable constraints are assigned an exponential barrier function, the finitely relaxable constraints are assigned a logarithmic barrier function, and the fully relaxable constraints are assigned a quadratic penalty function. The step of reconstructing the optimization objective function into a standard quadratic programming problem includes: The exponential barrier function and the logarithmic barrier function in the constraint penalty term are identified as non-standard quadratic functions. By performing a second-order Taylor approximation on the exponential barrier function and the logarithmic barrier function, a locally quadratic function is obtained. By incorporating the local quadratic function into the Hessian matrix, a standard quadratic programming problem is constructed.

[0018] In conjunction with the first aspect, in one implementation, the solution to obtain the optimal decision variables includes: The Hessian matrix of the standard quadratic programming problem is reduced in dimensionality using the Schur complement rule. The optimal decision variables are obtained by solving the dimensionality-reduced standard quadratic programming problem using a quadratic programming solver.

[0019] In conjunction with the first aspect, in one implementation, the optimal decision variables include the optimal feedforward torque, the desired joint position, and the desired joint velocity, and driving the robot joint motors according to the optimal decision variables includes: Acquire the real-time joint position and real-time joint velocity from the joint sensor; Calculate the positional error between the desired joint position and the real-time joint position, and the velocity error between the desired joint velocity and the real-time joint velocity; The position feedback term is obtained based on the position error and position gain. Based on the speed error and speed gain, the speed feedback term is obtained; The reference torque is obtained based on the optimal feedforward torque, the position feedback term, and the velocity feedback term; The robot joint motors are driven according to the reference torque.

[0020] Secondly, embodiments of this application provide a robot whole-body control system, including: The model building module is used to build the dynamic model of the robot's floating base and the acceleration constraint model between the task joints; The weight generation module is used to introduce continuous state phase variables and generate task activation weights; The penalty design module is used to classify physical constraints by relaxation, introduce relaxation variables, assign penalty function forms to various types of constraints and determine the corresponding penalty coefficients to obtain constraint penalty terms. The objective construction module is used to construct an optimization objective function by combining the task tracking error, task activation weight and constraint penalty term obtained from the acceleration constraint model, and to use the floating base dynamics model and acceleration constraint model as equality constraints, and the physical constraints after introducing relaxation variables as inequality constraints. The solver module is used to solve the optimization objective function to drive the robot's joint motors.

[0021] Thirdly, embodiments of this application provide a robot full-body control device, including a processor, a memory, and a robot full-body control program stored in the memory and executable by the processor, wherein when the robot full-body control program is executed by the processor, it implements the steps of the robot full-body control method as described in any of the preceding claims.

[0022] Fourthly, embodiments of this application provide a computer-readable storage medium storing a robot full-body control program, wherein when the robot full-body control program is executed by a processor, it implements the steps of the robot full-body control method as described in any of the preceding claims.

[0023] The beneficial effects of the technical solution provided in this application include: This application provides a robot whole-body control method, device, equipment, and storage medium. By constructing a dynamic model of a floating base and an acceleration constraint model between task joints, and introducing continuous state phase variables to generate task activation weights, the smoothness of the transition during task switching is quantified. By relaxing and classifying physical constraints, introducing relaxation variables, and assigning penalty function forms and penalty coefficients to obtain constraint penalty terms, rigid constraint conflicts are transformed into measurable optimization costs. An optimization objective function is constructed by combining task tracking error, task activation weights, and constraint penalty terms. The dynamic model of the floating base and the acceleration constraint model between task joints are used as equality constraints, and the physical constraints after introducing relaxation variables are used as inequality constraints. The optimization objective function is solved to drive the robot joint motors, solving the problems of torque jumps caused by task switching and unsolvable and singular problems under constraint conflicts, ensuring control continuity and feasibility. Attached Figure Description

[0024] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0025] Figure 1 This is a flowchart illustrating an embodiment of the robot whole-body control method of this application; Figure 2 This is a functional module diagram of an embodiment of the robot whole-body control device of this application; Figure 3 This is a schematic diagram of the hardware structure of the robot's whole-body control device involved in the embodiments of this application. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0027] In a first aspect, embodiments of this application provide a method for whole-body robot control.

[0028] In one embodiment, reference is made to Figure 1 , Figure 1 This is a schematic flowchart of an embodiment of the robot whole-body control method of this application. Figure 1As shown, the robot's whole-body control method includes: S101: Construct a dynamic model of the robot's floating base and an acceleration constraint model between task joints.

[0029] In this embodiment, a legged robot with multiple degrees of freedom (such as a bipedal or quadrupedal robot) is used as the controlled object. A dynamic model of the robot's floating base is established, and its mathematical expression is as follows:

[0030] in, Indicates joint position. Indicates joint velocity. This represents the generalized joint acceleration, and n+6 represents the 6 virtual floating base degrees of freedom and n actual driven joint degrees of freedom; The robot's inertia matrix; The matrix represents the centrifugal force and the Coriolis force. This is the term related to gravity. Select a matrix for the joints; Joint torque; The contact Jacobian matrix supports the contact between the sole of the foot and the ground; The reaction force is the force generated by the ground at the foot.

[0031] The floating base dynamics model is used to constrain the optimization results to ensure consistency with the robot's body dynamics. Subsequently, in the QP solution, it should be used as an equality constraint, or in the Shure complement dimensionality reduction process to eliminate some variables, for example, by... or Represented as This reduces the optimization dimensionality by using functions that can be optimized.

[0032] Define a set of tasks for the robot in different motion states. For the i-th task (e.g., center of mass posture maintenance, swing leg foot trajectory tracking, etc.), the acceleration constraint model between task joints (i.e., between task space and joint space) can be obtained through kinematic mapping:

[0033] in, The actual acceleration of the i-th task. Let be the task Jacobian matrix for the i-th task. for The derivative with respect to time.

[0034] S102: Introduce continuous state phase variables to generate task activation weights.

[0035] In the traditional hierarchical whole-body control (WBC) framework, task prioritization is achieved through a strict null projection matrix. However, at the moment of task transition (such as when the swinging leg touches the ground and switches to the supporting leg), the abrupt change in the dimensions of the projection matrix can lead to joint torques. It produces discontinuous pulse oscillations.

[0036] To address this issue, this embodiment introduces continuous state phase variables and constructs a dynamic priority adaptive mechanism based on nonlinear smooth transitions. It abandons strict null space projection, integrates all task constraints into the same optimization objective function, and assigns a task activation weight that dynamically evolves with the continuous state phase variables to the i-th task, transforming strict priority ranking into dynamic weight allocation. Through this adaptive weighting, when the state machine undergoes a transition, it ensures the smooth evolution of task weights and the equivalent weighted Jacobian matrix, thereby suppressing torque jumps caused by task additions or subtractions.

[0037] S103: Relax the physical constraints, introduce relaxation variables, assign penalty function forms to each type of constraint and determine the corresponding penalty coefficients to obtain the constraint penalty terms.

[0038] In real-world physical environments, robots are subject to stringent inequality constraints such as friction cone constraints, single-leg support force constraints, and joint limitations. If all of these are set as hard constraints, the quadratic programming (QP) solver is highly susceptible to getting stuck in a no-solution state when encountering strong external disturbances or being in a singular motion configuration.

[0039] This embodiment no longer adopts a uniform hard constraint processing method, but introduces a classification-based local relaxation strategy. Based on safety risks, all inequality constraints in the robot's whole-body control are divided into different categories, and different relaxation variables and penalty functions are configured for the physical characteristics and safety priorities of each type of constraint.

[0040] For example, for critical safety constraints, a safety boundary is preserved and a high-weighted barrier penalty is introduced; for constraints such as friction cones and contact forces that can be slightly violated in the short term, bounded relaxation variables and logarithmic barrier penalties are introduced; for comfort or minor accuracy constraints, a quadratic penalty is used for flexible handling. This mechanism can improve the solvability and smoothness of the optimization problem while ensuring safety, and avoid hard constraint conflicts that lead to unsolvable QP or sudden torque changes.

[0041] S104: Combine the task tracking error, task activation weight and constraint penalty term obtained from the acceleration constraint model to construct an optimization objective function, and use the floating base dynamics model and acceleration constraint model as equality constraints, and the physical constraints after introducing slack variables as inequality constraints.

[0042] S105: Solve the optimization objective function to drive the robot joint motors.

[0043] In this embodiment, by constructing a dynamic model of the floating base and an acceleration constraint model between task joints, and introducing continuous state phase variables to generate task activation weights, the smoothness of the transition during task switching is quantified. By relaxing and classifying physical constraints, introducing relaxation variables, and assigning penalty function forms and penalty coefficients to obtain constraint penalty terms, rigid constraint conflicts are transformed into measurable optimization costs. An optimization objective function is constructed by combining task tracking error, task activation weights, and constraint penalty terms. The dynamic model of the floating base and the acceleration constraint model between task joints are used as equality constraints, and the physical constraints after introducing relaxation variables are used as inequality constraints. The optimization objective function is solved to drive the robot joint motors, solving the problems of torque jumps and unsolvable and singular issues under constraint conflicts caused by task switching, and ensuring control continuity and feasibility.

[0044] Furthermore, in one embodiment, the introduction of continuous state phase variables refers to mapping the state changes during robot task switching to continuous values ​​between 0 and 1, i.e., continuous state phase variables. This is used to characterize the gradual change in the degree of task activation. The specific source of this continuous state phase variable can be flexibly determined according to the actual application scenario. The following are some typical scenarios as examples: Example 1: Foot contact state switching scenario Taking the task of the swing leg touching the ground and switching to the supporting leg as an example, the contact force signal between the foot and the ground can be used as the state input. When the foot is not in contact with the ground, the continuous state phase variable is 0; when the foot is fully in contact with the ground and provides stable support, the continuous state phase variable is 1; during the ground contact transition, the contact force is linearly or non-linearly mapped to an intermediate value between 0 and 1 to achieve a smooth transition of task weight.

[0045] Example 2: Centroid Stability Scenario In balance control tasks, the distance of the robot's center of mass relative to the boundary of the supporting polygon can be normalized to a continuous state phase variable between 0 and 1. The closer the center of mass is to the support boundary, the closer the phase variable is to 1, which is used to smoothly activate the balance recovery task.

[0046] Furthermore, in one embodiment, generating the task activation weight includes: calculating the task activation weight using a smooth transition function based on the continuous state phase variable, a preset smoothing factor, the maximum task activation weight, and the task switching critical phase point.

[0047] The smooth transition function can be a generalized Sigmoid function, in which case the task activation weights... for:

[0048] Where k is a smoothing factor, which determines the urgency of weight switching. Based on experience, its value ranges from 20 to 80. The critical phase point for task switching is obtained from the robot's gait state machine or contact timing, and its value ranges from [0,1]. For the first The maximum activation weight for each task is a normalized factor, ranging from [0,1]. It is generally determined by the importance of the task, and can be set to one of three levels: 1, 0.3–0.8, or 0.1–0.3. For the task exit process, a complementary approach can be used. Alternatively, a reverse Sigmoid function can be used to achieve smooth weight reduction.

[0049] Further, in one embodiment, the relaxation classification of physical constraints includes: dividing the physical constraints into absolutely non-relaxable constraints, finitely relaxable constraints, and fully relaxable constraints; assigning an exponential barrier function to the absolutely non-relaxable constraints, a logarithmic barrier function to the finitely relaxable constraints, and a quadratic penalty function to the fully relaxable constraints; introducing zero relaxation variables for the absolutely non-relaxable constraints; and introducing non-negative relaxation variables for the finitely relaxable and fully relaxable constraints.

[0050] The constraint penalty term for the above three types of constraints is calculated as follows: substitute the slack variable into the corresponding penalty function form to obtain the penalty function value; and obtain the constraint penalty term based on the penalty function value and the corresponding penalty coefficient.

[0051] The following sections elaborate on the constraint penalties for the three types of constraints: (1) For absolutely non-relaxable constraints Violation of such constraints will directly lead to hardware damage or robot malfunction. Therefore, zero slack variables, or no slack variables, are introduced while preserving the safety boundary.

[0052] Typical constraints include hard limits on joint torque, hard limits on joint velocity, and collision distance constraints.

[0053] Taking the joint torque hard limit constraint as an example, its original physical constraint form is as follows: ,in and These are the lower and upper limits of the joint torque, respectively. After introducing zero relaxation variables, the safety boundary remains unchanged, and the constraint form remains unchanged. That is, the obtained relaxation constraint is the same as the original physical constraint.

[0054] For hard limit constraints on joint torques, the corresponding penalty function adopts an exponential barrier function, which is steeper than a logarithmic barrier, and the penalty increases sharply when approaching the boundary.

[0055] Taking joint torque hard limit constraint as an example, constraint penalty item The calculation is as follows:

[0056] in, The penalty coefficient is the absolute non-relaxable constraint. This is a boundary buffer parameter used to adjust the growth slope of the barrier function. It is assigned a value according to the performance requirements of the constraint, and the value range is generally 0.001 to 0.01.

[0057] (2) For finitely relaxed constraints Such constraints, if slightly violated in the short term, will not cause hardware damage or loss of control, but long-term violations will affect the robot's motion performance (such as slippage and decreased trajectory tracking accuracy). Therefore, non-negative relaxation variables are introduced and combined with a logarithmic barrier penalty function for constraint.

[0058] Typical constraints include Coulomb friction cone constraints, nonnegativity constraints, and upper limit constraints on single-leg support forces.

[0059] Taking the Coulomb friction cone constraint as an example, the original physical constraint form is: , of which F x ,F y Let F represent the components of the contact tangential force in the x and y directions in the local coordinate system at the contact point. z For contact normal force, The coefficient of friction of the ground. Based on empirical values ​​or some professional experimental data, the viscosity for ordinary indoor flooring is generally 0.5–0.7, while for rubber flooring it is 0.7–1.2; non-negative relaxation variables are introduced. This is transformed into a relaxation constraint:

[0060] The corresponding penalty function adopts the logarithmic barrier function, which has good smoothness and is suitable for finite relaxation.

[0061] Constraints and penalties The calculation is as follows:

[0062] in, The penalty coefficient is the one for which the constraints can be relaxed to a finite degree. The maximum allowable relaxation amount for this non-negative relaxation variable is finitely relaxed. With absolutely no relaxation constraints The values ​​can be the same or different.

[0063] To prevent excessive relaxation of finitely slack constraints from causing robot loss of control, this embodiment sets a maximum allowable relaxation amount for each finitely slack constraint relaxation variable; wherein, the maximum allowable relaxation amount is determined based on a physical safety threshold.

[0064] For example, friction cone constraints: The maximum allowable slack should not exceed the ground friction coefficient. Contact normal force F z Use 20% of the product to avoid significant slippage.

[0065] For example, nonnegativity constraints: The maximum allowable relaxation amount shall not exceed the nominal contact normal force. Keep your feet at 10% to avoid lifting your toes off the ground.

[0066] For example, the upper limit constraint on single-leg support force: The maximum allowable slack should not exceed the rated output of the motor. 115%.

[0067] (3) For constraints that can be fully relaxed Such constraints are only used to improve motion comfort or secondary task accuracy. Violation will not affect the robot's basic motion capabilities and safety. Therefore, non-negative relaxation variables without fixed upper bounds are introduced, and a quadratic penalty function is used for constraint.

[0068] Typical constraints include soft limits on joint positions, fine-tuning constraints on trunk posture, and secondary posture constraints on the end effector.

[0069] Taking soft joint position limiting as an example, the original physical constraint form is: , Let these be the lower and upper limits of the joint position, respectively, and introduce non-negative relaxation variables without a fixed upper bound. This is transformed into a relaxation constraint:

[0070] Constraints and penalties The calculation is as follows:

[0071] in, The penalty coefficient is the one that allows for complete relaxation of constraints.

[0072] Because a fixed penalty coefficient cannot simultaneously satisfy the requirements of "constraint strictness" and "torque smoothness": if the penalty coefficient is too large, it will cause a sudden change in torque when approaching the constraint boundary; if the penalty coefficient is too small, it will cause excessive violation of the constraint.

[0073] Therefore, this embodiment proposes an adaptive penalty coefficient scheduling method for constraint penalty terms based on the degree of constraint violation, with the penalty coefficient dynamically adjusted according to the urgency of the constraint violation.

[0074] Specifically, determining the corresponding penalty coefficient includes: S201: Calculate the degree of constraint violation based on the actual amount of violation and the maximum permissible amount of violation.

[0075] Define the degree of constraint violation :

[0076] In this embodiment, the actual violation amount and the maximum permissible violation amount are used to quantify the degree to which the constraint has been broken. For example, taking joint position constraints as an example, if the soft limit of the joint is set to 100° and the hard limit to 105°, when the joint actually moves to 102°, the actual violation amount is 2° exceeding the soft limit, and the maximum permissible violation amount is the difference between the hard and soft limits, i.e., 5°. At this time, the degree of constraint violation can be calculated as the ratio of the actual violation amount to the maximum permissible violation amount (i.e., 0.4). This normalized value reflects the severity of the current constraint violation, rather than the absolute physical magnitude, facilitating the subsequent unified processing of constraints with different dimensions.

[0077] S202: Dynamically adjust the basic penalty coefficient according to the degree of constraint violation to obtain an adaptive penalty coefficient, and use the adaptive penalty coefficient as the penalty coefficient.

[0078]

[0079] in, For adaptive penalty coefficient, Basic penalty coefficient, To penalize the growth factor, different constraint types take different values, with the absolute non-slack constraint being one of them. Greater than the finitely relaxed constraints , with finitely relaxed constraints Greater than the fully relaxable constraint For example, constraints must never be relaxed. finitely relaxed constraints The constraints can be completely relaxed. .

[0080] For example, when the constraint violation rate of the friction cone reaches 10%, the penalty coefficient will be increased to 1 / 3 of the base penalty coefficient. When the constraint violation rate of the friction cone reaches 20%, the penalty coefficient is increased to 1 / 3 of the base penalty coefficient. The forced guide solver will pull the state back to the safe domain.

[0081] In this embodiment, the degree of constraint violation is calculated based on the actual amount of violation and the maximum permissible amount of violation, the degree of constraint deviation is quantified, and the basic penalty coefficient is dynamically adjusted according to the degree of constraint violation to obtain an adaptive penalty coefficient. This allows the penalty coefficient to evolve adaptively, solving the problem that a fixed penalty coefficient cannot simultaneously meet the requirements of "constraint strictness" and "torque smoothness".

[0082] Furthermore, in one embodiment, the task tracking error obtained from the acceleration constraint model is calculated through the following steps: S301: Obtain the robot's expected acceleration for the task.

[0083] The desired acceleration for the task is the reference trajectory information generated in real time by the upper-level motion planner (such as the model predictive controller MPC) based on the robot's motion target.

[0084] Specifically, the upper-level planner calculates the target acceleration in the task space within the current control cycle based on the desired walking speed, stride length, or operational task, and sends it down to the whole-body controller. In this embodiment, 'acquiring' refers to receiving this reference data from the upper-level planner's communication interface.

[0085] S302: Based on the acceleration constraint model, the actual acceleration of the task is obtained.

[0086] The task Jacobian matrix of the i-th task. and its time derivative And the joint velocity and acceleration are substituted into the acceleration constraint model. The actual acceleration of the task can then be calculated. .

[0087] S303: Calculate the difference between the actual acceleration of the task and the expected acceleration of the task, and use it as the tracking error of the task.

[0088] Assume the desired acceleration in the workspace is For the i-th task, its task tracking error is... .

[0089] Further, in one embodiment, the step of constructing an optimization objective function by combining the task tracking error, task activation weight, and constraint penalty term obtained from the acceleration constraint model includes: S401: Calculate the task tracking error based on the weighted quadratic norm of the task activation weights.

[0090] S402: The weighted quadratic norm is summed with the constraint penalty term to obtain the optimization objective function.

[0091] Specifically, the optimization objective function is as follows: :

[0092] Where N is the total number of tasks, such as center of mass attitude maintenance, swing leg trajectory tracking, torso attitude control, and contact force tracking.

[0093] In this embodiment, by calculating the weighted quadratic norm of the task tracking error, quantifying the task priority using the task activation weight, and accumulating constraint penalty terms to construct the optimization objective function, the problem of torque jump and unsolvable constraint conflict caused by task switching is solved, ensuring the feasibility of solution and the smoothness of control.

[0094] Furthermore, in one embodiment, solving the optimization objective function to drive the robot joint motors includes: S501: Reconstruct the optimization objective function into a standard quadratic programming problem and solve it to obtain the optimal decision variables.

[0095] The standard quadratic programming problem takes the following form:

[0096] in, This is the torque regularization weight coefficient, used to adjust the penalty intensity of the joint force torque amplitude in the optimization objective function. Its value ranges from 0.001 to 0.1, and it is adjusted according to the torque requirements in actual experiments, such as by gradually observing the torque peak value, task error, and solution stability. .

[0097] S502: Drive the robot joint motors according to the optimal decision variables.

[0098] In this embodiment, by reconstructing the objective function into a standard quadratic programming problem and solving it to obtain the optimal decision variables, the nonlinear optimization is transformed into an efficient convex optimization form. The global optimal solution is quickly obtained using a standard quadratic programming solver. The robot joint motors are driven according to the optimal decision variables, which meets the high-frequency real-time requirements and ensures the control response speed.

[0099] Furthermore, since the constraint penalty term includes an exponential barrier function and a logarithmic barrier function, which are themselves non-standard quadratic functions, they cannot be directly input into a quadratic programming solver. Therefore, in one embodiment, reconstructing the optimization objective function into a standard quadratic programming problem includes: S601: Identify that the exponential barrier function and logarithmic barrier function in the constraint penalty term are non-standard quadratic functions.

[0100] Since the exponential and logarithmic barrier functions assigned to handle hard constraints and finite relaxation constraints in the constraint penalty terms are mathematically nonlinear and non-quadratic functions, while standard quadratic programming solvers require the objective function to be quadratic, this step aims to identify these function terms that do not conform to the standard quadratic programming form. This provides objects for subsequent quadraticization processing, ensuring that the subsequently constructed optimization problem conforms to the solver's input format requirements and preventing the solver from failing to compute due to format mismatch.

[0101] S602: Perform a second-order Taylor approximation on the exponential barrier function and the logarithmic barrier function to obtain a locally quadratic function.

[0102] For example, at the operating point of the current control cycle, a second-order Taylor expansion is performed on the identified exponential and logarithmic barrier functions. Ignoring higher-order remainder terms, these nonlinear functions are approximated as polynomials containing quadratic, linear, and constant terms. This local approximation, while maintaining computational accuracy at the current time step, transforms the complex nonlinear penalty terms into mathematically simple quadratic functions, making them acceptable and processable by the quadratic programming solver. This achieves linearization compatibility of the nonlinear penalty, laying the foundation for high-frequency solutions.

[0103] S603: Incorporate the local quadratic function into the Hessian matrix to construct a standard quadratic programming problem.

[0104] The coefficients of the quadratic terms obtained in step S602 are accumulated into the Hessian matrix of the objective function, and the coefficients of the linear terms are accumulated into the gradient vector. Through this matrix element update method, the objective function, which originally included nonlinear penalty terms, is reconstructed into a standard quadratic form. The resulting standard quadratic programming problem can be directly input into a general solver for efficient solving, achieving compatibility between nonlinear constraint penalties and efficient quadratic programming solutions, thus improving computational efficiency.

[0105] Furthermore, in one embodiment, the solution to obtain the optimal decision variables includes: S701: Use the Schul complement rule to reduce the dimensionality of the Hessian matrix of the standard quadratic programming problem.

[0106] Since whole-body control involves high-dimensional state variables, directly solving for the inverse of the Hessian matrix is ​​computationally intensive. Using the Schur Complement, some variables in the standard quadratic programming problem can be analytically eliminated, decomposing the original large-scale matrix into low-dimensional sub-blocks. By performing operations on the dimensionality-reduced core matrix, the dimensionality and time consumption of matrix operations are significantly reduced, enabling the optimization problem to be solved within millisecond-level control cycles, meeting the requirements of high-frequency real-time control.

[0107] For example, by using dynamic equality constraints, and Extract and substitute the variables into the objective function to compress the dimension of the optimization variables to the smallest independent subspace.

[0108] S702: Solve the standard quadratic programming problem after dimensionality reduction using a quadratic programming solver to obtain the optimal decision variables.

[0109] Based on the dimensionality-reduced standard quadratic programming problem, a quadratic programming solver is invoked for numerical iterative computation. The solver searches for the global optimum based on the gradient information of the objective function and the constraints, provided the Hessian matrix is ​​positive definite. The final output optimal decision variable includes the optimal feedforward torque required at the current time step. Desired joint position and expected joint velocity This provides precise data support for the subsequent driving of the robot's joint motors, ensuring the optimality and real-time performance of control commands.

[0110] Further, in one embodiment, driving the robot joint motors according to the optimal decision variables includes: S801: Acquire the real-time joint position and real-time joint velocity from the joint sensor.

[0111] S802: Calculate the positional error between the desired joint position and the real-time joint position, and the velocity error between the desired joint velocity and the real-time joint velocity.

[0112] Specifically, the position error is .

[0113] Speed ​​error is .

[0114] S803: Based on the position error and position gain, obtain the position feedback term; based on the velocity error and velocity gain, obtain the velocity feedback term.

[0115] Specifically, the location feedback item is .

[0116] Speed ​​feedback item is .

[0117] in, For position gain, Both are speed gain values, and are empirical values ​​determined based on different robots and motors. Generally, a value can be chosen based on current experience. : = (8~10):1.

[0118] S804: The reference torque is obtained based on the optimal feedforward torque, the position feedback term, and the velocity feedback term.

[0119] The reference torque calculation as follows:

[0120] S805: Drive the robot joint motor according to the reference torque.

[0121] In the actual hardware loop deployment of a control system, there is often a cross-communication framework between the algorithm layer and the underlying hardware SDK (Software Development Kit). To address this, this embodiment constructs a low-latency custom TCP / UDP communication bridge to enable high-frequency data interaction between heterogeneous environments (such as ROS1 on the algorithm control side and ROS2 on the hardware driver side, where ROS stands for Robot Operating System). To ensure that control commands are accurately applied to physical entities, the bridge module embeds a strict joint mapping dictionary. This dictionary is responsible for precisely mapping and converting the algorithm-level URDF (United Robotics Description Format) joint names defined in the algorithm model into underlying hardware indices that are recognized by the actual hardware driver, thereby completing command issuance and sensor status feedback.

[0122] Finally, after receiving the command from the bridge, the joint module uses a feedforward and PD control strategy to combine the position and velocity feedback terms with the optimal feedforward torque sent by the WBC. Add them together to obtain the final reference torque. Reference torque The current is then converted into a reference current and fed into the motor to output torque.

[0123] Secondly, embodiments of this application also provide a robot whole-body control device.

[0124] In one embodiment, reference is made to Figure 2 , Figure 2 This is a functional module diagram of an embodiment of the robot whole-body control device of this application. Figure 2 As shown, the robot's whole-body control device includes: The model building module is used to build the dynamic model of the robot's floating base and the acceleration constraint model between the task joints; The weight generation module is used to introduce continuous state phase variables and generate task activation weights; The penalty design module is used to classify physical constraints by relaxation, introduce relaxation variables, assign penalty function forms to various types of constraints and determine the corresponding penalty coefficients to obtain constraint penalty terms. The objective construction module is used to construct an optimization objective function by combining the task tracking error, task activation weight and constraint penalty term obtained from the acceleration constraint model, and to use the floating base dynamics model and acceleration constraint model as equality constraints, and the physical constraints after introducing relaxation variables as inequality constraints. The solver module is used to solve the optimization objective function to drive the robot's joint motors.

[0125] Furthermore, in one embodiment, the weight generation module introduces continuous state phase variables by mapping state changes during robot task switching to continuous values ​​between 0 and 1.

[0126] Furthermore, in one embodiment, the weight generation module generates task activation weights by: calculating the task activation weights using a smooth transition function based on the continuous state phase variable, a preset smoothing factor, the maximum task activation weight, and the task switching critical phase point.

[0127] Furthermore, in one embodiment, the penalty design module classifies physical constraints into relaxation categories, including: dividing the physical constraints into absolutely non-relaxable constraints, finitely relaxable constraints, and fully relaxable constraints; the penalty design module assigns penalty function forms to each type of constraint, including: assigning an exponential barrier function to the absolutely non-relaxable constraints, a logarithmic barrier function to the finitely relaxable constraints, and a quadratic penalty function to the fully relaxable constraints; the penalty design module introduces relaxation variables, including: introducing zero relaxation variables to the absolutely non-relaxable constraints; and introducing non-negative relaxation variables to the finitely relaxable and fully relaxable constraints.

[0128] Furthermore, in one embodiment, the penalty design module is also used to set a maximum allowable relaxation amount for the relaxation variable of the finitely relaxed constraint; wherein the maximum allowable relaxation amount is determined based on a physical safety threshold.

[0129] Furthermore, in one embodiment, the penalty design module obtains the constraint penalty term by: substituting the slack variable into the corresponding penalty function form to obtain the penalty function value; and obtaining the constraint penalty term based on the penalty function value and the corresponding penalty coefficient.

[0130] Furthermore, in one embodiment, the penalty design module determines the corresponding penalty coefficient by: calculating the degree of constraint violation based on the actual amount of violation and the maximum allowable amount of violation; dynamically adjusting the basic penalty coefficient based on the degree of constraint violation to obtain an adaptive penalty coefficient, and using the adaptive penalty coefficient as the penalty coefficient.

[0131] Furthermore, in one embodiment, the robot's whole-body control device further includes a tracking error acquisition module, used to: acquire the robot's desired task acceleration; obtain the actual task acceleration based on the acceleration constraint model; and calculate the difference between the actual task acceleration and the desired task acceleration as the task tracking error.

[0132] Furthermore, in one embodiment, the objective construction module constructs an optimization objective function by combining the task tracking error, task activation weight, and constraint penalty term obtained from the acceleration constraint model, including: calculating the weighted quadratic norm of the task tracking error based on the task activation weight; and summing the weighted quadratic norm with the constraint penalty term to obtain the optimization objective function.

[0133] Furthermore, in one embodiment, the solving module solves the optimization objective function to drive the robot joint motor, including: reconstructing the optimization objective function into a standard quadratic programming problem and solving it to obtain the optimal decision variables; and driving the robot joint motor according to the optimal decision variables.

[0134] Furthermore, in one embodiment, the solution module reconstructs the optimization objective function into a standard quadratic programming problem, including: identifying the exponential barrier function and logarithmic barrier function in the constraint penalty term as non-standard quadratic functions; performing a second-order Taylor approximation on the exponential barrier function and logarithmic barrier function to obtain a locally quadratic function; and incorporating the locally quadratic function into the Hessian matrix to construct a standard quadratic programming problem.

[0135] Furthermore, in one embodiment, the solution module solves for the optimal decision variables by: reducing the dimensionality of the Hessian matrix of the standard quadratic programming problem using the Schur complement rule; and solving the dimensionality-reduced standard quadratic programming problem using a quadratic programming solver to obtain the optimal decision variables.

[0136] Further, in one embodiment, the solution module drives the robot joint motors according to the optimal decision variables, including: acquiring the real-time joint position and real-time joint velocity from the joint sensor; calculating the position error between the desired joint position and the real-time joint position, and the velocity error between the desired joint velocity and the real-time joint velocity; obtaining a position feedback term based on the position error and the position gain; obtaining a velocity feedback term based on the velocity error and the velocity gain; obtaining the reference torque based on the optimal feedforward torque, the position feedback term, and the velocity feedback term; and driving the robot joint motors according to the reference torque.

[0137] The functions of each module in the above-mentioned robot whole-body control device correspond to the steps in the above-mentioned robot whole-body control method embodiment, and their functions and implementation processes will not be described in detail here.

[0138] Thirdly, embodiments of this application provide a robot full-body control device, which can be a personal computer (PC), laptop computer, server, or other device with data processing capabilities.

[0139] Reference Figure 3 , Figure 3 This is a schematic diagram of the hardware structure of the robot's whole-body control device involved in the embodiments of this application. In the embodiments of this application, the robot's whole-body control device may include a processor, a memory, a communication interface, and a communication bus.

[0140] The communication bus can be of any type and is used to interconnect the processor, memory, and communication interface.

[0141] Communication interfaces include input / output (I / O) interfaces, physical interfaces, and logical interfaces used for interconnecting components within the robot's whole-body control system, as well as interfaces used for interconnecting the robot's whole-body control system with other devices (such as other computing devices or user devices). Physical interfaces can be Ethernet interfaces, fiber optic interfaces, ATM interfaces, etc.; user devices can be displays, keyboards, etc.

[0142] Memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0143] The processor can be a general-purpose processor, which can call the robot full-body control program stored in the memory and execute the robot full-body control method provided in the embodiments of this application. For example, the general-purpose processor can be a central processing unit (CPU). The method executed when the robot full-body control program is called can be referred to in the various embodiments of the robot full-body control method of this application, and will not be repeated here.

[0144] Those skilled in the art will understand that Figure 3The hardware structure shown does not constitute a limitation of this application and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0145] Fourthly, embodiments of this application also provide a computer-readable storage medium.

[0146] The present application has a computer-readable storage medium storing a robot full-body control program, wherein when the robot full-body control program is executed by a processor, it implements the steps of the robot full-body control method as described above.

[0147] The method implemented when the robot's whole-body control program is executed can be referred to in various embodiments of the robot's whole-body control method of this application, and will not be repeated here.

[0148] It should be noted that the sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0149] The terms "comprising" and "having," and any variations thereof, in the specification, claims, and accompanying drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such process, method, product, or apparatus. The terms "first," "second," and "third," etc., are used to distinguish different objects, etc., and do not indicate a sequence, nor do they limit "first," "second," and "third" to different types.

[0150] In the description of the embodiments of this application, terms such as "exemplary," "for example," or "for instance" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as "exemplary," "for example," or "for instance" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of terms such as "exemplary," "for example," or "for instance" is intended to present the relevant concepts in a concrete manner.

[0151] In the description of the embodiments of this application, unless otherwise stated, " / " means "or". For example, A / B can mean A or B. The "and / or" in the text is merely a description of the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist simultaneously, and B exists alone. In addition, in the description of the embodiments of this application, "multiple" means two or more.

[0152] In some processes described in the embodiments of this application, multiple operations or steps are included in a specific order. However, it should be understood that these operations or steps may not be executed in the order they appear in the embodiments of this application, or they may be executed in parallel. The sequence number of the operation is only used to distinguish different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed sequentially or in parallel, and these operations or steps may be combined.

[0153] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device to execute the methods described in the various embodiments of this application.

[0154] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A method for controlling the whole body of a robot, characterized in that, include: Construct a dynamic model of the robot's floating base and an acceleration constraint model between the task joints; Introduce continuous state phase variables to generate task activation weights; Physical constraints are classified into relaxation categories, relaxation variables are introduced, penalty function forms are assigned to each type of constraint, and corresponding penalty coefficients are determined to obtain constraint penalty terms. An optimization objective function is constructed by combining the task tracking error, task activation weight, and constraint penalty term obtained from the acceleration constraint model. The floating base dynamics model and acceleration constraint model are used as equality constraints, and the physical constraints after introducing relaxation variables are used as inequality constraints. The optimization objective function is solved to drive the robot's joint motors.

2. The robot whole-body control method according to claim 1, characterized in that, The introduction of continuous state phase variables includes mapping the state changes during robot task switching to continuous values ​​between 0 and 1.

3. The robot whole-body control method according to claim 1, characterized in that, The generated task activation weight includes: Based on the continuous state phase variable, the preset smoothing factor, the maximum activation weight of the task, and the critical phase point of task switching, the task activation weight is calculated using a smooth transition function.

4. The robot whole-body control method according to claim 1, characterized in that: The relaxation classification of physical constraints includes: classifying the physical constraints into absolutely non-relaxable constraints, finitely relaxable constraints, and fully relaxable constraints. The method of assigning penalty functions to various constraints includes: assigning an exponential barrier function to the absolutely non-relaxable constraint, assigning a logarithmic barrier function to the finitely relaxable constraint, and assigning a quadratic penalty function to the fully relaxable constraint; and / or, the method of introducing relaxation variables includes: introducing a zero relaxation variable to the absolutely non-relaxable constraint; and introducing non-negative relaxation variables to the finitely relaxable constraint and the fully relaxable constraint.

5. The robot whole-body control method according to claim 4, characterized in that, The method further includes: setting a maximum allowable relaxation amount for the relaxation variable of the finitely relaxed constraint; wherein the maximum allowable relaxation amount is determined based on a physical safety threshold.

6. The robot whole-body control method according to claim 1, characterized in that, The obtained constraint penalty term includes: Substituting the slack variables into the corresponding penalty function form, we obtain the penalty function value; The constraint penalty term is obtained based on the penalty function value and the corresponding penalty coefficient.

7. The robot whole-body control method according to claim 1, characterized in that, Determining the corresponding penalty coefficient includes: Calculate the degree of constraint violation based on the actual number of violations and the maximum permissible number of violations; The basic penalty coefficient is dynamically adjusted according to the degree of constraint violation to obtain an adaptive penalty coefficient, which is then used as the penalty coefficient.

8. The robot whole-body control method according to claim 1, characterized in that, The task tracking error obtained from the acceleration constraint model is calculated through the following steps: Obtain the robot's expected acceleration for the task; Based on the acceleration constraint model, the actual acceleration of the task is obtained; The difference between the actual acceleration of the task and the expected acceleration of the task is calculated as the task tracking error.

9. The robot whole-body control method according to claim 1, characterized in that, The step of constructing an optimization objective function by combining the task tracking error, task activation weight, and constraint penalty term obtained from the acceleration constraint model includes: The task tracking error is calculated based on the weighted quadratic norm of the task activation weights; The weighted quadratic norm is summed with the constraint penalty term to obtain the optimization objective function.

10. The robot whole-body control method according to claim 1, characterized in that, Solving the objective function to drive the robot joint motors includes: The optimization objective function is reconstructed into a standard quadratic programming problem and solved to obtain the optimal decision variables; Drive the robot joint motors according to the optimal decision variables.

11. The robot whole-body control method according to claim 10, characterized in that, The physical constraints are divided into absolutely non-relaxable constraints, finitely relaxable constraints, and fully relaxable constraints; the absolutely non-relaxable constraints are assigned an exponential barrier function, the finitely relaxable constraints are assigned a logarithmic barrier function, and the fully relaxable constraints are assigned a quadratic penalty function. The step of reconstructing the optimization objective function into a standard quadratic programming problem includes: The exponential barrier function and the logarithmic barrier function in the constraint penalty term are identified as non-standard quadratic functions. By performing a second-order Taylor approximation on the exponential barrier function and the logarithmic barrier function, a locally quadratic function is obtained. By incorporating the local quadratic function into the Hessian matrix, a standard quadratic programming problem is constructed.

12. The robot whole-body control method according to claim 10, characterized in that, The solution to obtain the optimal decision variables includes: The Hessian matrix of the standard quadratic programming problem is reduced in dimensionality using the Schur complement rule. The optimal decision variables are obtained by solving the dimensionality-reduced standard quadratic programming problem using a quadratic programming solver.

13. The robot whole-body control method according to claim 10, characterized in that, The optimal decision variables include the optimal feedforward torque, the desired joint position, and the desired joint velocity. Driving the robot joint motors according to the optimal decision variables includes: Acquire the real-time joint position and real-time joint velocity from the joint sensor; Calculate the positional error between the desired joint position and the real-time joint position, and the velocity error between the desired joint velocity and the real-time joint velocity; The position feedback term is obtained based on the position error and position gain. Based on the speed error and speed gain, the speed feedback term is obtained; The reference torque is obtained based on the optimal feedforward torque, the position feedback term, and the velocity feedback term; The robot joint motors are driven according to the reference torque.

14. A robot whole-body control system, characterized in that, include: The model building module is used to build the dynamic model of the robot's floating base and the acceleration constraint model between the task joints; The weight generation module is used to introduce continuous state phase variables and generate task activation weights; The penalty design module is used to classify physical constraints by relaxation, introduce relaxation variables, assign penalty function forms to various types of constraints and determine the corresponding penalty coefficients to obtain constraint penalty terms. The objective construction module is used to construct an optimization objective function by combining the task tracking error, task activation weight and constraint penalty term obtained from the acceleration constraint model, and to use the floating base dynamics model and acceleration constraint model as equality constraints, and the physical constraints after introducing relaxation variables as inequality constraints. The solver module is used to solve the optimization objective function to drive the robot's joint motors.

15. A robot whole-body control device, characterized in that, The system includes a processor, a memory, and a robot full-body control program stored in the memory and executable by the processor, wherein when the robot full-body control program is executed by the processor, it implements the steps of the robot full-body control method as described in any one of claims 1 to 13.

16. A computer-readable storage medium, characterized in that, It stores a robot full-body control program, wherein when the robot full-body control program is executed by a processor, it implements the steps of the robot full-body control method as described in any one of claims 1 to 13.

Images