Double-arm self-adaptive force control operation method and system for humanoid robot

Abstract

The invention belongs to the technical field of humanoid robot control, and particularly relates to a humanoid robot two-arm self-adaptive force control operation method and system, and the method comprises the steps: building a coordinate conversion algorithm between a world coordinate system and a left arm base coordinate system, a right arm base coordinate system, a left arm tail end coordinate system, a right arm tail end coordinate system and an operation target coordinate system; six-dimensional force data and torque data collected by six-dimensional force sensors at the tail ends of the left mechanical arm and the right mechanical arm are obtained, and gravity compensation and conversion are conducted; defining a grasping matrix to decompose the six-dimensional force data and the torque data into internal force and external force; dividing the cooperative task into a free motion stage, a constraint establishment stage and a constraint motion stage, and generating a corresponding control instruction in each stage; and the generated control instruction is input into a double-arm robot system, and coordinated motion control of the double-arm robot in different stages is achieved. According to the invention, coordination work can be completed and anti-interference capability is possessed.

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 progress of the times and the development of robotics technology, the traditional single-arm operation mode has been unable to meet the diverse and increasingly complex needs. In order to meet the challenges of complex and intelligent tasks, the coordinated operation of dual-arm robots has become an indispensable solution. Compared with the single-arm operation mode, dual-arm collaboration not only has stronger professional capabilities and can handle more complex operation tasks, but also has greater load capacity and a wider workspace. This dual-arm collaboration capability enables robots to perform fine operations, multi-task processing and environmental adaptation more efficiently, significantly enhancing their application potential in industrial automation, medical care, services and other fields.

[0003] Since dual-arm robots can be applied to different situations and handle complex tasks (such as carrying, turning valves, opening doors, etc.), their task planning and control methods have been widely studied. When performing collaborative tasks, dual-arm robots usually need to satisfy position constraints, force constraints, and reference trajectory constraints at the same time to achieve coordination and interactive completion of specific complex tasks. In dual-arm coordinated control, traditional position control is the main control method. This method achieves effective coordination and task completion between the two arms by maintaining a specific position constraint relationship during the coordinated operation of the dual-arm robot. However, during the operation, it does not consider the contact force between the two arms and the operated object, nor does it consider the situation where the two arms and the operated object are subject to external interference. In order to enable the dual-arm coordinated task to proceed smoothly, force-position coordinated control is essential. 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 have 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: S1. According to the preset coordinate system relationship, a coordinate conversion algorithm is established 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, obtaining six-dimensional force data and torque data collected by six-dimensional force sensors at the ends of the left and right robotic arms, performing gravity compensation on the six-dimensional force data and torque data and converting them into six-dimensional force data and torque data in the world coordinate system through the coordinate conversion algorithm; S3, defining a grasping matrix for the relative motion between the left and right robot arm end effectors and the operation target, and using the grasping matrix to decompose the six-dimensional force data and torque data in the world coordinate system into internal force and external force; S4. Divide the left and right robotic arm collaboration tasks 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, 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 the adaptive reference trajectory based on the estimated environmental stiffness and position parameters, and generate corresponding control instructions; in the constrained motion stage, use the 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.

[0006] 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: in, is the rotation change 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.

[0007] 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. ; The process of gravity compensation and conversion of the six-dimensional force data is as follows: in, The six-dimensional force data 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 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 They represent the right arm and the left arm respectively.

[0008] Furthermore, the gripping 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 Decompose into internal forces and external forces : in, and denote 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.

[0009] Furthermore, during the free motion phase, the force data and expectation Subtract Input 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 speed 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 .

[0010] Further, force data is obtained , 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: Compute the search direction: , where Constructed as , is the environmental stiffness parameter, is the environment location parameter, , is the objective function Rigidity to environment The gradient of is used to guide the update of the environment stiffness; Finding the right 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 , , In the formula, and Represent the increments of gradient and stiffness respectively; Until the gradient Less than the preset convergence threshold , stop iteration; Environmental stiffness estimated by the BFGS learning algorithm And the environment location , and obtain the estimated adaptive trajectory .

[0011] 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, and the external force Bring it into the outer loop adaptive variable admittance controller; The outer loop adaptive variable admittance controller is: In the formula, 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 it from the desired trajectory of the operation target, and then split it into the desired trajectory of 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 the 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 .

