Multi-rigid-body robot and its motion control method, storage medium and computer device

Abstract

The present application provides a multi-rigid-body robot, its motion control method, storage medium, and computer device. The method includes: determining an optimization variable group and a control equation set according to the received planning target, as well as the kinematic information and task information of the multi-rigid-body robot; calculating the vectors and matrices of the control equation set by using the state information of the multi-rigid-body robot and the kinematic model; calculating the optimal solution of the optimization variable group based on the configuration information, vectors, and matrices of the control equation set; and generating corresponding motion control instructions according to the optimal solution of the optimization variable group so as to perform motion control on the multi-rigid-body robot. The present application decouples the kinematic model of the multi-rigid-body robot and the task type, realizes that one task equation adapts to multiple kinematic models, and reduces repetitive design work.

Description

Technical Field

[0001] This application relates to the technical field of multi-rigid-body robots, and particularly to multi-rigid-body robots, their motion control methods, computer-readable storage media, and computer devices. Background Art

[0002] In modern industrial and service fields, due to their high flexibility and complexity, multi-rigid-body robots are widely used in various scenarios such as automated production lines, medical assistance, disaster relief, and home services. Multi-rigid-body robots usually include multiple robotic arms, robotic legs, or other forms of mechanical structures, which are interconnected through joints to form a complex dynamic system. Related multi-rigid-body robot motion control methods usually involve designing specialized kinematic models and control strategies for each type of robot or specific task. Although this method can achieve good control effects, it has problems such as a large amount of repetitive design work, poor adaptability, and difficulty in updating and iterating. When the structure or task requirements of the multi-rigid-body robot change, it is usually necessary to redesign the kinematic model and control strategy, which not only consumes a large amount of time and resources but also limits the flexibility and scalability of the robot system.

[0003] Based on this, this application provides a multi-rigid-body robot, its motion control method, computer-readable storage media, and computer devices to improve related technologies. Summary of the Invention

[0004] The purpose of this application is to provide a multi-rigid-body robot, its motion control method, computer-readable storage media, and computer devices, which decouple the kinematic model of the multi-rigid-body robot from the task type, enabling one task equation to adapt to multiple kinematic models and reducing repetitive design work.

[0005] The purpose of this application is achieved by the following technical solutions:

[0006] In a first aspect, this application provides a motion control method for a multi-rigid-body robot, the method including:

[0007] According to the received planning target, as well as the kinematic information and task information of the multi-rigid-body robot, determine an optimization variable group and a control equation set; the kinematic information is used to indicate a kinematic model and multiple task coordinate systems, the task information is used to define multiple task types and their corresponding task coordinate systems and task equations, the control equations in the control equation set are constructed based on the task equations, and the task equations are target equations or constraint equations;

[0008] Using the state information of the multi-rigid-body robot and the kinematic model, calculate the vectors and matrices of the control equation set;

[0009] Calculate the optimal solution of the optimization variable group based on the configuration information, vectors, and matrices of the control equation set;

[0010] Generate corresponding motion control instructions according to the optimal solution of the optimization variable group for motion control of the multi-rigid-body robot.

[0011] In some embodiments, the calculating the optimal solution of the optimization variable group based on the configuration information, vectors, and matrices of the control equation set includes:

[0012] Perform quadratic optimization calculations for each layer based on the configuration information, vectors, and matrices of the control equation set to obtain corresponding intermediate optimization results for each layer, where the intermediate optimization results include one or more of a null space matrix, a Hessian matrix, a gradient vector, an inequality matrix, and upper and lower bounds;

[0013] Calculate the optimal solution of the optimization variable group using the corresponding intermediate optimization results for each layer.

[0014] In some embodiments, the method further includes:

[0015] Construct an optimization equation set corresponding to the multi-rigid-body robot, where the optimization equation set includes the control equation set, and the optimization equations in the optimization equation set are constructed based on the task equations, and each optimization equation corresponds to a combination of a task coordinate system and a task type;

[0016] Configure the activation status, priority, and weight of each optimization equation; wherein, configure the activation status of each control equation as activated, and the configuration information includes activation status, priority, and weight.

[0017] In some embodiments, the constructing the optimization equation set corresponding to the multi-rigid-body robot includes:

[0018] For each task type, for each task coordinate system corresponding to the task type, construct an optimization equation corresponding to the task coordinate system based on the task equation.

[0019] In some embodiments, the determining the optimization variable group and the control equation set according to the received planning target and the kinematic information and task information of the multi-rigid-body robot includes:

[0020] Determine the corresponding target control state according to the received planning target;

[0021] Determine the optimization variable group and the control equation set corresponding to the target control state based on the kinematic information and task information of the multi-rigid-body robot.

[0022] In some embodiments, calculating the vectors and matrices of the control equations by using the state information of the multi-rigid-body robot and the kinematic model includes:

[0023] Calculating the kinematic state information of each rigid body based on the state information of the multi-rigid-body robot and the kinematic model;

[0024] For each task coordinate system, calculating the vectors and matrices of the corresponding control equations of the task coordinate system by using the kinematic state information of the corresponding rigid body and the desired task coordinate system information.

[0025] In some embodiments, the motion control instruction is a torque instruction, and the optimal solution of the optimization variable group includes the optimal generalized joint acceleration and the optimal constraint force. Generating the corresponding motion control instruction according to the optimal solution of the optimization variable group includes:

[0026] Substituting the optimal generalized joint acceleration and the optimal constraint force into a specified inverse dynamics calculation formula to obtain joint control torque information;

[0027] When it is detected that the joint control torque information satisfies the corresponding torque limit condition, generating the corresponding torque instruction based on the joint control torque information.

[0028] In some embodiments, the motion control instruction is a position instruction. Generating the corresponding motion control instruction according to the optimal solution of the optimization variable group includes:

[0029] When the error between the generalized joint coordinates of the multi-rigid-body robot and the desired generalized joint coordinates is greater than a preset error, after performing hierarchical quadratic optimization, calculating the generalized joint coordinate increment and accumulating it to the generalized joint coordinates until the error between the generalized joint coordinates and the desired generalized joint coordinates is not greater than the preset error or the number of iterations is greater than a preset number;

[0030] When it is detected that the generalized joint coordinates satisfy the specified position limit condition, generating the corresponding position instruction based on the generalized joint coordinates.

[0031] In a second aspect, an embodiment of the present application provides a multi-rigid-body robot, including a control module, a motor, and a motor drive module, where the control module is configured to execute any one of the above methods.

[0032] In some embodiments, the multi-rigid-body robot is a humanoid robot, and the corresponding multiple task coordinate systems of the multi-rigid-body robot respectively correspond to the fuselage, the left foot plane, the right foot plane, the left hand end, and the right hand end of the multi-rigid-body robot.

