A method and system for adaptive force control operation of humanoid robot arms

Abstract

The present invention belongs to the field of humanoid robot control technology, and specifically relates to a humanoid robot dual-arm adaptive force control operation method and system, including: establishing a coordinate conversion algorithm between the world coordinate system and the left arm base coordinate system, the right arm base coordinate system, the left arm end coordinate system, the right arm end coordinate system, and the operation target coordinate system; obtaining six-dimensional force and torque data collected by six-dimensional force sensors at the ends of the left and right robotic arms, and performing gravity compensation and conversion; defining a grasping matrix to decompose the six-dimensional force and torque data into internal force and external force; dividing the collaborative task into a free motion phase, a constraint establishment phase, and a constrained motion phase, and generating corresponding control instructions for each phase; inputting the generated control instructions into the dual-arm robot system to achieve coordinated motion control of the dual-arm robot at different stages. The present invention can complete coordination work and has anti-interference capabilities.

Description

Technical Field

[0001] The present invention belongs to the technical field of humanoid robot control, and in particular relates to a method and system for adaptive force control operation of dual arms of a humanoid robot. Background Art

[0002] With the advancement of technology and the development of robotics, traditional single-arm operation modes are no longer able to meet the diverse and increasingly complex demands. To meet the challenges of increasingly complex and intelligent tasks, dual-arm robotic coordinated operation has become an indispensable solution. Compared to single-arm operation modes, dual-arm collaboration not only possesses stronger professional capabilities and can handle more complex operational tasks, but also has a greater payload capacity and a wider workspace. This dual-arm collaborative capability enables robots to perform delicate operations, multitask, and adapt to environmental conditions more efficiently, significantly enhancing their application potential in industrial automation, healthcare, service, and other fields.

[0003] Because dual-arm robots can be applied in diverse situations and handle complex tasks (such as moving objects, turning valves, and opening doors), their task planning and control methods have been extensively studied. When performing collaborative tasks, dual-arm robots typically need to simultaneously satisfy position constraints, force constraints, and reference trajectory constraints to achieve coordinated and interactive completion of specific complex tasks. Traditional position control is the primary control method for dual-arm coordinated control. This approach maintains specific position constraints during coordinated operation, enabling effective coordination and task completion between the two arms. However, it does not consider the contact forces between the two arms and the manipulated object during operation, nor does it account for external interference with the two arms or the manipulated object. Force-position coordinated control is crucial for the smooth execution of dual-arm coordinated tasks. Summary of the Invention

[0004] Purpose of the invention: The purpose of the present invention is to address the deficiencies in the prior art and provide a method and system for adaptive force control operation of a humanoid robot's dual arms, which can complete coordinated work and has the ability to resist interference.

[0005] Technical solution: The method for adaptive force control of dual arms of a humanoid robot according to the present invention comprises the following steps:

[0006] S1. According to the preset coordinate system relationship, establish a coordinate conversion algorithm between the world coordinate system and the left arm base coordinate system, the right arm base coordinate system, the left arm end coordinate system, the right arm end coordinate system and the operation target coordinate system;

[0007] S2. Acquire six-dimensional force data and torque data collected by six-dimensional force sensors at the ends of the left and right robotic arms, perform gravity compensation on the six-dimensional force data and torque data, and convert them into six-dimensional force data and torque data in the world coordinate system through the coordinate conversion algorithm;

[0008] S3. Define a grip matrix for the relative motion between the left and right robotic arm end effectors and the manipulated target, and use the grip matrix to decompose the six-dimensional force and torque data in the world coordinate system into internal forces and external forces;