[0012] A dual-arm robot system, used to implement the above-mentioned humanoid robot dual-arm adaptive force control operation method, comprising: 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 operation object in real time; A data processing module is used to receive data collected from the six-dimensional force sensor module, perform gravity compensation and coordinate conversion, calculate the grasping matrix according to 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, comprising an adaptive variable admittance controller, an adaptive controller based on a BFGS learning algorithm, and an inner- and outer-loop adaptive variable admittance controller, wherein the adaptive variable admittance controller calculates the actual motion trajectory according to the force data and the expected force data measured in real time and generates corresponding control instructions; the adaptive controller based on the BFGS learning algorithm is used to estimate the environmental stiffness and position, dynamically calculate the adaptive reference trajectory and generate corresponding control instructions; the inner- and outer-loop adaptive variable admittance controller comprises an inner-loop adaptive variable admittance controller and an outer-loop adaptive variable admittance controller, which are used to control the internal force and external force of the left and right arms respectively, maintain the expected distance between the end position and the operation target, constrain the motion trajectory of the left and right mechanical arms 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.

[0013] Beneficial effect: Compared with the prior art, the advantages of the present invention are: the present invention first performs gravity compensation and coordinate transformation from the force reading to obtain the force that is actually in contact with the sensor; and the force is load distributed through the grasping matrix for use in closed-chain motion. For the coordinated operation of the dual-arm robot, the present invention decomposes 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 expected trajectory, prevent the end of the robot arm from generating a large contact force when contacting the operation target, and solve the slow force tracking characteristics; 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 scenes such as carrying and turning valves. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 Schematic diagram of coordinate system transformation of a dual-arm collaborative robot in an embodiment of the present invention; FIG2 is a flow chart of the coordinated operation of a dual-arm robot in an embodiment of the present invention, wherein FIG2(a) is a free motion stage, FIG2(b) is a constraint establishment stage, and FIG2(c) is a constrained motion stage; Figure 3 This is a control block diagram of the free motion stage in an embodiment of the present invention; Figure 4 This is a control block diagram of the constraint establishment phase in an embodiment of the present invention; Figure 5 This is a control block diagram of the constrained motion phase in an embodiment of the present invention.

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

[0016] 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.

[0017] Example 1: The present invention provides a method for controlling a coordinated operation process of a dual-arm robot. The control method has been implemented on a 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.

[0018] S101, such as 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 manipulated object. Among them, the world coordinate system {O} is at the fixed base position of the robot arm, and the manipulated object coordinate system { W}At the base position 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: In the above formula, i takes the value of R or L. When it takes the value of 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.

[0019] S102, the 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 As shown below: 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 system to the world coordinate system, is the identity matrix, and To install the inclination angle, is gravity, are the zero force values ​​for the three force components obtained by the least squares solution.

[0020] Force compensation and conversion also includes torque. The converted torque As shown below: 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.

[0021] S103, such as Figure 1 The position where the left and right robot arms contact the target is shown. In order to clarify the position relationship between the left and right arm end effectors and the target, the corresponding grasping matrix is ​​defined. Taking the right arm as an example, the grasping matrix is ​​as follows: in, is the input vector Cross product operator.

[0022] The overall grasping matrix for the coordinated operation of both arms is , They represent the grasping matrices of the left and right arms 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 which the six-dimensional force of the end can be (The force set felt by the six-dimensional sensor, including force and torque) is converted into internal force and external forces , which is expressed as follows: is the pseudo-inverse of the grasping matrix.

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

[0024] In the whole process of coordinated movement of both arms, the main problems that need to be solved in different stages are not the same. As shown in Figure 2, the present invention divides the whole process of collaborative operation of both arms into the free movement stage of Figure 2 (a), the constraint establishment stage of Figure 2 (b) and the constrained movement stage of Figure 2 (c). Among them, 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 both arms and the operation target, considering a contact force between the end and the operation target. The constrained movement stage is to complete the closed-chain movement under external interference.

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

[0026] S201: Through the above S101 and S102, the force data and torque data of the end of the dual-arm robot can be obtained. and expectation Subtract the phase difference into the adaptive variable admittance controller, which uses the 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.

[0027] The formula of the adaptive variable admittance controller is as follows: in, is the quality coefficient matrix, is the damping coefficient matrix, is the stiffness coefficient matrix, where , and is a constant matrix, The force can be tracked according to the adaptive control rate adjustment, 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: In the above formula, , used to prevent the denominator from being 0, is the update rate.

[0028] 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.

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

[0030] S301, move the above S201 step here, the difference is the transformation on the expected trajectory, because in the constraint establishment movement stage, the end of the robot arm is in contact with the environment, the parameters of the environment cannot be accurately obtained, and may be dynamically changing, and the uncertain environmental parameters will cause the control system to have a steady-state error in contact force. To solve 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.