[0033] In a third aspect, an embodiment of the present application provides a computer-readable storage medium storing a computer program, which when executed by a processor implements any one of the above methods.

[0034] In a fourth aspect, an embodiment of the present application provides a computer device, which includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements any one of the above methods.

[0035] The present application provides a multi-rigid-body robot, its motion control method, a computer-readable storage medium, and a computer device. The kinematic model of the multi-rigid-body robot and the task type are decoupled and designed, enabling one task equation to adapt to multiple kinematic models, and reducing repetitive design work. First, an optimization variable group and a control equation set are determined based on the received planning target, the kinematic information, and the task information of the robot. Subsequently, each matrix and vector of the control equation set are calculated by using the state information of the multi-rigid-body robot in combination with the kinematic model. According to the configuration information of the control equation set (such as weight distribution, priority setting, etc.), the optimal solution of the optimization variable group that satisfies all given constraints is calculated. Finally, specific motion control instructions are generated based on the optimal solution of the optimization variable group to drive the multi-rigid-body robot to complete the predetermined task. By decoupling the kinematic model from the task type, the present application allows the same set of task equations to be adapted to different kinematic models, significantly reducing repetitive design work caused by robot design changes or new task requirements. This decoupled design not only improves the generality and flexibility of the motion control method but also speeds up the development and deployment of new tasks. In addition, by accurately calculating the matrices and vectors of the control equation set, the accuracy and efficiency of the motion control instructions are ensured, thereby improving the accuracy and response speed of the multi-rigid-body robot's operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] The present application will be further described below in conjunction with the accompanying drawings of the specification and the specific embodiments.

[0037] Figure 1 It is a schematic diagram of a design pattern for decoupling a multi-rigid-body model and a task type provided by an embodiment of the present application.

[0038] Figure 2 It is a schematic diagram of the working principle of an equation scheduling module of a general whole-body motion control framework provided by an embodiment of the present application.

[0039] Figure 3 It is a schematic flowchart of a motion control method for a multi-rigid-body robot provided by an embodiment of the present application.

[0040] Figure 4 It is a structural block diagram of a computer device provided by an embodiment of the present application. Detailed implementation manners

[0041] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative efforts shall fall within the protection scope of the present application.

[0042] In the description of the embodiments of the present application, it should be understood that the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include one or more of the described features. In the description of the embodiments of the present application, the meaning of "a plurality" is two or more, unless otherwise specifically defined.

[0043] As a new carrier of general artificial intelligence, humanoid robots (i.e., bipedal robots, anthropomorphic robots) are moving from the laboratory environment to actual usage scenarios. On the way towards mass production, the iterative optimization of the motion mechanism and control algorithm needs to be carried out synchronously. The parallel mechanism has excellent stiffness, load ratio, and concentrated inertia, and can achieve the motion degrees of freedom of the corresponding serial mechanism in a more compact form. At the same time, the parallel mechanism is also more in line with the structure of muscles and bones in the human body and can be adapted to humanoid robots of similar sizes. Therefore, the parallel mechanism is an ideal configuration for humanoid robots.

[0044] Although robotics has become a relatively mature discipline, and common computer-aided design software can already design and simulate the motion and mechanical characteristics of parallel mechanisms, the high-dynamic motion control algorithm for parallel mechanism bipedal robots is still in the exploratory stage. The related motion control algorithms can be classified according to the type of model into: neural network control, model predictive control (MPC), and whole body control (WBC). Among them, neural network control is based on a multi-layer perceptron model and fits the dynamic characteristics and feedback strategies of the physical world through reinforcement learning, relying on fine simulation; model predictive control is based on a simplified single center-of-mass model and calculates the center-of-mass trajectory and support force reference values that meet the dynamic constraints through convex optimization, and omits specific multi-rigid-body dynamic constraints in order to ensure computational efficiency. Only whole body control is based on a complete multi-rigid-body kinematics and dynamics model, and directly outputs the control instructions for the motor drive module through the combined optimization of constraint equations and target equations. Therefore, it is necessary to specifically design the optimization variables and constraint equations of the parallel mechanism.

[0045] To ensure that control commands strictly follow the physical constraints of the actual motion mechanism while providing diverse control objectives, this application provides a general whole-body motion control framework (GWBC, General Whole Body Control) for multi-rigid-body robots, which is in the form of "multi-rigid-body model - scheduler - task equation - solver". The GWBC framework defines the two most critical elements in the control module - the task equation and the solver. By decoupling the multi-rigid-body model and the task equation, it can achieve the maximum degree of component reuse, thus obtaining the effect that the same set of task equations can be adapted to multiple models, significantly reducing the repetitive work in the design process of the control module. At the same time, the scheduler loads the multi-rigid-body model and all optimization equations during initialization and can switch the activation status, priority, and weight of the optimization equations in real time during operation. Subsequently, based on a general quadratic programming solver, this application constructs a hierarchical quadratic programming solver. Through the method of null space projection, subsequent objectives are optimized in turn while strictly ensuring high-priority constraints and objectives. Finally, each optimization equation can customize the input interface to meet the upper-level planning objectives, such as the desired body attitude, the desired end-effector attitude of the arm, the desired sole contact force, etc. Generally speaking, the GWBC framework plays an important role in connecting the upper-level planner to the underlying hardware driver in the entire robot system, especially providing a general template for algorithm combination and requirement changes during the development process, thus achieving rapid product iteration.

[0046] See Figure 1 and Figure 2 , Figure 1 is a schematic diagram of a design pattern that decouples the multi-rigid-body model and the task type provided by an embodiment of this application. Figure 2 is a schematic diagram of the working principle of the equation scheduling module of a general whole-body motion control framework provided by an embodiment of this application. Among them, the multi-rigid-body model refers to the kinematic model of the multi-rigid-body robot.

[0047] This application decouples the kinematic model of the multi-rigid-body robot from the task type. Taking the multi-rigid-body robot as an example of a humanoid robot, as Figure 1As shown, there is no direct connection between the task type and the task coordinate system. Instead, a series of optimization equations are constructed in a combined form. For example, task type A is defined as position and attitude targets, task type B is defined as the static friction constraint of contact force, task type C is defined as the underactuated constraint, and task type D is defined as force and moment targets. Task coordinate system 1 is defined as the fuselage, task coordinate system 2 is defined as the left foot plane, task coordinate system 3 is defined as the right foot plane, task coordinate system 4 is defined as the left hand end, and task coordinate system 5 is defined as the right hand end. The combination results in: Equation 1_A represents the position and attitude targets of the fuselage, Equation 1_C represents the underactuated constraint of the fuselage, Equation 2_A represents the position and attitude targets of the left foot plane, Equation 2_B represents the contact constraint of the left foot plane, Equation 2_D represents the force and moment targets of the left foot plane, and the meanings represented by the remaining optimization equations are obtained according to the combination method.