[0009] S4. Divide the left and right robotic arm collaboration task into a free motion stage, a constraint establishment stage, and a constrained motion stage, and execute corresponding control strategies in each stage: in the free motion stage, input the difference between the actual force data and the expected force data into an adaptive variable admittance controller, and add the output result of the adaptive variable admittance controller to the expected trajectory to obtain the actual motion trajectory and generate corresponding control instructions; in the constraint establishment stage, estimate the environmental stiffness and position parameters based on the BFGS learning algorithm, obtain an adaptive reference trajectory based on the estimated environmental stiffness and position parameters, and generate corresponding control instructions; in the constrained motion stage, use inner and outer loop adaptive variable admittance controllers to control the internal and external forces of the left and right arms respectively, maintain the expected distance between the end position and the operation target, and perform closed-chain constraints to obtain the motion trajectories of the left and right robotic arms and generate corresponding control instructions;

[0010] S5. Input the generated control instructions into the dual-arm robot system to realize the coordinated motion control of the dual-arm robot at different stages.

[0011] To further improve the above technical solution, the conversion relationship between the left arm end coordinate system, the right arm end coordinate system and the operation target coordinate system is:

[0012]

[0013] in, is the rotation matrix between the right arm end coordinate system and the operation target coordinate system, is the offset vector of the right arm end coordinate system relative to the operation target coordinate system;

[0014]

[0015] in, is the rotation change matrix between the left arm end coordinate system and the operation target coordinate system, It is the offset vector of the left arm end coordinate system relative to the operation target coordinate system.

[0016] Furthermore, the six-dimensional force data and torque data are gravity compensated and converted into force data in the world coordinate system through the coordinate conversion algorithm. ;

[0017] The process of gravity compensation and conversion of the six-dimensional force data is as follows:

[0018]

[0019] in, The six-dimensional force data is read by the six-dimensional sensors on the left and right arms. is the attitude transformation matrix from the left and right arm end coordinate systems to the world coordinate system, I is the unit matrix, U and V are the installation inclination angles, g is the gravitational acceleration, is the zero force value of the six-dimensional force component, The value is or , and Representing the right arm and the left arm respectively;

[0020] The process of gravity compensation and conversion of the torque data is as follows:

[0021]

[0022] in, It is the torque data read by the six-dimensional sensors of the left and right arms. is the zero force value of the moment component, is the coordinate of the center of mass in the coordinate system of the left and right arm ends, is the attitude transformation matrix from the world coordinate system to the left and right arm end coordinate systems, g is the gravitational acceleration, The value is or , and Representing the right arm and left arm respectively.

[0023] Furthermore, the gripping matrix Defined as:

[0024]

[0025] in, is the input vector Cross product operator;

[0026] The overall grasping matrix of the coordinated operation of both arms is , used to convert force data Decomposed into internal forces and external forces :

[0027]

[0028]

[0029] in, and represent the grasp matrices of the left and right arms respectively, is the terminal six-dimensional force and torque data in the world coordinate system, is the pseudo-inverse of the grasping matrix.

[0030] Furthermore, during the free motion phase, the force data and expectation Subtract Input the adaptive variable admittance controller for control,

[0031] The control formula of the adaptive variable admittance controller is:

[0032]

[0033] in, is the quality coefficient matrix, is the damping coefficient matrix, is the stiffness coefficient matrix, is the position error, which represents the difference between the actual position and the expected position at time t. is the speed error, which represents the difference between the actual speed and the expected speed at time t. is the acceleration error, which represents the difference between the actual acceleration and the expected acceleration at time t. is the position error at time t+1, is the velocity error at time t+1, is the position acceleration error at time t+1, T is the time step;

[0034] ∆b ​​is the adaptive damping adjustment parameter, and its update rate is: , where , is the update rate, represents the adaptive parameter at the current time t, represents the adaptive parameters at time t+1;

[0035] The expected trajectory The output of the adaptive admittance controller Add them together to get the actual movement trajectory of the left and right arms .

[0036] Furthermore, force data is obtained , end position x and terminal speed , the BFGS learning algorithm is used to estimate the adaptive reference trajectory, including:

[0037] When force data Greater than the preset threshold , the following iterative steps are performed:

[0038] Calculate the search direction: , where Constructed as , is the environmental stiffness parameter, is the environment location parameter, , is the objective function Stiffness to environment The gradient of is used to guide the update of the environment stiffness;

