A method for cooperative control of a humanoid robot multi-module

Abstract

The application discloses a kind of collaborative control methods of humanoid robot multi-module, belong to humanoid robot motion control technical field.The method gathers each module state data, constructs floating base whole body dynamics model and identifies module between dynamic coupling tensor on line;Establishes from foot bottom contact stability, torso posture, double-arm end position and posture synchronization, double-arm cooperative contact force to joint space regularization's hierarchical priority task error, obtains desired task space acceleration by admittance control;Dynamic coupling tensor is converted into feedforward compensation torque injection model, constructs hierarchical quadratic programming according to priority order and fuses dynamic coupling feedforward compensation;Solve using alternate direction multiplier method, inverse solve joint driving torque instruction and issue execution.The application realizes dynamic coupling structured feedforward compensation between modules and multi-task hierarchical constraint optimization in the same framework, significantly reduces end contact force oscillation, improves torso posture stability, strictly meets foot bottom contact friction cone constraint.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of humanoid robot motion control, and in particular to a kind of humanoid robot multi-module cooperative control method. BACKGROUND

[0002] Humanoid robot has double legs, arms, waist, head and other multiple motion modules. The existing humanoid robot cooperative control method is mainly divided into three categories.

[0003] The first category is an independent module control method, which regards each module as an independent mechanical arm or leg, respectively designs a joint space position controller and a force controller, and gives the desired trajectory of each module through an upper motion planner. This method is simple in structure, but ignores the dynamic coupling between the trunk and each module. When the humanoid robot performs a task of simultaneous double-arm cooperative handling and double-leg dynamic walking, the acceleration motion of the trunk will be transmitted to the end effector through inertia coupling, resulting in a large amplitude oscillation of the contact force of the end effector; The motion of the arms will also change the overall center of mass distribution, causing the distribution of the double-leg foot contact force to deviate from the desired value.

[0004] The second category is a whole body task space control method, which describes the tasks of each module in the task space, and uses zero space projection to realize the priority allocation of multiple tasks. This method can handle task-level redundancy, but the dynamic model used by this method usually estimates or compensates the coupling term as a whole disturbance online, without structurally modeling and feeding forward compensating the dynamic coupling between modules. The compensation lag is obvious in high dynamic tasks.

[0005] The third category is a whole body model predictive control method, which directly solves the joint torque and contact force using finite time domain optimization, can explicitly handle constraints, but the optimization problem has high dimension, and the online solution takes a lot of time, making it difficult to run at a frequency higher than 100 Hz on embedded hardware. Moreover, the handling of dynamic coupling terms is highly dependent on model accuracy, and model errors will lead to a decrease in control performance. The above methods do not simultaneously realize the structuralized feedforward compensation of dynamic coupling between modules and the hierarchical constraint optimization of multiple tasks in the same control framework, resulting in difficulty in balancing the end force control accuracy, trunk posture stability and contact constraint satisfaction when the humanoid robot operates cooperatively.

[0006] Therefore, a humanoid robot multi-module cooperative control method is needed that can explicitly compensate for the inertia coupling between modules in high dynamic conditions while strictly meeting the contact constraints and task priority. SUMMARY

[0007] The technical problem to be solved by the present application is that the existing humanoid robot multi-module cooperative control method is difficult to compensate for the dynamic coupling between modules and meet the hierarchical constraints of multiple tasks at the same time, resulting in low end force control accuracy and poor posture stability during cooperative operation.

[0008] This invention provides a collaborative control method for multiple modules of a humanoid robot, comprising the following steps:

[0009] Step 1: Collect the status data of each module of the humanoid robot. The status data includes the joint angle, joint angular velocity and joint angular acceleration of all rotating joints, the six-dimensional contact force and contact torque of the end effectors of the two arms, the contact force and contact torque of the soles of the two feet, the torso posture quaternion, the torso three-axis angular velocity and the torso three-axis acceleration.

[0010] Step 2: Construct a full-body dynamic model of the humanoid robot floating base based on the state data, and identify the dynamic coupling tensor between modules online;