[0048] Figure 1 When the design pattern shown is used on different types of robots, there is no need to rewrite the specific task types. Instead, only the task coordinate system needs to be added to the multi-rigid body model, and then the task type that matches the control target is selected from the existing task types, and the two are combined to obtain a new optimization equation. Using the decoupled design pattern is one of the characteristics of the general framework, which can significantly improve the code reuse rate, reduce the workload of repeated development, and quickly match new product requirements through combination. Considering the computational efficiency, the optimization equations using the same task coordinate system do not need to repeatedly calculate the kinematic information of the task coordinate system, thereby reducing the computational overhead of the control module and improving the calculation speed.

[0049] In addition to the design pattern of the optimization equations (including the target equations and constraint equations), the general whole-body control framework provided by the embodiments of the present application can also use an equation scheduling module to configure the optimization equations. Specifically, the equation scheduling module can switch between different control states according to the upper-level planning target. As Figure 2 shown, taking the walking plan as an example, the equation scheduling module defines control states A, B, and C. Intuitively, these three control states respectively represent the behaviors of the humanoid robot during walking, where the left leg supports and the right leg swings, the right leg supports and the left leg swings, and both legs support.

[0050] As an example, the control equation sets included in control state A are: the underactuated constraints of the fuselage, the static friction constraints of the left foot sole force, the left foot sole force and moment targets, and the position and attitude targets of the fuselage, the right foot plane, and the ends of the left and right hands; the control equation sets included in state B are: the underactuated constraints of the fuselage, the static friction constraints of the right foot sole force, the right foot sole force and moment targets, and the position and attitude targets of the fuselage, the left foot plane, and the ends of the left and right hands; the control equation sets included in state C are: the underactuated constraints of the fuselage, the static friction constraints of the left and right foot sole forces, the left and right foot sole force and moment targets, and the position and attitude targets of the fuselage and the ends of the left and right hands.

[0051] It can be seen that the included control equation sets are different under different control states. For example, after decoupling the multi-rigid-body model and the task type, 10 optimization equations can be obtained as the optimization equation set. However, for a specific control state, the included control equation set can be a subset of the optimization equation set, such as including 7 optimization equations, 8 optimization equations, etc. The optimization equations included in a specific control state are called the corresponding control equations of this control state, and the set of these control equations is called the corresponding control equation set of this control state to distinguish it from the complete optimization equation set. Then, a specific control state corresponds to a specific control equation set.

[0052] In the initialization stage of motion control, the complete optimization equation set corresponding to the multi-rigid-body model can be loaded. During the actual use process, the control state of the equation scheduling module can be switched in a timely manner to activate the control equation set corresponding to the current control state (that is, configure the activation state of this part of the optimization equations to be activated), and configure parameters such as the priority and weight of each control equation (or use the default configuration). In other words, when the activation state of an optimization equation is configured to be activated, this optimization equation can be used as a control equation under the current control state.

[0053] The purpose of this application is to further reduce the design complexity and improve the applicability of the control strategy while ensuring the control accuracy and response speed, and automatically adapt to different kinematic models and task requirements. Therefore, this application adopts a decoupled design method. By separating the kinematic model from the task type, a general whole-body motion control framework is realized, allowing the same set of task equations and the same set of control strategies to be adapted to different kinematic models. Through a more efficient decoupled design of the kinematic model and task type of the multi-rigid-body robot, one task equation can be adapted to multiple kinematic models, thereby effectively reducing the repetitive development work caused by robot design modifications or task changes, and improving the flexibility and versatility of the multi-rigid-body robot system.

[0054] The specific implementation manners of this application will be described below.

[0055] See Figure 3 ,Figure 3 It is a schematic flowchart of a motion control method for a multi-rigid-body robot provided by an embodiment of the present application.

[0056] In order to improve the related art, an embodiment of the present application provides a motion control method for a multi-rigid-body robot, and the method includes steps S101 to S104.

[0057] Step S101: Determine an optimization variable group and a control equation set according to the received planning target, the kinematic information, and the task information of the multi-rigid-body robot; the kinematic information is used to indicate a kinematic model and a plurality of task coordinate systems, the task information is used to define a plurality of task types and their corresponding task coordinate systems and task equations, the control equations in the control equation set are constructed based on the task equations, and the task equations are target equations or constraint equations.

[0058] Step S102: Calculate the vectors and matrices of the control equation set by using the state information of the multi-rigid-body robot and the kinematic model.

[0059] Step S103: Calculate the optimal solution of the optimization variable group based on the configuration information, vectors, and matrices of the control equation set.

[0060] Step S104: Generate corresponding motion control instructions according to the optimal solution of the optimization variable group so as to perform motion control on the multi-rigid-body robot.