[0039] Finding the appropriate step size using a linear search method , so that the objective function decrease rate satisfies the Wolfe condition:

[0040] ;

[0041] Update the environment stiffness: ;

[0042] Update environment location: ;

[0043] Calculate the position deviation: ;

[0044] Update the matrix by the following formula ,

[0045] ,

[0046] Where, and represent the increments of gradient and stiffness respectively;

[0047] Until the gradient Less than the preset convergence threshold , stop iteration;

[0048] The environmental stiffness estimated by the BFGS learning algorithm and environmental location , get the estimated adaptive trajectory .

[0049] Furthermore, the control strategy of the constraint motion stage includes: Convert to internal force and external forces and the internal force Bring it into the inner loop adaptive variable admittance controller, the external force Bring it into the outer loop adaptive variable admittance controller;

[0050] The outer loop adaptive variable admittance controller is:

[0051]

[0052] Where, is the outer loop acceleration error, is the quality coefficient matrix, is the external force, is the damping coefficient matrix, ∆b is the adaptive damping adjustment parameter, Outer loop speed error, is the outer ring displacement error, is the stiffness coefficient matrix, 、 、 They are respectively the outer loop acceleration error, outer loop velocity error, and outer loop displacement error at the next moment;

[0053] The closed-loop displacement error output by the outer loop adaptive variable admittance controller is: , the closed-chain constraint will Subtract the desired trajectory from the target, and then split it into the desired trajectory for the left arm and the expected trajectory of the right arm ;

[0054] The inner loop adaptive variable admittance controller:

[0055]

[0056] Where: is the inner loop acceleration error, is the quality coefficient matrix, is the damping coefficient matrix, ∆b is the adaptive damping adjustment parameter, is the inner loop speed error, is the inner ring displacement error, is the stiffness coefficient matrix, 、 、 They are the inner loop acceleration error, outer loop velocity error, and outer loop displacement error at the next moment respectively;

[0057] The inner loop adaptive variable admittance controller outputs displacement error , the displacement error Add the desired trajectory of the left and right arms to obtain the motion trajectory of the left and right robotic arms and .

[0058] A dual-arm robot system, used to implement the above-mentioned humanoid robot dual-arm adaptive force control operation method, comprising:

[0059] A dual-arm robotic arm, including a left robotic arm and a right robotic arm for performing collaborative tasks;

[0060] The six-dimensional force sensor module is configured on the left and right robotic arms and is used to detect the six-dimensional force and torque between the end effector and the operating object in real time;

[0061] The data processing module is used to receive data collected by the six-dimensional force sensor module, perform gravity compensation and coordinate conversion, calculate the grasping matrix based on the relative position relationship between the left and right arms and the operation target, decompose the six-dimensional force into internal force and external force, and input the processed data into the control system;

[0062] A control system includes an adaptive variable admittance controller, an adaptive controller based on a BFGS learning algorithm, and an inner- and outer-loop adaptive variable admittance controller. The adaptive variable admittance controller calculates the actual motion trajectory based on real-time measured force data and expected force data and generates corresponding control instructions. The adaptive controller based on the BFGS learning algorithm is used to estimate environmental stiffness and position, dynamically calculate an adaptive reference trajectory, and generate corresponding control instructions. The inner- and outer-loop adaptive variable admittance controllers include an inner-loop adaptive variable admittance controller and an outer-loop adaptive variable admittance controller, which are used to control the internal and external forces of the left and right arms, respectively, maintain the desired distance between the end position and the operation target, constrain the motion trajectories of the left and right manipulators through a closed chain, and generate corresponding control instructions.

[0063] The dual-arm robot system is used to obtain control instructions output by the control system and realize coordinated motion control of the dual-arm robot at different stages.