[0011] Step 3: Establish the hierarchical priority and task error of the multi-module collaborative task. The hierarchical priority is from high to low as follows: foot contact stability task, trunk posture task, double arm end pose synchronization task, double arm collaborative contact force task and joint space regularization task. Define the task function, expected value and task Jacobian matrix for each priority task. Obtain the expected task space acceleration of each priority task through admittance control.

[0012] Step 4: Convert the dynamic coupling tensor between the modules into feedforward compensation torque, inject it into the whole-body dynamic model of the floating base, and construct the hierarchical quadratic programming problem of fusion dynamic coupling feedforward compensation in sequence according to the hierarchical priority order.

[0013] Step 5: Solve the hierarchical quadratic programming problem using the alternating direction multiplier method to obtain the globally optimal joint acceleration and optimal contact force vector, and then solve for the joint driving torque command.

[0014] Step 6: Send the joint drive torque command and the optimal contact force vector to each module of the humanoid robot for execution.

[0015] The beneficial effects of this invention are:

[0016] 1. This invention identifies the dynamic coupling tensor between modules online and transforms it into a feedforward compensation torque injected into the whole-body dynamic model of the floating base. This provides structured feedforward compensation for the inertial coupling between modules, significantly reducing the oscillation amplitude of the contact force of the dual-arm end effector and improving the end force control accuracy during multi-module collaborative operation.

[0017] 2. This invention establishes hierarchical priority tasks based on plantar contact stability, trunk posture, end-arm pose synchronization, bi-arm cooperative contact force, and joint space regularization, and uses admittance control to generate desired task space acceleration. When bi-arm cooperative transport and bi-leg dynamic walking are performed simultaneously, the root mean square value of trunk posture angle deviation is significantly reduced, thereby improving trunk posture stability accuracy.

[0018] 3. This invention incorporates the linearized polyhedral friction cone constraint and joint moment boundary constraint into a hierarchical quadratic programming method and uses the alternating direction multiplier method to solve it, strictly satisfying the foot contact friction cone constraint, avoiding the foot contact force from violating the friction cone constraint, and ensuring the contact stability of the humanoid robot during dynamic walking. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in this invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, those skilled in the art can obtain other drawings based on these drawings without creative effort.

[0020] Figure 1 The flowchart illustrates the overall process of the collaborative control method for multiple modules of a humanoid robot provided in this embodiment of the invention. Detailed Implementation

[0021] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. It should also be noted that, to make the embodiments more comprehensive, the following embodiments are the best and preferred embodiments, and those skilled in the art can use other alternative methods to implement some well-known technologies; moreover, the accompanying drawings are only for more specific description of the embodiments and are not intended to specifically limit the present invention.

[0022] This invention provides a collaborative control method for multiple modules of a humanoid robot. Within the same control framework, it achieves online identification and feedforward compensation of the dynamic coupling tensor between modules, multi-task hierarchical priority constraint optimization, and satisfaction of contact force friction cone constraints, enabling stable and synchronized operation of the two arms and dynamic walking of the two legs. Each module of the humanoid robot includes two legs, two arms, a torso, and a head. Each rotational joint of the legs and arms is equipped with a joint encoder, the wrists of the arms are equipped with six-dimensional force sensors, each foot is equipped with an array of four one-dimensional pressure sensors, and the torso is equipped with an inertial measurement unit. This method identifies the dynamic coupling tensor between modules online using a block-based recursive least squares method, transforms the dynamic coupling tensor into a feedforward compensation torque, and injects it into the floating-base whole-body dynamic model. Then, it constructs and solves a quadratic programming problem with friction cone constraints and torque boundary constraints according to hierarchical priority, ultimately obtaining the joint driving torque command and the optimal contact force vector.

[0023] The following detailed description is provided through examples:

[0024] This embodiment uses a humanoid robot with 20 rotational joints as an example. The humanoid robot has 6 rotational joints in each leg, 4 rotational joints in each arm, 2 rotational joints in the waist, and 2 rotational joints in the head. The humanoid robot performs the task of carrying a 2 kg object with both arms in a coordinated manner and walking forward on a horizontal surface at a walking speed of 0.3 m / s.

[0025] Step 1: Collect status data of each module of the humanoid robot