[0061] The above method can be executed on the control module of the multi-rigid-body robot, and the control module is used to execute steps S101 to S104. The multi-rigid-body robot is, for example, a humanoid robot (i.e., a biped robot, a humanoid robot), a quadruped robot, a wheeled robot, etc. As its name implies, a multi-rigid-body robot is a robot that includes multiple rigid bodies. As an example, the multi-rigid-body robot is, for example, a humanoid robot, and the humanoid robot includes a fuselage, two robotic arms (left arm and right arm), and two robotic legs (left leg and right leg). Each robotic arm is composed of 6 single-degree-of-freedom joints connected in series, and each robotic leg is composed of 6 single-degree-of-freedom joints connected in series. The motion of the fuselage relative to the global coordinate system can be represented by a 6-degree-of-freedom virtual joint (i.e., Figure 2 the floating base virtual joint in

[0062] The contact model between the sole of the foot and the ground is defined as a rectangular surface contact composed of an anchor point and four vertices. There is a three-dimensional force vector at each vertex, which satisfies the static friction constraint. The force vectors at the four vertices are combined at the anchor point to calculate the resultant force and resultant moment of the contact. Therefore, the dimension of the contact force for each sole is 12. The static friction coefficient of the contact force can be estimated based on the lower material and the ground material. The modeling method of the plantar contact force can refer to the related technology, which will not be elaborated in this application.

[0063] The generalized joint coordinates q of the multi-rigid body model include: the 6-degree-of-freedom joint of the fuselage composed of translation and rotation, which are represented by three-dimensional vectors and quaternions respectively; all single-degree-of-freedom joints on the robotic arm and robotic legs, and their coordinates are represented by multi-dimensional vectors Therefore, the dimension of the generalized joint coordinates q is 31. The generalized joint velocity v includes: the linear velocity and angular velocity of the fuselage, which are represented by three-dimensional vectors and respectively; the angular velocities of all single-degree-of-freedom joints, which are represented by multi-dimensional vectors Therefore, the dimension of the generalized joint velocity v is 30. The dimension of the generalized joint coordinate increment δq and the generalized joint acceleration is the same as that of v. Therefore, when the generalized joint acceleration and the constraint force λ are used as the optimization variables x in the subsequent calculation process, the maximum dimension of the solution space is 54.

[0064] In the above embodiment, the planning objectives for the whole-body motion control of the humanoid robot may include, for example: the positions and postures of the fuselage, left and right feet, and left and right hands in the global coordinate system; the desired contact forces between the left and right feet and the ground. The constraints include: the underactuated constraint of the fuselage, and the contact force satisfies the static friction constraint. Therefore, the task coordinate systems that need to be added to the multi-rigid body model are: the fuselage, the planes of the left and right feet, and the ends of the left and right hands. As an example, the kinematic information and task information of the multi-rigid body robot can be loaded through the configuration module during the initialization of the control module.

[0065] As described above, the control equation set is a subset of the optimization equation set. The optimization equation set includes, for example, multiple optimization equations, and the control equation set includes, for example, one or more control equations. As an example, when configuring the optimization equation set, the activation status of some optimization equations can be configured as activated, so as to determine this part of the set of optimization equations as the control equation set.

[0066] Each task type corresponds to one or more task coordinate systems, and each combination of a task type and a task coordinate system corresponds to one optimization equation. As an example, if task type A corresponds to 5 task coordinate systems (e.g., task coordinate systems 1-5), then for task type A, 5 optimization equations can be formed, corresponding to the combinations of task type A and task coordinate system 1, task type A and task coordinate system 2, task type A and task coordinate system 3, task type A and task coordinate system 4, and task type A and task coordinate system 5 respectively.

[0067] It should be noted that the task equation is different from the optimization equation. The task equation corresponds one-to-one with the task type and serves as the basis for constructing the optimization equation. That is, for each task type, there is a unique task equation. On this basis, one or more corresponding optimization equations of the task type can be constructed based on the task equation.

[0068] The embodiments of the present application do not limit the manner of constructing the optimization equation based on the task equation. In some embodiments, the method may further include: constructing an optimization equation set corresponding to the multi-rigid-body robot, the optimization equation set including the control equation set, and the optimization equations in the optimization equation set are constructed based on the task equation, and each optimization equation corresponds to a combination of a task coordinate system and a task type; configuring the activation state, priority, and weight of each optimization equation; wherein, the activation state of each control equation is configured as activated, and the configuration information includes the activation state, priority, and weight.

[0069] In some embodiments, the constructing the optimization equation set corresponding to the multi-rigid-body robot may include: for each task type, for each task coordinate system corresponding to the task type, constructing one optimization equation corresponding to the task coordinate system based on the task equation.

[0070] The task equation will be illustrated by examples below.

[0071] In a specific application scenario, Figure 1 the specific definitions of task types A, B, C, and D are as follows.

[0072] Task type A is the position and attitude target of the task coordinate system in the global coordinate system, and the expression is:

[0073]

[0074] where the subscript opt is the name of the task coordinate system, J is the Jacobian matrix of linear velocity and angular velocity, is the velocity-product acceleration, K p is the feedback gain of the position and attitude error, K dis the feedback gain of the linear velocity and angular velocity errors, Δp is the position error, Δr is the attitude error, Δv is the linear velocity error, and Δω is the angular velocity error. The error is obtained by subtracting the current state value from the expected value. Therefore, the task equation corresponding to task type A is the target equation, and this target equation is a 6-dimensional system of equalities.

[0075] Task type B is the static friction constraint of the contact force. According to the contact model, it only needs all components of the contact force to be greater than 0, and the expression is:

[0076]

[0077] where the subscript ct is the name of the contact coordinate system (i.e., the task coordinate system where contact occurs), and λ ct is part of the complete contact force parameter λ. According to the contact model, the dimension of the contact force for each plane is 12. Therefore, the task equation corresponding to task type B is the constraint equation, and this constraint equation is a 12-dimensional system of inequalities.

[0078] Task type C is the underactuated dynamic constraint. The 6-degree-of-freedom virtual joint between the fuselage and the global coordinate system cannot apply forces and torques, and the expression is:

[0079]

[0080] where I 6×6 is a 6-dimensional identity matrix, 0 6×24 is a 6×24-dimensional zero matrix, M is the joint space inertia matrix, is the transpose of the projection matrix of the constraint force to the joint space, and C is the gravity and Coriolis force vector in the joint space. Therefore, the task equation corresponding to task type C is the constraint equation, and this constraint equation is a 6-dimensional system of equalities.

[0081] Task type D is the force and torque target of the contact coordinate system, and the expression is:

[0082]

[0083] where Γ ct,u is the projection matrix from the contact force parameter to the contact force and torque, F ct,ref and T ct,ref are the expected resultant force and expected resultant torque at the anchor point position. Therefore, the task equation corresponding to task type D is the target equation, and this target equation is a 6-dimensional system of equalities.

[0084] In some embodiments, the control module obtains the planning target provided by the upper layer through a custom input interface for the optimization equation. After that, it can switch the control state of the equation scheduling module, calculate the dimensions of the optimization variables and the system of equations after the control state is switched, and update the dimensions of the matrices and vectors. The optimization variables areFigure 2 The optimized joint variables therein show that the optimized variable groups under different control states can be different.

[0085] As an example, the planning goals provided by the upper layer are, for example, the desired walking gait, the fuselage speed, and the positions and postures of the ends of the left and right hands in the global coordinate system specified by the remote controller. Among them, the equation scheduling module can switch between three control states: left leg supporting and right leg swinging, right leg supporting and left leg swinging, and both legs supporting, according to the desired walking gait. The model predictive control module calculates the fuselage motion trajectory in the next 1-2 seconds, the desired sole contact force, and the positions and postures of the next landing points according to the desired walking gait and the fuselage speed. The desired positions and postures of the ends of the left and right hands in the global coordinate system are directly input into the target equation of the whole-body motion control.

[0086] In the above embodiment, the optimized variable group and the control equation set are determined according to the received planning goals and the kinematic information and task information of the robot. The kinematic information includes, for example, the kinematic model of the multi-rigid-body robot and the task coordinate systems added to the kinematic model, while the task information defines the specific task types to be completed, the task coordinate systems related to the task types, and the corresponding task equations, which can be target equations or constraint equations. These task equations are intended to specify the target states that the multi-rigid-body robot needs to reach or the physical constraint conditions to be followed during the execution of specific tasks. Subsequently, each matrix and vector of the control equation set is calculated by combining the state information of the multi-rigid-body robot with the kinematic model. This step is to transform the theoretical model into executable control logic. During the calculation process, according to the configuration information of the control equation set (such as weight distribution, priority setting, etc.), the optimal solution of the optimized variable group that satisfies all given constraints is calculated. Finally, specific motion control instructions are generated according to the optimal solution of the optimized variable group to drive the multi-rigid-body robot to complete the predetermined task.

[0087] By decoupling the kinematic model from the task type in the above embodiment, it is allowed that the same set of task equations can be adapted to different kinematic models, significantly reducing the repetitive design work caused by robot design changes or new task requirements. This decoupled design not only improves the generality and flexibility of the motion control method but also speeds up the development and deployment of new tasks. By accurately calculating the matrices and vectors of the control equation set, the accuracy and efficiency of the motion control instructions are ensured, thereby improving the accuracy and response speed of the multi-rigid-body robot operation. In addition, this motion control method can improve the overall performance of the robot system, reduce energy consumption, and enhance the operation stability and reliability of the multi-rigid-body robot in complex environments.

[0088] In some embodiments, determining the optimization variable group and the control equation set according to the received planning objective and the kinematic information and task information of the multi-rigid-body robot (i.e., step S101) may include: determining the corresponding target control state according to the received planning objective; and determining the optimization variable group and the control equation set corresponding to the target control state based on the kinematic information and task information of the multi-rigid-body robot.

[0089] When the control state is switched, according to the latest control state (i.e., the target control state), the corresponding optimization variable group and the control equation set are determined. Then, based on the configuration information of the control equation set in this control state, the optimization variables and the dimension of the equation set are recalculated. Taking Figure 2 control state A as an example, in the configuration, the left leg joint acceleration is omitted, and when only the left foot is in contact, the optimization variable group includes the selected 24-dimensional generalized joint acceleration and the 12-dimensional contact force λ of the left foot plane. Therefore, the dimension of the equation set variable x is 42, and the dimension of the P0-layer optimization variable z 0 is 36. The control equation set in this control state includes 4 task types A, 1 task type B, 1 task type C, and 1 task type D. The overall expression is:

[0090] Ax = b,

[0091] According to the corresponding definitions of the task types, the dimension of the equality equation in this control state is 36, and the dimension of the inequality equation is 12. Therefore, the dimension of the equality equation matrix A is 36×42, the dimension of b is 36×1, the dimension of the inequality equation matrix D is 12×42, f and the dimension of is 12×1. It can be seen that GWBC supports the complete reduction mapping from joint variables to optimization variables (the optimization variable group can be regarded as a subset of the joint variable group). By ignoring the joints with smaller inertia, the computational efficiency and stability of the solver are significantly improved under the condition that the overall control effect is hardly affected.

[0092] In some embodiments, calculating the vectors and matrices of the control equation set by using the state information of the multi-rigid-body robot and the kinematic model (i.e., step S102) may include: calculating the kinematic state information of each rigid body based on the state information of the multi-rigid-body robot and the kinematic model; and for each task coordinate system, calculating the vectors and matrices of the control equation corresponding to the task coordinate system by using the kinematic state information of the corresponding rigid body and the desired task coordinate system information.

[0093] In the above embodiments, the state information of the multi-rigid-body robot may include joint state information and fuselage state information. For example, the multi-rigid-body robot further includes joint encoders and a state estimation module, and the control module may obtain the joint angle q from the joint encoders i and angular velocity Moreover, the control module may also obtain the position p of the fuselage from the state estimation module base , attitude η base , linear velocity v base and angular velocity ω base . Optional state estimation algorithms include linear Kalman filter algorithm, extended Kalman filter algorithm, real-time positioning and mapping algorithms based on vision and lidar, etc.

[0094] As an example, the state information of the multi-rigid-body robot may include generalized joint coordinates q and generalized joint velocity v. According to the obtained kinematic model of the multi-rigid-body robot, generalized joint coordinates q and generalized joint velocity v, using the forward kinematics recurrence formula, the position of each rigid body can be calculated attitude linear velocity and angular velocity and Jacobian matrix as the kinematic state information of the corresponding rigid body. Where the subscript b i is the name of the i-th rigid body, and the rigid body corresponds to the rigid body coordinate system one by one. Using the forward dynamics recurrence formula to calculate the joint space inertia matrix M, gravity and Coriolis force vector C. Optionally, the forward kinematics and forward dynamics recurrence formulas may adopt related technologies, which will not be elaborated in this application

[0095] For a specific task coordinate system, according to the kinematic state information of the rigid body to which the task coordinate system adheres and the task coordinate system information required in the current control state (i.e., the desired task coordinate system information), the position p of the task coordinate system can be calculated opt , attitude η opt , linear velocity v opt and angular velocity ω opt and Jacobian matrix J opt , to obtain the vector and matrix of the control equation corresponding to the task coordinate system. The specific calculation method is as follows:

[0096]

[0097] Among them, the subscript opt is the name of the task coordinate system, b i is the name of the rigid body to which the task coordinate system adheres is the rotation matrix corresponding to the rigid body attitude is the translation transformation of the task coordinate system relative to the rigid body coordinate system is the quaternion of the relative rotation transformation

[0098] According to the kinematic state information of the task coordinate system and the control objective of the motion planning (i.e., the planning objective), the calculation method of the error in the task equation is as follows:

[0099] △p opt = p opt,ref - p opt

[0100]

[0101] Δυ opt = υ opt,ref - υ opt

[0102] Δω opt = ω opt,ref - ω opt .

[0103] Among them, the subscript ref represents the expected value, R is the rotation matrix corresponding to the attitude quaternion, and SO3log(·) is a function that converts the rotation matrix into an error vector in the tangent space.

[0104] According to the kinematic state information of the task coordinate system and the above error calculation method, at the beginning of each control cycle, the vector and matrix of all control equations included in the current control state can be calculated.

[0105] In the above embodiment, the planning objective can come from a custom interface or the upper layer. Moreover, the planning objective received through the custom interface can be for the entire multi-rigid-body robot or a specific rigid body, or for a specific optimization equation.

[0106] Kinematics information is used to indicate the kinematic model of a multi-rigid-body robot and multiple task coordinate systems. Each task type is associated with a specific task coordinate system, defined by the task information, and each task type corresponds to a task equation, which can be an objective equation or a constraint equation. Then, based on the defined task information, a corresponding set of optimization equations is constructed, and some of the optimization equations are determined as the control equations from the set of optimization equations. These control equations are directly used to calculate the optimal solution of the optimization variable group. The calculation process of the optimal solution takes into account all task-related planning objectives and constraint conditions to be satisfied, ensuring the global optimality and applicability of the solution. In this way, precise motion control instructions can be dynamically generated for different kinematic states and task requirements. By dynamically integrating and optimizing the motion control instructions, the motion efficiency and precision of the multi-rigid-body robot in a complex environment are significantly improved. The multi-rigid-body robot can quickly adapt to and respond to the requirements of different planning objectives, enhancing the flexibility and applicability of the operation. In addition, through the precise calculation of the optimization variable group, the stability and reliability of the motion of the multi-rigid-body robot are maximized, and the error rate during the motion execution is reduced.

[0107] As described above, the present application provides a hierarchical quadratic optimization solver for calculating the optimal solution of the optimization variable group. Specifically, in some embodiments, calculating the optimal solution of the optimization variable group (i.e., step S103) based on the configuration information, vectors, and matrices of the control equation set may include: performing quadratic optimization calculations on each layer according to the configuration information, vectors, and matrices of the control equation set to obtain corresponding intermediate optimization results for each layer, where the intermediate optimization results include one or more of a null space matrix, a Hessian matrix, a gradient vector, an inequality matrix, and upper and lower bounds; and calculating the optimal solution of the optimization variable group using the corresponding intermediate optimization results for each layer.

[0108] In the above embodiments, according to the equation set configuration of the current moment control state (i.e., the target control state), calculate the null space matrix, Hessian matrix, gradient vector, inequality matrix, and upper and lower bounds of the quadratic optimization for each layer, and use the quadratic optimization solver to calculate the optimal solution that satisfies the constraints.

[0109] Take Figure 2 control state A as an example. The equation set for full-body motion control is divided into three levels in total. The equation set of layer P0 includes control equations 1_C and 2_B, and according to the multi-rigid-body model and task type, an n 0 = 36-dimensional optimization variable z 0 、6-dimensional equality equation, 12-dimensional inequality equation, and slack variable are obtained. The corresponding expression for quadratic optimization is:

[0110]

[0111] Among them, the subscript 0 represents the control task of the 0th layer, s is the slack variable of the inequality constraint, H is the Hessian matrix, and g is the gradient vector, and their expressions are as follows:

[0112]

[0113] Use the quadratic optimization calculation module to obtain the optimal variables of the P0 layer and Calculate the null space of the P0 layer from the equation matrix, and use QR decomposition to write it as the product of an orthogonal matrix Q 0 and an upper triangular matrix R 0 Calculate the matrix R using the basic matrix calculation module 0 to obtain the rank r 0 of R, and the null space matrix N 1 is equal to the rightmost n 0 columns of Q 0 ―r 0 The expression is as follows:

[0114]

[0115] The optimized variable z 1 of the equation set of the P1 layer has a dimension n 1 = n 0 ―r 0 According to the null space matrix N 1 accumulate z 1 to to obtain the generalized joint variable of the P1 layer, and the expression is as follows:

[0116]

[0117] The equation set of the P1 layer includes the control equations 1_A, 3_A, and 2_D, and the optimized variable z 1 also needs to satisfy the inequality constraints of the P0 layer. Therefore, the dimension of the equality constraint of the P1 layer is 18, and the dimension of the inequality constraint is 12. Since there are no inequality constraints in the P1 layer, the dimension of the slack variable is 0, and the corresponding quadratic optimization expression is:

[0118]

[0119] Among them, is the optimal variable obtained by quadratic optimization of the P0 layer, H 1 is the Hessian matrix, and g 1 is the gradient vector, which is calculated from the equality equations of the P1 layer and the null space matrix N 1 of the P0 layer, the optimal variable The expression is as follows:

[0120]

[0121] According to the equation matrix of layer P1 and the null space matrix N of layer P0 1 , calculate the null space matrix N of layer P1 2 :

[0122]

[0123] where r 1 is the rank of matrix R 1 , and thus obtain the dimension n 2 of the optimization variable z of layer P2 2 = n 1 ― r 1 . Similarly, the quadratic optimization expression of layer P2 can be obtained:

[0124]

[0125]

[0126] where is the optimal variable obtained by quadratic optimization of layer P1, H 2 is the Hessian matrix, g 2 is the gradient vector, which is calculated from layer P2 and the null space matrix N 2 of layer P1, and the optimal variable , and its expression is:

[0127]

[0128] Accumulate the optimization results of each layer based on the null space matrix to obtain the optimal generalized joint acceleration and constraint force, which are used as the optimal solution of the optimization variable group:

[0129]

[0130] After obtaining the optimal solution of the optimization variable group, it is necessary to generate motion control instructions for the multi-rigid-body robot. The motion control instructions can be torque instructions or position instructions, and this application does not limit this.

[0131] In some embodiments, the motion control instructions can be torque instructions. The optimal solution of the optimization variable group includes the optimal generalized joint acceleration and the optimal constraint force. Generating the corresponding motion control instructions according to the optimal solution of the optimization variable group (i.e., step S104) can include: substituting the optimal generalized joint acceleration and the optimal constraint force into the specified inverse dynamics calculation formula to obtain joint control torque information; and generating the corresponding torque instructions based on the joint control torque information when it is detected that the joint control torque information satisfies the corresponding torque limit conditions.

[0132] In the above embodiments, according to the result of hierarchical quadratic optimization, the optimal generalized joint acceleration and constraint force are substituted into the inverse dynamics to obtain the joint control torque information, and then it is checked whether the joint control torque information meets the corresponding performance limitations (for example, the torque limit conditions corresponding to the joint motors). If it meets, a torque command is generated and sent to the motor drive module corresponding to the joint motor.

[0133] The optimal generalized joint acceleration is obtained according to hierarchical quadratic optimization and the constraint force λ * , and the inverse dynamics recurrence formula (i.e., the inverse dynamics calculation formula) of the multi-rigid-body model is used to calculate the joint control torque information, and its expression is:

[0134]

[0135] where τ * is the optimal joint control torque (i.e., the joint control torque information), and values are taken from the position corresponding to the i-th joint according to the arrangement order of the joints of the multi-rigid-body model and restricted within the maximum torque of the corresponding joint motor. The torque command is sent to the corresponding motor drive module using the hardware communication module of the multi-rigid-body robot, and thus the torque control of the multi-rigid-body robot can be realized.

[0136] In some other embodiments, the motion control command may be a position command, and generating the corresponding motion control command according to the optimal solution of the optimization variable group (i.e., step S104) may include: when the error between the generalized joint coordinates and the desired generalized joint coordinates of the multi-rigid-body robot is greater than a preset error, after hierarchical quadratic optimization, calculating the generalized joint coordinate increment and accumulating it to the generalized joint coordinates until the error between the generalized joint coordinates and the desired generalized joint coordinates is not greater than the preset error or the number of iterations is greater than a preset number; when it is detected that the generalized joint coordinates meet the specified position limit conditions, generating the corresponding position command based on the generalized joint coordinates.

[0137] According to the result of hierarchical quadratic optimization, iteratively accumulate the generalized joint coordinate increment until the target error converges within the threshold or exceeds the maximum number of iterations, check whether the control command meets the position limit, and send the position command to the motor drive module.

[0138] According to the initial value q init of the generalized joint coordinates of the obtained multi-rigid-body robot and the error calculation method of the control equation, calculate the error Δ eq of all equality equations in the control equation set, and judge whether the initial error is less than the convergence threshold Its expression is:

[0139]

[0140] If the initial error is greater than the convergence threshold, hierarchical quadratic optimization is performed using a hierarchical quadratic optimization solver to obtain the generalized joint coordinate increment δq * , and its expression is:

[0141]

[0142] The generalized joint coordinate increment δq * is accumulated to the initial value q init to calculate the error again. The iteration is repeated until the error is less than the convergence threshold or the number of iterations t exceeds the maximum value T to obtain the optimal generalized joint coordinate q * , and its expression is:

[0143]

[0144] According to the arrangement order of the joints of the multi-rigid body model, values are taken from the positions corresponding to the u-th joint and restricted within the physical limits of the corresponding joint motors . The position command is sent to the corresponding motor drive module using the hardware communication module of the multi-rigid body robot, and the position control of the multi-rigid body robot can be achieved.

[0145] The embodiment of the present application also provides a multi-rigid body robot, including a control module, motors, and motor drive modules, and the control module is used to execute any one of the above methods.

[0146] In some embodiments, the multi-rigid body robot can be a humanoid robot, and the corresponding multiple task coordinate systems of the multi-rigid body robot respectively correspond to the fuselage, left foot plane, right foot plane, left hand end, and right hand end of the multi-rigid body robot.

[0147] In some embodiments, the multi-rigid body robot may further include one or more of a configuration module, an equation scheduling module, a model predictive control module, a joint encoder, a state estimation module, a hierarchical quadratic optimization solver, a fundamental matrix calculation module, and a hardware communication module. Among them, the hierarchical quadratic optimization solver includes, for example, a quadratic optimization calculation module.

[0148] In some embodiments, one or more of the configuration module, equation scheduling module, model predictive control module, state estimation module, hierarchical quadratic optimization solver, and fundamental matrix calculation module can be integrated with the control module.

[0149] The embodiments of the present application further provide a computer-readable storage medium storing a computer program, which when executed by a processor implements any of the above methods.

[0150] The embodiments of the present application further provide a computer program product including a computer program, which when executed by a processor implements any of the above methods.

[0151] The computer program product may be a portable compact disc read-only memory (CD-ROM) including program code and may run on a terminal device such as a personal computer. However, the computer program product of the present application is not limited thereto, and the computer program product may adopt any combination of one or more computer-readable media.

[0152] The embodiments of the present application further provide a computer device including a memory and a processor, where the memory stores a computer program, and the processor implements any of the above methods when executing the computer program.

[0153] See Figure 4 , Figure 4 which is a structural block diagram of a computer device provided by the embodiments of the present application.

[0154] The embodiments of the present application do not limit the computer device, which may be, for example, a local computer device, a cloud computer device, a distributed computer device, etc.

[0155] The computer device may include: a memory 110, a processor 120, and a communication interface 130. Among them, the memory 110, the processor 120, and the communication interface 130 are connected through an internal connection path.

[0156] The memory 110 is used to store a computer program. In some implementation manners, the computer program may include code for implementing the method of the embodiments of the present application.

[0157] The processor 120 is used to execute the computer program stored in the memory 110 to control the communication interface 130 to receive input data and information and output operation result data, etc. In some implementation manners, when implementing the solution of the embodiments of the present application through software or firmware, the computer program for implementing the solution of the embodiments of the present application may be stored in the processor 120 and executed by the processor 120.

[0158] The memory 110 can be a volatile memory, a non-volatile memory, or can include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory can be a random access memory (RAM). It should be noted that the memory 110 described herein is intended to include, but is not limited to, any of these and other suitable types of memories. As an example, the memory 110 includes a random access memory (RAM), a cache memory, and a read-only memory (ROM). Among them, the memory 110 stores a computer program, and the computer program can be executed by the processor 120, so that the processor 120 implements the steps of any of the above methods.

[0159] The processor 120 can be a central processing unit (CPU), and the processor 120 can also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor, or, the processor 120 can also be any conventional processor, etc.

[0160] In the implementation process, each step of the above method can be completed by the integrated logic circuit of the hardware in the processor 120 or the instructions in the form of software. The method disclosed in combination with the embodiments of the present application can be directly embodied as being executed and completed by a hardware processor, or can be executed and completed by a combination of the hardware and software modules in the processor 120. The software module can be located in a mature storage medium in the art such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, or an electrically erasable programmable memory, a register, etc. This storage medium is located in the memory 110, and the processor 120 reads the information in the memory 110 and combines its hardware to complete the steps of the above method. To avoid repetition, it will not be described in detail here.

[0161] In some implementations, in addition to the hardware units described above, a computer device may further include software modules. Among them, software modules may be, for example, an operating system, a Basic Input Output System (BIOS), application software, etc.

[0162] The operating system is used to manage the hardware and / or software resources of a computer device and is the core and foundation of the computer device. The operating system needs to handle basic tasks such as managing and configuring memory, determining the priority order of system resource supply and demand, controlling input and output devices, operating the network, and managing the file system. To facilitate user operation, most operating systems provide an operation interface for users to interact with the system.

[0163] The BIOS is used to run hardware initialization during the power-on boot phase and provide runtime services for the operating system and application programs. In some implementations, the BIOS can also monitor the temperature of the display processor and perform functions such as adjusting the temperature protection strategy.

[0164] Application software, also known as an application program, can be understood as software written for a specific application purpose of users and is one of the main classifications of computer software. For example, application software can be a program for achieving purposes such as power control and temperature management.

[0165] It should be noted that although some embodiments of this application take a mobile robot as an example, this application can be applied to other self-mobile devices, such as AGVs, drones, etc., and this application does not limit this.

[0166] It can be understood that the specific examples in this specification are only to help those skilled in the art better understand the implementation manners of this application, rather than limiting the protection scope of this application.

[0167] It can be understood that in various implementation manners of this specification, the magnitudes of the sequence numbers of each process do not mean the sequence of execution. The execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of this application.

[0168] It can be understood that the various implementation manners described in this specification can be implemented alone or in combination, and this application does not limit this.

[0169] Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by those skilled in the technical field of this specification. The terms used in this specification are only for the purpose of describing specific embodiments and are not intended to limit the scope of this specification. The term "and / or" used in this specification includes any and all combinations of one or more of the related listed items. The singular forms "a", "above-mentioned", and "the" used in this specification and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise.

[0170] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this specification.

[0171] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the above-described embodiments can refer to the corresponding processes in other embodiments and will not be elaborated herein.

[0172] In several embodiments provided in this specification, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of units is only a logical function division. In actual implementation, there may be other division methods. For 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 couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of devices or units can be in electrical, mechanical, or other forms.

[0173] 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 distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the technical solution of this application.

[0174] In addition, the functional units in each embodiment of this specification can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit.

[0175] If a function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this specification, in essence, or the part that contributes to the prior art or the technical solution can be embodied in the form of a software product. The 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 this specification. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs that can store program codes.

[0176] The above are only specific embodiments of this specification, but the protection scope of this application is not limited thereto. Any person skilled in the art in the technical field disclosed in this specification can easily think of changes or substitutions, which should all be covered by the protection scope of this specification. Therefore, the protection scope of this application should be subject to the protection scope of the claims.

Claims

1. A motion control method for a multi-rigid-body robot, characterized in that: Applied to a whole-body motion control framework, the whole-body motion control framework includes a quadratic solver, the method includes: Determine an optimization variable group and a control equation group according to the received planning target and the kinematic information and task information of the multi-rigid body robot; the kinematic information is used to indicate a kinematic model and a plurality of task coordinate systems, the task information is used to define a plurality of task types and their corresponding task coordinate systems and task equations, the control equations in the control equation group are constructed based on the task equations, each control equation corresponds to a combination of a task coordinate system and a task type, and the task equation is a target equation or a constraint equation; Calculating the vectors and matrices of the control equations using the state information of the multi-rigid-body robot and the kinematic model; Based on the configuration information, vectors and matrices of the control equation group, using the quadratic solver to calculate the optimal solution of the optimization variable group; According to the optimal solution of the optimization variable group, corresponding motion control instructions are generated to perform motion control on the multi-rigid body robot.

2. The motion control method of a multi-rigid-body robot according to claim 1, characterized in that: The method of calculating the optimal solution of the optimization variable group using the quadratic solver based on the configuration information, vectors and matrices of the control equation group includes: According to the configuration information, vectors and matrices of the control equation group, using the quadratic solver to perform quadratic optimization calculation on each layer of the control equation group, to obtain the corresponding intermediate optimization results of each layer, wherein the intermediate optimization results include one or more of a null space matrix, a Hesser matrix, a gradient vector, an inequality matrix, and upper and lower bounds; The optimal solution of the optimization variable group is calculated using the corresponding intermediate optimization results of each layer.

3. The motion control method of a multi-rigid-body robot according to claim 1, characterized in that: The method further comprises: Constructing an optimization equation group corresponding to the multi-rigid body robot, the optimization equation group includes the control equation group, the optimization equations in the optimization equation group are constructed based on the task equations, and each optimization equation corresponds to a combination of a task coordinate system and a task type; The activation state, priority and weight of each optimization equation are configured; wherein the activation state of each control equation is configured as activated, and the configuration information includes the activation state, priority and weight.

4. The motion control method of a multi-rigid-body robot according to claim 3, characterized in that: The constructing of the optimization equation group corresponding to the multi-rigid body robot comprises: For each task type and for each task coordinate system corresponding to the task type, an optimization equation corresponding to the task coordinate system is constructed based on the task equation.

5. The motion control method of a multi-rigid-body robot according to claim 1, characterized in that: The step of determining the optimization variable group and the control equation group according to the received planning target and the kinematic information and task information of the multi-rigid-body robot comprises: Determine the corresponding target control state according to the received planning target; Based on the kinematic information and task information of the multi-rigid-body robot, an optimization variable group and a control equation group corresponding to the target control state are determined.

6. The motion control method of a multi-rigid-body robot according to claim 1, characterized in that: The step of calculating the vectors and matrices of the control equations by using the state information of the multi-rigid-body robot and the kinematic model comprises: Based on the state information of the multi-rigid-body robot and the kinematic model, calculating the kinematic state information of each rigid body; For each task coordinate system, the kinematic state information of the corresponding rigid body and the expected task coordinate system information are used to calculate the vector and matrix of the control equation corresponding to the task coordinate system.

7. The motion control method of a multi-rigid-body robot according to claim 1, characterized in that: The motion control instruction is a torque instruction, the optimal solution of the optimization variable group includes the optimal generalized joint acceleration and the optimal constraint force, and the corresponding motion control instruction is generated according to the optimal solution of the optimization variable group, including: Substituting the optimal generalized joint acceleration and the optimal constraint force into a specified inverse dynamics calculation formula to obtain joint control torque information; When it is detected that the joint control torque information satisfies the corresponding torque restriction condition, a corresponding torque instruction is generated based on the joint control torque information.

8. The motion control method of a multi-rigid-body robot according to claim 1, characterized in that: The motion control instruction is a position instruction, and the generating of the corresponding motion control instruction according to the optimal solution of the optimization variable group includes: When the error between the generalized joint coordinates of the multi-rigid body robot and the expected generalized joint coordinates is greater than a preset error, after performing hierarchical quadratic optimization, the generalized joint coordinate increments are calculated and added to the generalized joint coordinates until the error between the generalized joint coordinates and the expected generalized joint coordinates is not greater than the preset error or the number of iterations is greater than a preset number; When it is detected that the generalized joint coordinates satisfy the specified position restriction conditions, a corresponding position instruction is generated based on the generalized joint coordinates.

9. A multi-rigid body robot, characterized in that: The method comprises a control module, a motor and a motor driving module, wherein the control module is used to execute the method according to any one of claims 1 to 8.

10. The multi-rigid body robot according to claim 9, characterized in that: The multi-rigid-body robot is a humanoid robot, and the corresponding multiple task coordinate systems of the multi-rigid-body robot respectively correspond to the body, left foot plane, right foot plane, left hand end and right hand end of the multi-rigid-body robot.

11. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

12. A computer device, characterized in that: The computer device comprises a memory and a processor, the memory stores a computer program, and the processor implements the method according to any one of claims 1 to 8 when executing the computer program.

Images