[0064] Beneficial effect: Compared with the existing technology, the advantages of the present invention are: the present invention first performs gravity compensation and coordinate transformation from the force reading to obtain the actual contact force with the sensor; and the force is distributed through the grasping matrix for load distribution for closed-chain motion. For the coordinated operation of the dual-arm robot, the present invention breaks down the process into a free movement stage, a constraint establishment stage and a constrained movement stage. The free movement stage adopts an adaptive variable admittance controller in which the left and right arms do not interfere with each other, so that the robot arm can still maintain flexibility and reach the destination under the interference of external forces; the difference between the constraint establishment stage and the free movement stage is that the BFGS learning algorithm is used in this stage to estimate the environmental stiffness and environmental position to estimate the desired trajectory, prevent the end of the robot arm from generating a large contact force when contacting the operation target, and solve the problem of slow force tracking; the constraint establishment stage relies on the decomposed internal and external forces to perform adaptive variable admittance control of the inner and outer loops respectively to complete the coordination work, and has the ability to resist interference. The dual-arm coordinated operation process has been implemented in scenarios such as carrying and turning valves. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 Schematic diagram of coordinate system transformation of a dual-arm collaborative robot in an embodiment of the present invention;

[0066] FIG2 is a flowchart of the coordinated operation of a dual-arm robot according to an embodiment of the present invention, wherein FIG2(a) is the free motion stage, FIG2(b) is the constraint establishment stage, and FIG2(c) is the constrained motion stage;

[0067] Figure 3 This is a control block diagram of the free motion stage in an embodiment of the present invention;

[0068] Figure 4 This is a control block diagram of the constraint establishment phase in an embodiment of the present invention;

[0069] Figure 5 This is a control block diagram of the constrained motion stage in an embodiment of the present invention.

[0070] In the figure: 10, main body, 11, right arm, 12, left arm, 20, operation target. DETAILED DESCRIPTION

[0071] The technical solution of the present invention is described in detail below with reference to the accompanying drawings, but the protection scope of the present invention is not limited to the embodiments.

[0072] Example 1: The present invention provides a method for controlling the coordinated operation process of a dual-arm robot. The control method has been implemented on the UR10 dual-arm robot and a domestic dual-arm robot. It is hereby stated that the control method is not limited to the above two dual-arm robots.