[0026] Joint angle vector and joint angular velocity vector Data is acquired by 20 joint encoders with a sampling period of 1 millisecond, and then processed by a first-order low-pass filter with a time constant of 5 milliseconds. Joint angular acceleration vector. It is obtained by differentiating the joint angular velocity vector and passing it through a first-order low-pass filter with a cutoff frequency of 50 Hz, with a difference step size of 1 millisecond.

[0027] The six-dimensional contact force and contact torque of the dual-arm end effector are collected by two six-dimensional force sensors installed on the left and right wrists, with a sampling period of 1 millisecond. Each six-dimensional force sensor outputs 3 force components and 3 torque components.

[0028] The contact force and torque of both feet were collected by an array consisting of four one-dimensional pressure sensors in the left foot and four in the right foot, with a sampling period of 1 millisecond. Each one-dimensional pressure sensor outputs a pressure value perpendicular to the foot's direction. The four pressure values ​​from each foot are combined according to the geometric position of the sensors on the foot to form a resultant force vector and a resultant torque vector. Let the position coordinates of the four pressure sensors in the left foot be... , , , The position coordinates of the four pressure sensors on the right foot are symmetrical to those on the left foot. The magnitude of the resultant force on the sole of the foot is the sum of the four pressure values, and the resultant torque on the sole of the foot is the sum of the cross products of each pressure value and its corresponding position coordinate.

[0029] Trunk posture quaternion Angular velocity of the torso along three axes acceleration along the three axes of the torso Data was acquired by an inertial measurement unit (IMU) installed at the torso's center of mass, with a sampling period of 1 millisecond. The IMU's accelerometer range was ±16g, and the gyroscope range was ±2000 degrees per second. The torso attitude quaternion was obtained through extended Kalman filtering (EPK). The EPK state vector consisted of the torso attitude quaternion and the gyroscope zero bias, while the observation vectors were the accelerometer output and the magnetometer output. The EPK process noise covariance matrix had diagonal elements of 0.5. The diagonal matrix, the observation noise covariance matrix, has diagonal elements that are all... A diagonal matrix.

[0030] All collected data underwent noise reduction processing using a sliding window with a mean of zero and a sliding window length of 10 sampling points.

[0031] Step 2: Construct a full-body dynamic model of the humanoid robot's floating base and identify the dynamic coupling tensors between modules online.

[0032] The floating-base whole-body dynamics model treats the humanoid robot's torso as a 6-DOF floating base, with the rotational joints of the legs and arms described by joint angle vectors. Based on the joint angle vectors acquired in step 1... Joint angular velocity vector With joint angular acceleration vector Construct a full-body dynamic model of the floating base:

[0033]

[0034] Among them, the mass matrix The Coriolis force and centrifugal force matrices are calculated using the inertial parameters and joint angles of each link in the humanoid robot. The gravity term is calculated from the partial derivatives of the mass matrix with respect to the joint angles. The joint driving torque vector is calculated from the mass and center of mass of each link. , No. Jacobian matrix of contact points Map the joint angular velocity to the first The spatial velocity at the first contact point, the... The generalized contact force vector at each contact point It contains 3 force components and 3 torque components, and the number of contact points... These correspond to the soles of the left and right feet, the contact point between the left end effector and the object, and the contact point between the right end effector and the object, respectively.

[0035] Define the nominal mass matrix Nominal Coriolis force and centrifugal force matrix and nominal gravity terms The nominal mass matrix is ​​calculated from the nominal inertial parameters of each link of the humanoid robot, and the deviation between the nominal and actual inertial parameters does not exceed 5% of the actual inertial parameters. The nominal joint driving torque is calculated as follows:

[0036]

[0037] The residual between the actual joint driving torque and the nominal joint driving torque It includes dynamic coupling terms between modules and slowly varying disturbance terms. The residual parameterization is as follows:

[0038]

[0039] Among them, the dynamic coupling tensor between modules A function of the joint angular velocity vector and the joint angle vector, a slowly varying disturbance vector. This represents slowly varying disturbances such as joint friction and unmodeled dynamics.