[0031] In order to estimate the adaptive reference trajectory, we must first estimate the stiffness of the environment. And the environment location , input values ​​are: force data , end position x and terminal speed , the specific estimation pseudo code is as follows: 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 the 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# In the above pseudo code is a force threshold. Only when the force is greater than this value will the following learning algorithm be performed. The constructed matrix is ​​approximately (Hessian matrix) is .

[0032] in, , is right Perform a partial derivative.

[0033] Search for a value that satisfies the Wolfe linear search condition , the following equation needs to be satisfied: For Update , the update formula is as follows: In the above formula and Respectively represent and .

[0034] The control process estimates the environmental stiffness through the BFGS algorithm And the environment location , aims to optimize the force control system so that the robot 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 updates the reference trajectory. .

[0035] Embodiment 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.

[0036] S401, perform the above steps S101 and S102 to obtain 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 adaptive variable admittance controller of the outer loop: 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 the 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 opposite. Subtract it from the desired trajectory of the operation target, and then split it into the desired trajectory of the left arm and the expected trajectory of the right arm .

[0037] The following is the adaptive variable admittance controller of the inner loop: The left and right arms each have an inner loop adaptive variable admittance controller, so as to obtain the displacement error 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 .

[0038] Through the inner and outer loop adaptive variable admittance controllers, the system can adjust the motion trajectory of the left and right arms in real time according to the actual force and the expected trajectory. The outer loop controller is responsible for processing the feedback of the external force and calculating the displacement error, while the inner loop controller compensates according to the internal force and displacement error to ensure that the entire system can accurately complete the task under the coordinated movement of both arms. This control strategy combines variable admittance control and adaptive adjustment mechanism, which can effectively cope with complex external environments and task requirements.

[0039] 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 in form and details may be made 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, a coordinate conversion algorithm is established 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, obtaining six-dimensional force data and torque data collected by six-dimensional force sensors at the ends of the left and right robotic arms, performing gravity compensation on the six-dimensional force data and torque data and converting them into six-dimensional force data and torque data in the world coordinate system through the coordinate conversion algorithm; S3, defining a grasping matrix for the relative motion between the left and right robot arm end effectors and the operation target, and using the grasping matrix to decompose the six-dimensional force data and torque data in the world coordinate system into internal force and external force; S4. Divide the left and right robotic arm collaboration tasks 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, 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 the adaptive reference trajectory based on the estimated environmental stiffness and position parameters, and generate corresponding control instructions; in the constrained motion stage, use the 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 change 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 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 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 They represent the right arm and the 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 Decompose into internal forces and external forces : in, and denote 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 motion phase, the force data and expectation Subtract Input 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 speed 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 controlling the coordinated operation process of a dual-arm 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: Compute the search direction: , where Constructed as , is the environmental stiffness parameter, is the environment location parameter, , is the objective function Rigidity to environment The gradient of is used to guide the update of the environment stiffness; Find 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 , ， In the formula, and Represent the increments of gradient and stiffness respectively; Until the gradient Less than the preset convergence threshold , stop iteration; Environmental stiffness estimated by the BFGS learning algorithm And the environment location , and obtain the estimated adaptive trajectory .

7. The method for controlling the coordinated operation process of a dual-arm 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, and the external force Bring it into the outer loop adaptive variable admittance controller; The outer loop adaptive variable admittance controller is: In the formula, 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 it from the desired trajectory of the operation target, and then split it into the desired trajectory of 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 the 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 operation object in real time; A data processing module is used to receive data collected from the six-dimensional force sensor module, perform gravity compensation and coordinate conversion, calculate the grasping matrix according to 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, comprising an adaptive variable admittance controller, an adaptive controller based on a BFGS learning algorithm, and an inner- and outer-loop adaptive variable admittance controller, wherein the adaptive variable admittance controller calculates the actual motion trajectory according to the force data and the expected force data measured in real time and generates corresponding control instructions; the adaptive controller based on the BFGS learning algorithm is used to estimate the environmental stiffness and position, dynamically calculate the adaptive reference trajectory and generate corresponding control instructions; the inner- and outer-loop adaptive variable admittance controller comprises an inner-loop adaptive variable admittance controller and an outer-loop adaptive variable admittance controller, which are used to control the internal force and external force of the left and right arms respectively, maintain the expected distance between the end position and the operation target, constrain the motion trajectory of the left and right mechanical arms 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