[0073] S101、 Figure 1 The dual-arm collaborative coordinate system transformation is shown, where {O} is the world coordinate system, {0L} is the left arm base coordinate system, {0R} is the right arm base coordinate system, { L} is the left arm end coordinate system, { R} is the right arm end coordinate system, { W} is the coordinate system (center of mass) of the operation target. Among them, the world coordinate system {O} is at the fixed base position of the robot arm, and the operation target coordinate system { W At the base of the operation object, the coordinates of the left and right arm ends are at the center of mass of the six-dimensional sensor, which are all related to the robot arm coordinate system. They provide a conversion relationship from the operation target coordinate system to the robot arm world coordinate system. Other coordinate systems are used to process the conversion matrix from the operation target to the world coordinate system, such as the robot arm kinematic posture conversion matrix, the coordinate system of the operation target { W There is a fixed conversion relationship between the left arm end coordinate system {L} and the right arm end coordinate system {R}. Take the conversion of the end coordinate system to the world coordinate system as an example:

[0074]

[0075] In the above formula, i takes the value R or L. When it takes the value R, is the rotation change matrix between the right arm end coordinates and the operation target, is the offset vector of the right arm end coordinate system relative to the operation target coordinate system; when the value is L, is the rotation change matrix between the left arm end coordinate system and the operation target coordinate system, It is the offset vector of the left arm end coordinate system relative to the operation target coordinate system.

[0076] S102, six-dimensional force data and torque data correspond to the external real-time contact force and contact torque respectively, and both participate in the calculation of the grasping matrix, gravity compensation conversion and adaptive variable admittance controller. The force data read by the six-dimensional force sensor is subjected to gravity compensation and coordinate conversion to obtain the force data normally read in the world coordinate system. The converted force data is As shown below:

[0077]

[0078] In the above formula, is the force data read from the six-dimensional sensor, is the posture transformation from the left and right arm end coordinate systems to the world coordinate system, is the identity matrix, and To install the inclination, is gravity, are the zero force values ​​for the three force components obtained by the least squares solution.

[0079] Force compensation and conversion also include torque, the converted torque As shown below:

[0080]

[0081] In the above formula, The torque data read from the six-dimensional sensor, are the zero force values ​​on the three moment components obtained by the least squares solution, is the coordinate expression of the center of mass in the six-dimensional force sensor coordinate system, It is the posture transformation from the world coordinate system to the left and right arm end coordinate systems.

[0082] S103, such as Figure 1 The figure shows the contact position of the target at the end of the left and right robotic arms. In order to clarify the position relationship between the end effectors of the left and right arms and the target, the corresponding grasping matrix is ​​defined. Taking the right arm as an example, the grasping matrix is ​​as follows:

[0083]

[0084] in, is the input vector Cross product operator.

[0085] The overall grasping matrix for the coordinated operation of both arms is , Represents the grasping matrix of the left arm and the right arm respectively. Each grasping matrix is ​​a 6×6 matrix (corresponding to the six-dimensional force of each arm). The two matrices are spliced ​​together to form a 6×12 grasping matrix. Through this grasping matrix, the six-dimensional force of the end can be (The force set sensed by the six-dimensional sensor, including force and torque) is converted into internal force and external forces , which is expressed as follows:

[0086]

[0087] is the pseudo-inverse of the grasping matrix.

[0088] FIG2 is a schematic diagram of a coordinated operation flow of a dual-arm robot according to an embodiment of the present invention.

[0089] During the entire dual-arm coordinated movement process, the main problems that need to be solved at different stages are not the same. As shown in Figure 2, the present invention divides the entire dual-arm collaborative operation process into the free movement stage (Figure 2(a), the constraint establishment stage (Figure 2(b)) and the constrained movement stage (Figure 2(c)). The free movement stage mainly considers whether it can reach the specified position under external interference. The constraint establishment stage is the establishment of a kinematic and dynamic relationship between the end of the dual arms and the operation target, taking into account a contact force between the end and the operation target. The constrained movement stage is the ability to complete closed-chain movement under external interference.

[0090] Example 2: Figure 3 This is a control block diagram of the free motion stage in an embodiment of the present invention.

[0091] S201: The force data and torque data of the end of the dual-arm robot can be obtained through the above S101 and S102. and expectation Subtraction is introduced into the adaptive variable admittance controller, which uses mass, damping and stiffness coefficients, combined with the feedback signal (position error , speed error Acceleration error ) to adjust the admittance (i.e., the stiffness and damping characteristics of the robot arm) to respond to changes in force and ensure stable motion.

[0092] The formula of the adaptive variable admittance controller is as follows:

[0093]

[0094] in, is the quality coefficient matrix, is the damping coefficient matrix, is the stiffness coefficient matrix, where 、 and is a constant matrix, The adaptive control rate can be adjusted to track the force, so that the adaptive variable admittance can automatically adjust the control stiffness and damping according to the current error and force changes, thereby improving the adaptability of the system. The specific update rate is as follows:

[0095]

[0096] In the above formula, , used to prevent the denominator from being 0, is the update rate.

[0097] By putting the expected trajectory The adaptive admittance controller calculates Add together to calculate the actual movement trajectory of the left and right arms , and input this value into the dual-arm robot system, which controls the movement of the dual-arm robot according to the calculated motion trajectory.

[0098] Example 3: Figure 4 The control block diagram of the constraint establishment phase is shown.

[0099] S301. Move the above-mentioned step S201 here. The difference lies in the transformation on the desired trajectory. During the constraint establishment motion phase, the end of the manipulator is in contact with the environment, and the parameters of the environment cannot be accurately obtained and may change dynamically. Uncertain environmental parameters will cause the control system to have steady-state error in contact force. To address this problem, the present invention adopts a BFGS learning algorithm to estimate the adaptive reference trajectory. The stiffness in each direction is uncoupled, and the designed impedance estimation algorithm estimates the environmental parameters in each direction.

[0100] In order to estimate the adaptive reference trajectory, the stiffness of the environment must first be estimated. and environmental location , input values ​​are: force data , end position x and terminal speed , the specific estimation pseudo code is as follows:

[0101] Adaptive reference trajectory based on BFGS learning algorithm Input: Initial iteration value #timg#, initial step size #timg#, fixed values ​​#timg# and #timg#, and a sufficiently small value #timg# If#timg#then While#timg# #timg# #timg##timg# Search for a value that satisfies the Wolfe linear search condition#timg# #timg# #timg# #timg# Update calculation#timg# #timg# Endwhile Endif Output: Environmental impedance parameter #timg#, environmental position #timg#

[0102] In the above pseudo code Is a force threshold, only when the force is greater than this value, the following learning algorithm will be performed. The matrix constructed is approximately (Hessian matrix) is .

[0103] in, , It is right Perform a partial derivative.

[0104] Search for a value that satisfies the Wolfe linear search condition , the following equation needs to be satisfied:

[0105]

[0106] For updates , the update formula is as follows:

[0107]

[0108] In the above formula and Respectively represent and .

[0109] The control process estimates the environmental stiffness using the BFGS algorithm and environmental location , aims to optimize the force control system so that the robot arm can adapt to different external force changes during the free motion phase. By feeding back force data, end position and speed, the system adaptively adjusts the environmental stiffness and then updates the reference trajectory. .

[0110] Example 4: Figure 5 This is a control block diagram of the constrained motion stage in an embodiment of the present invention. For the dual-arm collaborative robot of the present invention, this process is the constrained motion stage, which will realize the coordinated motion process of the dual-arm robot, and the end simultaneously operates the same object target to move.

[0111] S401, perform the above steps S101 and S102 to obtain the force data after compensation and coordinate conversion , and perform load distribution in step S103, Convert to internal force and external forces , the internal force is brought into the inner loop adaptive variable admittance controller, and the external force is brought into the outer loop adaptive variable admittance controller. The following is the outer loop adaptive variable admittance controller:

[0112]

[0113] The closed-loop displacement error output by the outer loop controller is , represents the difference between the current system and the target trajectory. The closed-chain constraint compares this error with the desired trajectory. The specific constraint also depends on the motion characteristics of the object. For example, when moving an object, it is only necessary to keep the end position of the robot arm at the same distance from the target and the posture in the opposite direction. Subtract the desired trajectory from the target, and then split it into the desired trajectory for the left arm and the expected trajectory of the right arm .

[0114] The adaptive variable admittance controller of the inner loop is as follows:

[0115]

[0116] Each arm has an inner loop adaptive variable admittance controller, so the displacement error is obtained The displacement error is added to the desired trajectory of the left and right arms to obtain the motion trajectory of the left and right robotic arms. and .

[0117] Using adaptive variable admittance controllers in both the inner and outer loops, the system can adjust the motion trajectories of the left and right arms in real time based on the actual forces and desired trajectories. The outer loop controller processes external force feedback and calculates displacement errors, while the inner loop controller compensates for these internal forces and displacement errors, ensuring the entire system accurately completes tasks while coordinating the movements of both arms. This control strategy, combining variable admittance control with adaptive adjustment mechanisms, can effectively cope with complex external environments and mission requirements.

[0118] As described above, although the present invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the present invention itself. Various changes may be made to it in form and detail without departing from the spirit and scope of the present invention as defined in the appended claims.

Claims

1. A method for adaptive force control operation of dual arms of a humanoid robot, characterized in that: The steps include: S1. According to the preset coordinate system relationship, establish a coordinate conversion algorithm between the world coordinate system and the left arm base coordinate system, the right arm base coordinate system, the left arm end coordinate system, the right arm end coordinate system and the operation target coordinate system; S2. Acquire six-dimensional force data and torque data collected by six-dimensional force sensors at the ends of the left and right robotic arms, perform gravity compensation on the six-dimensional force data and torque data, and convert them into six-dimensional force data and torque data in the world coordinate system through the coordinate conversion algorithm; S3. Define a grip matrix for the relative motion between the left and right robotic arm end effectors and the manipulated target, and use the grip matrix to decompose the six-dimensional force and torque data in the world coordinate system into internal forces and external forces; S4. Divide the left and right robotic arm collaboration task into a free motion stage, a constraint establishment stage, and a constrained motion stage, and execute corresponding control strategies in each stage: in the free motion stage, input the difference between the actual force data and the expected force data into an adaptive variable admittance controller, and add the output result of the adaptive variable admittance controller to the expected trajectory to obtain the actual motion trajectory and generate corresponding control instructions; in the constraint establishment stage, estimate the environmental stiffness and position parameters based on the BFGS learning algorithm, obtain an adaptive reference trajectory based on the estimated environmental stiffness and position parameters, and generate corresponding control instructions; in the constrained motion stage, use inner and outer loop adaptive variable admittance controllers to control the internal and external forces of the left and right arms respectively, maintain the expected distance between the end position and the operation target, and perform closed-chain constraints to obtain the motion trajectories of the left and right robotic arms and generate corresponding control instructions; S5. Input the generated control instructions into the dual-arm robot system to realize the coordinated motion control of the dual-arm robot at different stages.

2. The method for adaptive force control of dual arms of a humanoid robot according to claim 1, characterized in that: The conversion relationship between the left arm end coordinate system, the right arm end coordinate system and the operation target coordinate system is: in, is the rotation matrix between the right arm end coordinate system and the operation target coordinate system, is the offset vector of the right arm end coordinate system relative to the operation target coordinate system; in, is the rotation change matrix between the left arm end coordinate system and the operation target coordinate system, It is the offset vector of the left arm end coordinate system relative to the operation target coordinate system.

3. The method for adaptive force control of dual arms of a humanoid robot according to claim 2, characterized in that: The six-dimensional force data and torque data are gravity compensated and converted into force data in the world coordinate system through the coordinate conversion algorithm. ; The process of gravity compensation and conversion of the six-dimensional force data is as follows: in, The six-dimensional force data is read by the six-dimensional sensors on the left and right arms. is the attitude transformation matrix from the left and right arm end coordinate systems to the world coordinate system, I is the unit matrix, U and V are the installation inclination angles, g is the gravitational acceleration, is the zero force value of the six-dimensional force component, The value is or , and Representing the right arm and the left arm respectively; The process of gravity compensation and conversion of the torque data is as follows: in, It is the torque data read by the six-dimensional sensors of the left and right arms. is the zero force value of the moment component, is the coordinate of the center of mass in the coordinate system of the left and right arm ends, is the attitude transformation matrix from the world coordinate system to the left and right arm end coordinate systems, g is the gravitational acceleration, The value is or , and Representing the right arm and left arm respectively.

4. The method for adaptive force control of dual arms of a humanoid robot according to claim 3, characterized in that: The grip matrix Defined as: in, is the input vector Cross product operator; The overall grasping matrix of the coordinated operation of both arms is , used to convert force data Decomposed into internal forces and external forces : in, and represent the grasp matrices of the left and right arms respectively, is the terminal six-dimensional force and torque data in the world coordinate system, is the pseudo-inverse of the grasping matrix.

5. The method for adaptive force control of dual arms of a humanoid robot according to claim 4, characterized in that: During the free movement phase, the force data and expectation Subtract Input the adaptive variable admittance controller for control, The control formula of the adaptive variable admittance controller is: in, is the quality coefficient matrix, is the damping coefficient matrix, is the stiffness coefficient matrix, is the position error, which represents the difference between the actual position and the expected position at time t. is the speed error, which represents the difference between the actual speed and the expected speed at time t. is the acceleration error, which represents the difference between the actual acceleration and the expected acceleration at time t. is the position error at time t+1, is the velocity error at time t+1, is the position acceleration error at time t+1, T is the time step; ∆b ​​is the adaptive damping adjustment parameter, and its update rate is: , where , is the update rate, represents the adaptive parameter at the current time t, represents the adaptive parameters at time t+1; The expected trajectory The output of the adaptive admittance controller Add them together to get the actual movement trajectory of the left and right arms .

6. The method for adaptive force control of dual arms of a humanoid robot according to claim 4, characterized in that: Obtaining force data , end position x and terminal speed , the BFGS learning algorithm is used to estimate the adaptive reference trajectory, including: When force data Greater than the preset threshold , the following iterative steps are performed: Calculate the search direction: , where Constructed as , is the environmental stiffness parameter, is the environment location parameter, , is the objective function Stiffness to environment The gradient of is used to guide the update of the environment stiffness; Finding the appropriate step size using a linear search method , so that the objective function decrease rate satisfies the Wolfe condition: ； Update the environment stiffness: ; Update environment location: ; Calculate the position deviation: ; Update the matrix by the following formula , ， Where, and represent the increments of gradient and stiffness respectively; Until the gradient Less than the preset convergence threshold , stop iteration; The environmental stiffness estimated by the BFGS learning algorithm and environmental location , get the estimated adaptive trajectory .

7. The method for adaptive force control of dual arms of a humanoid robot according to claim 4, characterized in that: The control strategy of the constraint motion stage includes: Convert to internal force and external forces and the internal force Bring it into the inner loop adaptive variable admittance controller, the external force Bring it into the outer loop adaptive variable admittance controller; The outer loop adaptive variable admittance controller is: Where, is the outer loop acceleration error, is the quality coefficient matrix, is the external force, is the damping coefficient matrix, ∆b is the adaptive damping adjustment parameter, Outer loop speed error, is the outer ring displacement error, is the stiffness coefficient matrix, 、 、 They are respectively the outer loop acceleration error, outer loop velocity error, and outer loop displacement error at the next moment; The closed-loop displacement error output by the outer loop adaptive variable admittance controller is: , the closed-chain constraint will Subtract the desired trajectory from the target, and then split it into the desired trajectory for the left arm and the expected trajectory of the right arm ; The inner loop adaptive variable admittance controller: Where: is the inner loop acceleration error, is the quality coefficient matrix, is the damping coefficient matrix, ∆b is the adaptive damping adjustment parameter, is the inner loop speed error, is the inner ring displacement error, is the stiffness coefficient matrix, 、 、 They are the inner loop acceleration error, outer loop velocity error, and outer loop displacement error at the next moment respectively; The inner loop adaptive variable admittance controller outputs displacement error , the displacement error Add the desired trajectory of the left and right arms to obtain the motion trajectory of the left and right robotic arms and .

8. A dual-arm robot system, used to implement the humanoid robot dual-arm adaptive force control operation method according to claim 1, characterized in that: include: A dual-arm robotic arm, including a left robotic arm and a right robotic arm for performing collaborative tasks; The six-dimensional force sensor module is configured on the left and right robotic arms and is used to detect the six-dimensional force and torque between the end effector and the operating object in real time; The data processing module is used to receive data collected by the six-dimensional force sensor module, perform gravity compensation and coordinate conversion, calculate the grasping matrix based on the relative position relationship between the left and right arms and the operation target, decompose the six-dimensional force into internal force and external force, and input the processed data into the control system; A control system includes an adaptive variable admittance controller, an adaptive controller based on a BFGS learning algorithm, and an inner- and outer-loop adaptive variable admittance controller. The adaptive variable admittance controller calculates the actual motion trajectory based on real-time measured force data and expected force data and generates corresponding control instructions. The adaptive controller based on the BFGS learning algorithm is used to estimate environmental stiffness and position, dynamically calculate an adaptive reference trajectory, and generate corresponding control instructions. The inner- and outer-loop adaptive variable admittance controllers include an inner-loop adaptive variable admittance controller and an outer-loop adaptive variable admittance controller, which are used to control the internal and external forces of the left and right arms, respectively, maintain the desired distance between the end position and the operation target, constrain the motion trajectories of the left and right manipulators through a closed chain, and generate corresponding control instructions. The dual-arm robot system is used to obtain control instructions output by the control system and realize coordinated motion control of the dual-arm robot at different stages.

Images