[0040] The block-based recursive least squares method is used to identify the dynamic coupling tensors between modules online. Each line. The first row of the dynamic coupling tensor between modules. Row parameterization into parameter vector Regression vector The value is the current joint angular velocity vector. . No. The next recurrence relation is:

[0041]

[0042]

[0043]

[0044] in, For the dynamic coupling tensor between modules, the first line in Parameter vector estimation after recursion. For residual vectors The The component in the first The value obtained in the next iteration, the forgetting factor The value is 0.98, representing the identity matrix. Initial covariance matrix The value is that all diagonal elements are Diagonal matrix, initial parameter vector estimation The value is a zero vector. After each recursion, the 20 parameter vector estimates are concatenated row by row to obtain the estimated value of the dynamic coupling tensor between modules.

[0045] Step 3: Establish hierarchical priorities and task errors for multi-module collaborative tasks.

[0046] The multi-module collaborative tasks are sorted by priority from high to low as follows: priority 1: foot contact stability task, priority 2: trunk posture task, priority 3: bilateral arm end-effector pose synchronization task, priority 4: bilateral arm collaborative contact force task, and priority 5: joint space regularization task.

[0047] The error for the first priority plantar contact stability task is:

[0048]

[0049] in, The generalized force vector is formed by splicing the generalized force vector of the left foot and the generalized force vector of the right foot. Each generalized force vector of the foot contains 3 force components and 3 torque components, which are obtained from the resultant force and resultant torque of the foot collected in step 1. The desired generalized force vector of the foot is obtained from the center-of-mass momentum programming. The center-of-mass momentum programming calculates the desired generalized force vector of the foot based on the desired center-of-mass trajectory, the desired rate of change of angular momentum, and the phase of the two-foot support. The desired center-of-mass trajectory is a periodic trajectory of walking forward at a speed of 0.3 meters per second.

[0050] The error for the second priority trunk posture task consists of the vector part of the trunk posture quaternion error:

[0051]

[0052] in, The quaternion represents the torso posture data collected in step 1. Let the desired torso posture quaternion be a constant quaternion that keeps the torso vertical and forward. To represent quaternion multiplication, This indicates taking the vector part of the quaternion; the vector part of the quaternion error is a 3-dimensional vector.

[0053] The error for the third priority dual-arm end-effector pose synchronization task is:

[0054]

[0055] in, This represents the actual position of the left end effector. The desired position of the left end effector. This is the actual attitude rotation matrix of the left end effector. Let be the rotation matrix of the desired attitude of the left end effector. This represents the actual position of the right-end actuator. The desired position of the right-end actuator. This is the actual attitude rotation matrix of the right end effector. Let the desired attitude rotation matrix of the right end effector be denoted as . This is a logarithmic mapping of the rotation matrix. The desired positions of the left and right end effectors are calculated based on the desired trajectory of the object and the relative clamping positions of the end effectors to the object.

[0056] The error for the fourth priority dual-arm cooperative contact force task is:

[0057]

[0058] in, This represents the six-dimensional contact force and contact torque vector of the left end effector. This represents the desired six-dimensional contact force and contact torque vector for the left-end actuator. This represents the six-dimensional contact force and contact torque vector of the right-end actuator. The expected six-dimensional contact force and contact torque vectors of the right end effector, the expected six-dimensional contact force and contact torque vectors of the left end effector and the expected six-dimensional contact force and contact torque vectors of the right end effector are calculated based on the object weight and the dual-arm clamping force distribution strategy.

[0059] The error for the 5th priority joint space regularization task is:

[0060]

[0061] in, The preset natural posture joint angle vector is set to the midpoint angle of each rotation joint when the humanoid robot is standing.

[0062] Define a task function for each priority task. Expected value And the task Jacobian matrix ,in . No. The desired task space acceleration for priority tasks is obtained through admittance control:

[0063]

[0064] in, For the first The proportional gain matrix of priority tasks, For the first The differential gain matrix for the priority task. In this embodiment, the proportional gain matrix for the plantar contact stability task. The diagonal elements are 200, and the differential gain matrix is... The diagonal elements are 40; the scaling gain matrix for torso pose task. The diagonal elements are all 100, and the differential gain matrix is... The diagonal elements are 25; the proportional gain matrix for dual-arm end-effector pose synchronization task. The diagonal elements are 150, and the differential gain matrix... The diagonal element is 30; the proportional gain matrix for the dual-arm cooperative contact force task. The diagonal elements are 50, and the differential gain matrix is... The diagonal elements are 10; the scaling gain matrix for the joint space regularization task. The diagonal elements are 20, and the differential gain matrix is... The diagonal element has a value of 8.

[0065] Step 4: Construct a hierarchical quadratic programming problem that integrates dynamic coupling feedforward compensation.

[0066] The inter-module dynamic coupling tensor identified in step 2 Converted into feedforward compensation torque:

[0067]

[0068] The feedforward compensation torque is injected into the overall dynamic model of the floating base, and the result is corrected as follows:

[0069]

[0070] According to the hierarchical priority order, for the first Priority tasks Solve the following quadratic programming problem:

[0071]

[0072] The following constraints must be met:

[0073]

[0074]

[0075]

[0076]

[0077] Among them, the Friction cone constraint set at each contact point Specifically, a linearized polyhedral friction cone is used. The tangential contact force component at each foot contact point is constrained within a linearized polyhedron with the normal contact force component as its axis. The friction coefficient of the linearized polyhedral friction cone is 0.8, and the number of sides of the polyhedron is 6. For the contact point between the end effector and the object, the friction cone constraint also uses a linearized polyhedral friction cone, with a friction coefficient of 0.8 and 6 sides. (Lower bound vector of joint driving torque) and the upper bound vector of joint driving torque The lower bound of the driving torque for each rotary joint is determined based on the rated output torque of each joint actuator. The upper bound of the driving torque is taken as: . For the first The optimal joint acceleration vector obtained from the priority task solution is used to solve for the 1st to 5th priority tasks sequentially to obtain the globally optimal joint acceleration. and optimal contact force vector The optimal joint acceleration obtained from solving the high-priority task is introduced into the quadratic programming of the next-priority task through equality constraints, ensuring that the low-priority task does not affect the execution result of the high-priority task.

[0078] Step 5: Solve the hierarchical quadratic programming problem using the alternating direction multiplier method to obtain the joint driving torque command and contact force command.

[0079] The friction cone constraint and joint moment boundary constraint in each priority quadratic programming problem are addressed by introducing auxiliary variables. and indicator functions Processing and constructing the augmented Lagrangian function:

[0080]

[0081] Among them, the original variables It contains the global joint acceleration vector and the generalized contact force vector at four contact points, with dimensions of [dimension number missing]. Positive definite matrix From the task Jacobian matrix Weighted composition, specifically .vector It consists of a task error term and a dynamically coupled feedforward compensation term, specifically as follows: Mapping matrix Original variables Variables related to friction cone constraints and joint moment boundary constraints are mapped to auxiliary variables. Indicator functions Defined as: when the auxiliary variable When the linearized polyhedral friction cone constraint and joint moment boundary constraint are satisfied, the indicator function takes the value of 0; when the auxiliary variable When the linearized polyhedral friction cone constraint or joint moment boundary constraint is not satisfied, the indicator function takes the value of Dual variables Dimensions and auxiliary variables Same, penalty parameter The value is 10.

[0082] The iterative process of the alternating direction multiplier method is as follows: Step 1, fix the auxiliary variable. and dual variables Minimize the augmented Lagrange function with respect to the original variables The first step is to determine the value of the variable and obtain the updated original variable; the second step is to fix the updated original variable. and dual variables Minimize the augmented Lagrange function with respect to the auxiliary variable The value of is used to obtain the updated auxiliary variable. This step is equivalent to changing the mapping matrix. The result multiplied by the updated original variables is projected onto the intersection of the linearized polyhedral friction cone constraint set and the joint moment boundary constraint set; Step 3, fix the updated original variables. and updated auxiliary variables Update the dual variable according to the dual ascent rule. The stopping condition for the alternating direction multiplier method iteration is: the original residual With dual residual All less than ,in These are the auxiliary variables updated in this iteration. This is an auxiliary variable from the previous iteration. The maximum number of iterations is set to 200.

[0083] Obtain the global optimal joint acceleration and optimal contact force vector Then, resolve the joint drive torque command:

[0084]

[0085] in, This is the joint driving torque command vector. For the first The optimal contact force vector at each contact point.

[0086] Step 6: Send the joint drive torque command and contact force command to each module for execution.

[0087] The joint driving torque command obtained in step 5 The optimal contact force vector is transmitted to the 20 joint actuators of the humanoid robot via the CAN bus at 1-millisecond intervals. The commands are sent to the force control modules of the dual-arm end effectors and the dual-foot plantar force control modules. The left end effector force control module receives the optimal contact force vector at the contact point between the left end effector and the object; the right end effector force control module receives the optimal contact force vector at the contact point between the right end effector and the object; the left foot plantar force control module receives the optimal contact force vector at the contact point between the left foot and the right foot; and the right foot plantar force control module receives the optimal contact force vector at the contact point between the right foot and the object. Each module synchronously executes coordinated actions according to the received commands, completing the task of collaboratively moving an object with both arms and walking synchronously.

[0088] To verify the effectiveness of this embodiment, a comparative example was set up. The comparative example employs a full-body task space control method from the prior art, uniformly describing the tasks of each module in the task space and using null-space projection to allocate task priorities, but without performing structured modeling and feedforward compensation for the dynamic coupling between modules. The proportional gain matrix and differential gain matrix of the task space control in the comparative example have the same values ​​as the gain matrices of the 2nd to 4th priority tasks in this embodiment. When the humanoid robot performs the same dual-arm collaborative object handling and synchronous walking task, the comparative example records the oscillation amplitude of the contact force of the end effectors of the two arms, the root mean square value of the torso posture angle deviation, and the number of times the foot contact force violates the friction cone constraint. The test lasts for 30 seconds, with the first 5 seconds being the starting phase and the following 25 seconds being the stable walking phase. The results are shown in Table 1.

[0089] Table 1 Performance comparison between this embodiment and the comparative example.

[0090] Left end effector contact force oscillation amplitude 2.1N 8.7N Right end effector contact force oscillation amplitude 2.3N 9.2N Trunk posture angle deviation root mean square value 0.8 degrees 3.5 degrees Foot contact force violation of friction cone constraint times 0 times 12 times

[0091] In Table 1, the oscillation amplitudes of the left and right end effector contact forces are the maximum absolute values ​​of the differences between the peak and average values ​​of the left and right end effector contact force signals during the stable walking phase, respectively. The root mean square value of the torso posture angle deviation is the root mean square value of the error angle between the actual and desired torso posture during the stable walking phase. The number of times the plantar contact force violates the friction cone constraint is the sum of the number of sampling points where the generalized contact force vectors of the left and right feet fall outside the linearized polyhedral friction cone during the stable walking phase. As shown in Table 1, in this embodiment, the oscillation amplitude of the left end effector contact force is reduced by 75.9% compared to the comparative example, the oscillation amplitude of the right end effector contact force is reduced by 75.0% compared to the comparative example, the root mean square value of the torso posture angle deviation is reduced by 77.1% compared to the comparative example, and the number of times the plantar contact force violates the friction cone constraint is reduced from 12 times in the comparative example to 0 times. The above results show that this embodiment significantly suppresses end effector contact force oscillation by identifying the dynamic coupling tensor between modules online and performing feedforward compensation. At the same time, hierarchical quadratic programming strictly ensures that the plantar contact force meets the friction cone constraint, thereby improving the stability of the torso posture.

[0092] This invention encompasses any substitutions, modifications, equivalent methods, and solutions made within the spirit and scope of this invention. To provide the public with a thorough understanding of this invention, specific details are described in detail in the following preferred embodiments; however, those skilled in the art will fully understand the invention even without these details. Furthermore, to avoid unnecessary misunderstanding of the essence of this invention, well-known methods, processes, procedures, components, and circuits are not described in detail.

[0093] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A collaborative control method for multiple modules of a humanoid robot, characterized in that, Includes the following steps: Step 1: Collect the status data of each module of the humanoid robot. The status data includes the joint angle, joint angular velocity and joint angular acceleration of all rotating joints, the six-dimensional contact force and contact torque of the end effectors of the two arms, the contact force and contact torque of the soles of the two feet, the torso posture quaternion, the torso three-axis angular velocity and the torso three-axis acceleration. Step 2: Construct a full-body dynamic model of the humanoid robot floating base based on the state data, and identify the dynamic coupling tensor between modules online; Step 3: Establish the hierarchical priority and task error of the multi-module collaborative task. The hierarchical priority is from high to low as follows: foot contact stability task, trunk posture task, double arm end pose synchronization task, double arm collaborative contact force task and joint space regularization task. Define the task function, expected value and task Jacobian matrix for each priority task. Obtain the expected task space acceleration of each priority task through admittance control. Step 4: Convert the dynamic coupling tensor between the modules into feedforward compensation torque, inject it into the whole-body dynamic model of the floating base, and construct the hierarchical quadratic programming problem of fusion dynamic coupling feedforward compensation in sequence according to the hierarchical priority order. Step 5: Solve the hierarchical quadratic programming problem using the alternating direction multiplier method to obtain the globally optimal joint acceleration and optimal contact force vector, and then solve for the joint driving torque command. Step 6: Send the joint drive torque command and the optimal contact force vector to each module of the humanoid robot for execution.

2. The collaborative control method for multiple modules of a humanoid robot according to claim 1, characterized in that, The dynamic coupling tensor between online identification modules mentioned in step 2 is specifically based on the acquired joint angle vectors. Joint angular velocity vector With joint angular acceleration vector Constructing a full-body dynamic model of a humanoid robot floating base: in, For the quality matrix, The matrix represents the Coriolis force and the centrifugal force. For gravity, This is the joint driving torque vector. For the first Jacobian matrix of contact points For the first The generalized contact force vector at each contact point This refers to the number of contact points; Define the nominal mass matrix Nominal Coriolis force and centrifugal force matrix and nominal gravity terms Calculate the nominal joint driving torque: The residual between the actual joint driving torque and the nominal joint driving torque Parameterized as: in, For slowly varying perturbation vectors, a block-based recursive least squares method is used to identify the dynamic coupling tensors between modules online. Each line, the first The next recurrence relation is: in, For the dynamic coupling tensor between modules, the first line in Parameter vector estimation after recursion. It consists of the components of the joint angular velocity vector. For residual vectors The The component in the first The value obtained in the next iteration Forgetting factor, It is an identity matrix.

3. The collaborative control method for multiple modules of a humanoid robot according to claim 1, characterized in that, The error in the foot contact stability task described in step 3 is: in, This is the generalized force vector composed of the collected contact forces and contact torques on both feet. The desired generalized force vector of the foot is obtained from the center-of-mass momentum programming. The error of the torso posture task consists of the vector part of the torso posture quaternion error: in, For the collected torso posture quaternions. For the desired trunk posture quaternion, To represent quaternion multiplication, This represents the vector part of a quaternion; The error of the dual-arm end-effector pose synchronization task is composed of the difference between the actual pose and the desired pose of the left end-effector, and the difference between the actual pose and the desired pose of the right end-effector; the error of the dual-arm cooperative contact force task is composed of the difference between the contact force of the left end-effector and the desired contact force, and the difference between the contact force of the right end-effector and the desired contact force; the error of the joint space regularization task is the current joint angle vector. With respect to the preset natural posture joint angle vector The difference; No. The desired task space acceleration for priority tasks is obtained through admittance control: in, For the first The task function for priority tasks. For the first Expected value of priority tasks For the first The proportional gain matrix of priority tasks, For the first Differential gain matrix of priority task, .

4. The collaborative control method for multiple modules of a humanoid robot according to claim 2, characterized in that, Step 4 specifically involves: processing the identified inter-module dynamic coupling tensor... Converted into feedforward compensation torque: The feedforward compensation torque is injected into the overall dynamic model of the floating base, and the result is modified as follows: According to the aforementioned hierarchical priority order, for the first Priority tasks Solve the following quadratic programming problem: The following constraints must be met: in, For the first A set of friction cone constraints at each contact point. This is the lower bound vector of the joint driving torque. This is the upper bound vector of the joint driving torque. For the first The optimal joint acceleration vector obtained from the priority task solution is used to solve for the 1st to 5th priority tasks sequentially to obtain the globally optimal joint acceleration. and optimal contact force vector .

5. The collaborative control method for multiple modules of a humanoid robot according to claim 4, characterized in that, Step 5, which uses the alternating direction multiplier method, specifically involves introducing auxiliary variables to solve the friction cone constraint and joint moment boundary constraint in each priority quadratic programming problem. and indicator functions Processing and constructing the augmented Lagrangian function: in, , Let be a positive definite matrix formed by weighting the task Jacobian matrices. It consists of a task error term and a dynamically coupled feedforward compensation term. To map inequality constraint variables to auxiliary variables The mapping matrix, This is an indicator function used to enforce friction cone constraints and joint moment boundary constraints. As dual variables, For penalty parameters, penalty parameters The value is 10; Alternately update the original variables Auxiliary variables and dual variables until both the original residual and the dual residual are less than The globally optimal joint acceleration is obtained. and optimal contact force vector Then, resolve the joint drive torque command: in, This is the joint driving torque command vector. For the first The optimal contact force vector at each contact point.

6. The collaborative control method for multiple modules of a humanoid robot according to claim 1, characterized in that, In step 1, joint angles and joint angular velocities are acquired by a joint encoder with a sampling period of 1 millisecond and processed by a first-order low-pass filter; joint angular acceleration is obtained by differential joint angular velocity and processed by a first-order low-pass filter with a cutoff frequency of 50 Hz; the six-dimensional contact force and contact torque of the dual-arm end effector are acquired by a six-dimensional force sensor installed on the wrist with a sampling period of 1 millisecond; the plantar contact force and contact torque of both feet are acquired by an array of four one-dimensional pressure sensors in each foot and converted into a resultant force and resultant torque on the plantar surface with a sampling period of 1 millisecond; the torso attitude quaternion, torso triaxial angular velocity, and torso triaxial acceleration are acquired by an inertial measurement unit installed on the torso with a sampling period of 1 millisecond and processed by extended Kalman filter attitude calculation; all acquired data are denoised by a sliding window with a mean of zero.

7. The collaborative control method for multiple modules of a humanoid robot according to claim 2, characterized in that, Forgetting factor in step 2 The value is 0.

98.

8. The collaborative control method for multiple modules of a humanoid robot according to claim 3, characterized in that, The proportional gain matrix of the foot contact stability task The diagonal elements range from 100 to 400, and the differential gain matrix... The diagonal elements range from 20 to 60; the scaling gain matrix for the torso posture task The diagonal elements range from 50 to 200, and the differential gain matrix... The diagonal elements range from 10 to 40; the proportional gain matrix of the dual-arm end-effector pose synchronization task. The diagonal elements range from 80 to 300, and the differential gain matrix... The diagonal elements range from 15 to 50; the proportional gain matrix of the dual-arm cooperative contact force task The diagonal elements range from 20 to 100, and the differential gain matrix... The diagonal elements range from 5 to 20; the proportional gain matrix of the joint space regularization task The diagonal elements range from 10 to 50, and the differential gain matrix... The diagonal elements range from 5 to 15.

9. A collaborative control method for multiple modules of a humanoid robot according to claim 4, characterized in that, The friction cone constraint set mentioned in step 4 Specifically, it is a linearized polyhedral friction cone, in which the tangential contact force component at each contact point is constrained within a linearized polyhedron with the normal contact force component as its axis. The friction coefficient of the linearized polyhedron ranges from 0.6 to 1.0, and the number of sides of the polyhedron ranges from 4 to 8.

10. The collaborative control method for multiple modules of a humanoid robot according to claim 1, characterized in that, Joint driving torque command in step 6 The optimal contact force vector is transmitted to the actuators of each joint of the humanoid robot via the CAN bus at 1-millisecond intervals. The commands are sent to the force control module of the dual-arm end effector and the force control module of the dual-foot plantar force control module, and each module executes coordinated actions synchronously according to the received commands.

Images