Double-arm robot main and auxiliary moving operation coordination control method based on vision / force sense servo and implementation

Abstract

The invention discloses a double-arm robot main and auxiliary moving operation coordination control method based on vision / force sense servo and implementation thereof. Double-arm coordination is supported, multi-sensing information is fused, compliant operation is considered, and the method is adaptive to a moving scene. The mobile main and auxiliary double-arm robot control system comprises a mobile base subsystem, a main and auxiliary double-arm subsystem and a control subsystem. The double-arm main and auxiliary operation coordination control method based on visual / force sense servo comprises visual servo target tracking based on a tail end camera and force sense servo compliant operation control based on a force sensor, operation tasks are achieved in the state that a base moves, and the double-arm main and auxiliary operation coordination control method adapts to the complex working environment; a state machine mechanism is adopted to manage a mechanical arm motion mode, screw assembly task-oriented operation process arrangement is realized, and the efficiency and controllability of the operation process are improved.

Description

Technical Field

[0001] This invention belongs to the field of robot control and automated assembly technology, specifically relating to a coordinated control method and implementation for the main and auxiliary movement operations of a dual-arm robot based on visual / force servo systems. Background Technology

[0002] In fields such as industrial automation assembly, logistics, and aerospace engineering, robotic arms, as highly automated devices that liberate manpower and improve efficiency, are being widely used in various practical tasks. Screw assembly, as a typical high-precision operation, places stringent requirements on the dynamic response, target tracking stability, and force control compliance of robotic arm systems. However, traditional robotic arm vision servo control schemes based on segmented trajectory planning follow a cyclical process of visual target localization, robotic arm trajectory planning, and planned trajectory execution. Each closed-loop cycle is time-consuming, and the response to moving targets is lagging, making it difficult to adapt to dynamic operation scenarios. The generated joint planning trajectory is difficult to take into account the target visibility, which can easily lead to the target going out of the camera's field of view, seriously affecting operational efficiency. The utilization rate of visual information is low, wasting sensor bandwidth, while force sensor information is not fully utilized, failing to meet the requirements for operational compliance. Currently, the mainstream single-arm robotic system has drawbacks such as low degrees of freedom and low collaboration, making it difficult to complete delicate operations such as screw assembly and collaborative grasping. In addition, although mobile base robotic arms are being used more widely as a solution to overcome the limited working range of traditional fixed base robotic arms, there are still no mature solutions for issues such as dynamic compensation of the robotic arm end pose, synchronous transmission of data between multiple subsystems, and dual-arm collaborative operation. This restricts the improvement of the automation level of screw assembly.

[0003] Against this backdrop, this invention presents a hybrid servo control scheme for a robotic arm that integrates multi-sensor information, supports dual-arm collaboration, and adapts to mobile scenarios. This scheme achieves high-precision, high-efficiency screw assembly while also ensuring compliant operation. Compared to control methods based on traditional trajectory planning, this invention utilizes the MoveIt! Servo real-time control software package within the MoveIt! framework to directly control the pose of the end-effector tools on both arms, making it a more suitable solution for high-frequency, low-latency applications such as target tracking. The system employs a master-slave dual-arm mode, with detailed division of labor between the two arms for the screw assembly task. A state machine mechanism is used to automatically connect all task sub-processes, simplifying algorithm design while improving process controllability. Summary of the Invention

[0004] Purpose of the invention: The purpose of this invention is to propose a mobile master-slave dual-arm robot control system and its dual-arm master-slave operation coordination control method based on vision / force servoing, providing a dual-arm control scheme that supports dual-arm collaboration, integrates multi-sensor information, takes into account compliant operation, and adapts to mobile scenarios, thereby realizing high-precision and high-efficiency screw assembly tasks.

[0005] Technical solution: A mobile main-auxiliary dual-arm robot control system, comprising a mobile base subsystem, a main-auxiliary dual-arm subsystem, and a control subsystem.

[0006] The mobile base subsystem includes a Mecanum wheel base chassis and a control box. The base chassis is driven by a set of four parallel Mecanum wheels, enabling omnidirectional movement in a plane. The control box under the base chassis houses a battery, a switching power supply, a Mecanum wheel motor driver, an Ethernet switch, an Ethernet-CAN protocol conversion module, a tool gripper control box, and an onboard computer.

[0007] The main and auxiliary dual-arm subsystem includes two 6-DOF robotic arms (main and auxiliary), a vision camera, a six-dimensional force sensor, and a tool gripper. The main and auxiliary dual-arm bases are fixed to the base chassis, and their ends are respectively fixed to the vision camera, the six-dimensional force sensor, and the tool gripper via adapter flanges.

[0008] The control subsystem is based on the ROS2 Iron development environment on the Ubuntu 22.04 platform, and includes a camera image processing module, a ROS2 controller framework, a MoveIt! Servo framework, a robotic arm servo drive module, and a core operation flow control module. The Robot Operating System (ROS2) provides functions for robotic arm model description, motion control, and communication and coordination between modules.

[0009] The camera image processing module receives image data through the camera's ROS2 driver node and identifies and calculates the relative pose of Apriltag code targets in the image through the Apriltag_ros image processing node.

[0010] The ROS2 controller framework publishes the expected angle data of each joint of the robotic arm, and simultaneously receives the actual state data of each joint returned from the system.

[0011] The MoveIt! Servo framework uses the six-dimensional velocity command data stream of the robotic arm's end effector as control input to generate smooth joint position commands and send them to the robotic arm's joint motors to control the robotic arm's movement.

[0012] The robotic arm servo drive module packages the desired angle data of each joint of the robotic arm into UDP data packets and sends them, while parsing the received UDP data packets into the actual state data of each joint of the robotic arm.

[0013] The core operation process control module uses a state machine mechanism to manage the behavior and state switching process of all movement modes of the main and auxiliary arms, ensuring that the two arms can cooperate to complete the screw assembly operation task.

[0014] A dual-arm master-slave operation coordination control method based on visual / force servoing includes visual servoing based on images from the end-effector camera and force servoing compliant operation control based on six-dimensional force sensor information. This method is used in the aforementioned mobile master-slave dual-arm robot control system to perform screw assembly operations on a moving target while the base is in motion. During the target pre-grabbing phase, both the master and slave arms are in visual servoing motion mode, and the end-effector camera provides image information for the robot arm's target tracking and approach process. In the combined operation phase after the tool gripper grasps the target, the slave arm performs compliant gripping operations based on the force sensor measurement information. The dual-arm master-slave operation coordination control method based on visual / force servoing mainly includes the following steps:

[0015] Step 1: Perform kinematic modeling on the mobile dual-arm system, export the model file, and load it into ROS2 to create a robot model. Specifically, this includes the following steps:

[0016] Step 1.1: Use the DH method to perform kinematic modeling of the system.

[0017] Step 1.2: Export the established mobile dual-arm model as URDF and the corresponding mesh file, and perform a series of robot configurations in the ROS2 environment.

[0018] Step 1.3: Integrate the ROS2 controller functionality into the model file.

[0019] Step 1.4: Configure and run MoveIt! to check if the robot system visualization model is working properly.

[0020] Step 2: Power on the system and initialize the software and hardware.

[0021] Step 3: In the camera image processing module, the end-effector camera driver node of the main and auxiliary dual arms acquires image data, and the Apriltag_ros image processing node calculates the relative pose of the Apriltag target based on the image data.

[0022] Step 4: The moving base performs trajectory tracking motion, and feedback control is performed based on the base position and the desired trajectory.

[0023] Step 5: Move both arms to track moving targets using a visual servoing algorithm, and calculate the target system ∑. io 、Tool system ∑ it With the expected system of tools Σ id The pose is calculated and the tool system Σ is generated based on the control law. it The six-dimensional velocity commands are used to control the movement of the robotic arm via MoveIt! Servo. Specifically, this includes the following steps:

[0024] Step 5.1, Obtain the tool system Σit Relative to the base system Σ B pose matrix Tool System Σ it Relative to the expected system ∑ id pose matrix Target system ∑ io Compared to the tool system ∑ it Position vector (represented in the system ∑) it middle)

[0025] Step 5.2, Calculate the matrix Axis-angle parameters (θ) i u i ).

[0026] Step 5.3: Calculate the tool system ∑ using the control law. it Linear velocity and angular velocity control quantities

[0027] Step 5.4: Calculate the system ∑ through coordinate transformation. it Linear velocity and angular velocity control quantities Composition system ∑ B Lower series ∑ it Six-dimensional generalized velocity expectation

[0028] Step 5.5, will The six-dimensional velocity command at the end is input in real time to the MoveIt! Servo node of arm i.

[0029] Step 5.6: The main and auxiliary arms simultaneously execute steps 5.1-5.5 in a loop, controlling the movement of each joint through independent MoveIt! Servo node running threads to achieve visual servoing function.

[0030] Step 6: The dual arms move based on the pose servo algorithm, setting a static target pose in the workspace and using it as the tool expectation system ∑. id The pose is controlled using a method similar to step 5, which controls the movement of the robotic arm to make the tool system Σ it Motion and approaching the system Σ id Specifically, it includes the following steps: Set a static target pose in the workspace and use it as the tool expectation system Σ id posture,

[0031] Step 6.1, Obtain the current pose matrix

[0032] Step 6.2, similar to step 5.2, calculate the matrix. Axis-angle parameters (θ)i u i ).

[0033] Step 6.3, similar to step 5.3, calculates the system Σ. it Linear velocity and angular velocity control quantities

[0034] Step 6.4, similar to step 5.4, calculates the system Σ. it Expected speed

[0035] Step 6.5, similar to steps 5.5-5.6, sets the desired speed. The six-dimensional velocity command at the end of the arm is input in real time to the MoveIt! Servo node of arm i to drive the movement of each joint.

[0036] Step 7: After the auxiliary arm gripper closes and holds the target handle, the system enters the assembly operation phase. Based on the force value measured by the end effector force sensor, compliant gripping control of the target is performed to ensure that the contact force of the assembly system is within a reasonable range. Specifically, this includes the following steps:

[0037] Step 7.1: Obtain the actual force measurement value of the force sensor under the current configuration of the auxiliary arm using the gravity compensation algorithm under static conditions. s f TF and torque measurement value s m TF (represented in this system Σ) S middle).

[0038] Step 7.2, obtain the system Σ S Relative system Σ B pose matrix B RS, represented in the base system Σ through coordinate transformation B Forces in B f TF and torque B m TF .

[0039] Step 7.3: The auxiliary arm adjusts the movement of the robotic arm based on the measured value of the actual applied force. Its underlying control mode is consistent with that in Step 5, according to Step 7.2. B f TF , B m TF The results of each component after dead zone processing B f TF(dz) , B m TF(dz) Σ was calculated lt Linear velocity and angular velocity control quantities of the system

[0040] Step 7.4, similar to steps 5.5-5.6, involves setting the desired speed. The six-dimensional velocity command at the end of the arm is input in real time to the MoveIt! Servo node of the auxiliary arm to drive the movement of the auxiliary arm, so that its end moves in the direction of the applied force / torque, thereby weakening the applied force.

[0041] Step 8: In the core operation process control module, the main and auxiliary arms cooperate to perform screw assembly operations according to the preset state machine process.

[0042] like Figure 3 As shown, the auxiliary arm state machine has 5 states: IDLE (idle), APPROACH_HOME (approaching the auxiliary arm HOME configuration), TRACK_TARGET (visual servo tracking target), COMPENSATE (target compliant gripping), and ERROR (error occurred).

[0043] like Figure 4 As shown, the main arm state machine has 10 states: IDLE (idle), APPROACH_TOOLBOX (approaching toolbox configuration), GRASP_TOOL (grabbing screw), LEAVE_TOOLBOX (leaving toolbox configuration), APPROACH_HOME (approaching main arm HOME configuration), TRACK_TARGET (visual servo tracking target), INSERT (pre-inserting screw), SCREW (screw tightening operation), PULL (retracting end effector), and ERROR (error occurred).

[0044] The specific steps for tightening screws when the main boom is in the SCREW position are as follows:

[0045] Step 8.1, set the starting position of the main boom screw tightening operation in the workspace. rd pose matrix B R rD(ini) .

[0046] Step 8.2: The main arm stops moving.

[0047] Step 8.3, the main arm identifies the target system Σ with the identification code on the hexagonal screw cap. ro Corresponding system Σ rd pose matrix B R rD , in turn the system Σ rd After rotating around its own x-axis by 60°, 120°, 180°, 240°, and 300°, a total of 6 alternative posture matrices for grasping the hexagonal screw cap are obtained.

[0048] Step 8.4: Calculate the matrix of each candidate pose sequentially.B R rD(i) (i=0, 1, 2, 3, 4, 5) and B R rD(ini) The difference is expressed by the attitude error function Ψ E (i) is used to measure.

[0049] Step 8.5, the function Ψ in step 8.4 E (i) The minimum value corresponds to B R rD(i) As a system rd The attitude matrix is ​​substituted into the visual servoing algorithm of the main arm in step 5 to perform visual tracking of the hexagonal screw cap identification code target.

[0050] Step 8.6: Close the main arm jaws to hold the hexagonal nut.

[0051] Step 8.7 is similar to step 5.4. The six-dimensional velocity command input from the end effector is given to the MoveIt! Servo node of the main arm, controlling the end effector to move at an angular velocity ω. screw Around the tool system ∑ rt The x-axis rotates in the positive direction, causing the screw to be screwed into the screw hole. Step 8.8: After the main arm gripper has been screwed into the screw in the forward direction for a period of time, stop the movement and open the gripper, then loosen the screw cap.

[0052] Step 8.9, similar to step 8.7, controls the end effector of the main arm to move at an angular velocity ω. screw Around the tool system ∑ rt After rotating the x-axis in the opposite direction for a period of time to restore the main arm to its initial position, proceed to step 8.2.

[0053] Repeat steps 8.2-8.9 until the screw is fully screwed into the screw hole, and the screw-tightening operation is complete.

[0054] Step 9: The operation task is completed, the system stops running, and the power is cut off.

[0055] Compared with existing technologies, the significant advantages of the mobile master-slave dual-arm robot control system and the dual-arm master-slave operation coordination control method based on vision / force servoing proposed in this invention include:

[0056] 1. Compared to the segmented, cyclical "positioning-planning-execution" mode in traditional robotic arm operation control, this invention uses the MoveIt! Servo framework to directly control the speed of the robotic arm's end effector. The speed command matches the underlying motor control frequency, resulting in a smooth trajectory with low latency and faster tracking response to dynamic targets.

[0057] 2. By integrating visual and force sensor data, visual information guides target tracking and pre-grabbing, while force sensor information guides compliant post-grabbing operations, achieving complementary functions. Sensor data is transmitted and parsed in real time through the ROS2 framework, enabling rapid data interaction with the underlying actuators. The visual servoing algorithm ensures that the target's trajectory in the camera image is approximately a straight line, avoiding the target slipping out of the field of view due to unreasonable joint trajectories in traditional planning.

[0058] 3. The mobile base adopts an omnidirectional movement design with Mecanum wheels, which allows the system to perform operations while in a mobile base state, overcoming the application limitations of traditional fixed robotic arms and adapting to complex and special working environments.

[0059] 4. The main and auxiliary arms employ a state machine mechanism to manage motion modes, enabling task-oriented operation flow programming for screw assembly. The main arm focuses on core operations such as screw gripping and screwing, while the auxiliary arm is responsible for capturing and stabilizing the target and coordinating internal forces within the system, resulting in a clear division of tasks. It supports automatic connection of all task sub-flows, simplifying algorithm design while improving process controllability. Attached Figure Description

[0060] Figure 1 This is a schematic diagram of the control system for the mobile main and auxiliary dual-arm robot in this invention.

[0061] Numbered in the diagram: 1: Main boom; 2: Main boom end-effector camera; 3: Main boom end-effector tool gripper; 4: Auxiliary boom end-effector camera; 5: Auxiliary boom end-effector force sensor; 6: Auxiliary boom end-effector tool gripper; 7: Auxiliary boom; 8: Base chassis; 9: Base wheel; 10: Control box.

[0062] Figure 2 This is a schematic diagram of the overall framework of the mobile main and auxiliary dual-arm robot control system in this invention.

[0063] Figure 3 This is a flowchart of the auxiliary arm state machine of the dual-arm master-auxiliary operation coordination control method based on visual / force servo in this invention.

[0064] Figure 4 This is a flowchart of the master arm state machine of the dual-arm master-slave operation coordination control method based on visual / force servo in this invention. Detailed Implementation

[0065] like Figure 2 The diagram shows the overall framework of the mobile dual-arm robot control system of this invention. The system includes a mobile base subsystem, a main and auxiliary dual-arm subsystem, and a control subsystem.

[0066] The mobile base subsystem is driven by a set of four parallel Mecanum wheels, enabling omnidirectional movement in a plane. The control box beneath the base chassis houses the hardware of the electrical control system, including the battery and voltage regulator, peripheral drivers, network and communication cables, and the onboard computer at the control center. A DC switching power supply connects to the lithium battery pack at its input and provides regulated power to the control system at its output. The Mecanum wheel motor driver operates in speed control mode, receiving speed signals from the CAN bus and controlling the Mecanum wheel motor to run at the desired speed. An Ethernet switch handles network data transmission between the onboard computer and the Ethernet-CAN protocol conversion module. The Ethernet-CAN protocol conversion module has two CAN bus ports, CAN1 and CAN2, with different IP addresses, completing the protocol conversion between network data and CAN bus data. The CAN1 port connects to the Mecanum wheel motor driver, the main arm joint motors, and the end effector via the CAN bus to form a network control system. The CAN2 port connects to the auxiliary arm joint motors and the end effector via the CAN bus to form a network control system.

[0067] In the main and auxiliary dual-arm subsystem, both the main arm and the auxiliary arm are assembled from 6 Kollmorgen RGM collaborative joint modules and alloy rods. Each joint module is a drive and control unit, connected in series via power lines and a CAN bus to form a network. The joint motors operate in position control mode, receiving the desired position signal from the CAN bus to perform servo motion. Realsense D405 cameras are installed at the ends of both arms for target recognition, six-dimensional force sensors are used to measure force / torque, and tool grippers provide object grasping and manipulation functions.

[0068] In the control subsystem, the Allspark 2-x86 onboard computer is equipped with Ubuntu 22.04 and the ROS2 Iron environment. It connects to and communicates with the end effector cameras and force sensors on the main and auxiliary arms via USB ports, connects to an Ethernet switch via a wired network port, and connects to the local area network via a WiFi interface. The computer's underlying system controls the motion of the base wheel, the joints of the robotic arm, and the end effector by sending UDP network packets to specific IP addresses.

[0069] In the camera image processing module, the camera's ROS2 driver node publishes the received image data to a topic. The apriltag_ros image processing node subscribes to the topic to obtain images, identifies all Apriltag target objects in the image using a specific algorithm, calculates their relative poses relative to the camera system, and publishes them to the ROS2 coordinate transformation tree.

[0070] In the ROS2 controller framework, the kernel publishes messages containing the desired angles of each joint of the robotic arm to the robotic arm servo drive module at a frequency of 400Hz, and subscribes to messages returned by the robotic arm servo drive module containing the real-time values ​​of the joint's actual angle, speed, and torque.

[0071] The MoveIt! Servo framework takes a 30Hz six-dimensional velocity command data stream from the robotic arm's end effector as input. Internally, it generates a series of desired joint position trajectory points within each command update cycle, then sends these points as position commands to the underlying joint motors in timestamp order. The order-of-magnitude difference between the underlying motor control frequency and the end effector velocity command issuance frequency ensures the smoothness and stability of the trajectory generated by MoveIt! Servo.

[0072] The robotic arm servo drive module is the underlying node responsible for UDP communication via the network port. It parses and converts the subscribed desired angle messages of each joint of the robotic arm, packages them into UDP data packets, and sends them to the Ethernet switch. At the same time, it parses the received UDP data packets into real-time angle, speed, and torque data of each joint of the robotic arm and publishes them.

[0073] The core operation process control module uses a state machine mechanism to manage the behavior and state transition processes of all motion modes of the main and auxiliary arms. By arranging the execution sequence of the arm states and designing the state transition conditions appropriately, it ensures that the two arms can cooperate to complete the screw assembly operation task under normal process control.

[0074] The present invention provides a dual-arm master-slave operation coordination control method based on visual / force servoing, which mainly includes the following steps:

[0075] Step 1: Perform kinematic modeling of the mobile dual-arm system using the DH method. Create a 3D model in Solidworks and export it as a URDF file and mesh files for each link using a plugin. Then load these files into ROS2 to create the robot model. Further steps include:

[0076] Step 1.1: Use the DH method to perform kinematic modeling of the system and obtain the coordinate system ∑ of each link of the main arm. r1 ~Σ r6 Posture and the angle of the main arm joint q r1 ~q r6 The relationships between the links and the coordinate system of each link in the auxiliary arm ∑ l1 ~Σ l6 Position and auxiliary arm joint angle q l1 ~q l6 The relationship between the main and auxiliary double-arm base linkage system ∑ r0 ,∑ l0 With the moving base coordinate system ∑ B Fixed connection.

[0077] Step 1.2: In Solidworks, use the SW2URDF plugin to export the created mobile dual-arm model as a URDF file and its corresponding mesh file. Run Moveit! Setup Assistant in the ROS2 environment to read the URDF file and perform a series of configurations, including robot structure, visualization, home pose, kinematics solver, and joint group definition planning, generating the basic ROS2 function package. Within the ROS2 coordinate transformation framework, the coordinate transformation tree of the mobile dual-arm system is based on the base system ∑ B As the root node and extending to the ends of the main and auxiliary arms, the relative pose between any two systems in the transformation tree can be obtained immediately after the real-time angle values ​​of each joint are input.

[0078] Step 1.3: Modify the URDF file to use the XML scripting language under the XACROS framework, and add a reference to the ROS2_Control plugin to integrate the ROS2 controller functionality.

[0079] Step 1.4: Configure and run MoveIt!, and view and check whether the created robot system visualization model is normal in the Rviz interface.

[0080] Step 2: Power on the system and initialize the hardware and software, including enabling the base wheel motor, enabling the robotic arm joint motor, enabling and initializing the end effector gripper, starting the global vision positioning system data receiving node, camera image receiving and processing node, force sensor data receiving node, underlying driver node, and ROS2 controller manager node, loading the ROS2 controller, MoveIt! Servo and other nodes, and initializing the dual-arm state machine.

[0081] Step 3: In the camera image processing module, the main and auxiliary dual-arm end-effector camera driver nodes publish image data to a topic, and then the corresponding Apriltag_ros image processing nodes subscribe to it, calculate and publish the coordinate system of the Apriltag tag code recognized by the main and auxiliary arm end-effector cameras relative to the end-effector camera system Σ. rc Σ lc The pose transformation relationship.

[0082] Step 4: The mobile base performs trajectory tracking motion. Its actual position is measured by the global visual positioning system and calculated in real time by the receiving node, serving as feedback for trajectory tracking control. The desired position signal of the base is a sequence of trajectory points pre-calculated based on the mission objective.

[0083] Step 5: Move both arms to track moving targets using a visual servoing algorithm, denoted as i = l, r, where l is the auxiliary arm and r is the main arm, Σ roΣ lo The coordinate systems are the target-fixed coordinate systems of the main arm and the auxiliary arm, respectively. rt Σ lt The coordinate systems are the tool coordinate systems for the main arm and the auxiliary arm, respectively; Σ rd Σ ld The coordinate systems are the expected coordinate systems of the main arm and the auxiliary arm, respectively; ∑ rc ,∑ lc The coordinate systems are the camera-fixed coordinate systems for the main arm and the auxiliary arm, respectively; the system ∑ id With system Σ io Relative pose fixed, indicating the pose offset of the tool relative to the target when the robotic arm grasps the target, where the position offset is (represented by Σ). id middle) The visual servoing algorithm specifically includes the following steps:

[0084] Step 5.1: Obtain the current tool system Σ in the ROS2 transformation tree. it Relative to the base system Σ B pose matrix Tool System Σ it Relative to the instrumental expectation system Σ id pose matrix Target system Σ io Compared to the tool system ∑ it Position vector (represented in system Σ) it middle)

[0085] Step 5.2, Calculate the matrix Axis-angle parameters (θ) i u i ).

[0086] Step 5.3, using the control law calculation tool system Σ it Speed ​​control quantity in Σ it The system relative to Σ B The linear velocity and angular velocity (represented in ∑) it (In the system). The control law is calculated in proportional-integral form as follows: Where the matrix sin c(x) = sin x / x, λ i1 , λ i2 >0 is a constant coefficient. Equation (1) guarantees that Tool System ∑ it Converging to the expected value of the instrument ∑ id The target and the target are aligned and reach a pre-grabbing pose that is relatively stationary. At the same time, the target's trajectory in the camera image is approximately a straight line, making it difficult for it to escape the camera's field of view.

[0087] Step 5.4, calculate ∑ through coordinate transformation. it The system relative to ∑ B linear velocity of the system With angular velocity (indicated in ∑) B (in the system), forming the base system ∑ B Lower Tool System ∑ it Six-dimensional generalized velocity expectation The coordinate transformation formula is:

[0088] Step 5.5, will The six-dimensional velocity command at the end of arm i is input in real time to the MoveIt! Servo node of that arm.

[0089] Step 5.6: The main and auxiliary arms each form a planned joint group, and simultaneously execute steps 5.1-5.5 in a loop. The movement of each joint is controlled by the independent MoveIt! Servo node threads to achieve visual servoing function.

[0090] Step 6: The dual arms move based on the pose servo algorithm, and a static tool expectation system Σ is set in the workspace. id Pose, control tool system Σ it Motion and approaching the system ∑ id The difference from step 5 is that the target system Σ is now... io It does not participate in the calculation; in the algorithm, Σ can be considered as not participating in the calculation. id With system Σ io Overlap. Specifically, this includes the following steps:

[0091] Step 6.1: Obtain the current tool system Σ in the ROS2 transformation tree. it Relative to the base system ∑ B pose matrix Tool System Σ it Relative to the instrumental expectation system Σ id pose matrix

[0092] Step 6.2, similar to step 5.2, calculate the matrix. Axis-angle parameters (θ) i u i ).

[0093] Step 6.3, similar to step 5.3, uses the control law calculation tool system Σ it Speed ​​control quantity Guarantee system Σ it Converging to the system ∑ idThe calculation formula is the following proportional-integral form control law: The above formula represents the sum of all target systems in the control law of step 5.3. io Replace with the instrumental expectation system ∑ id It was obtained later.

[0094] Step 6.4, similar to step 5.4, involves calculating the base system Σ through coordinate transformation. B Lower Tool System ∑ it Six-dimensional generalized velocity expectation

[0095] Step 6.5 is similar to steps 5.5-5.6, where... The six-dimensional velocity command at the end of arm i is input in real time to the MoveIt! Servo node of that arm to control the movement of each joint and realize the pose servo function.

[0096] Step 7: After the auxiliary arm's gripper closes and holds the target handle, the system enters the combined operation phase. A relatively tight connection is formed between the arm and the target, creating strong coupling between their movements. The auxiliary arm uses end-effector force sensor measurements to compliantly grip the target, ensuring that the internal forces of the combined system remain within a reasonable range and preventing damage to the mechanical structure. Specific steps include:

[0097] Step 7.1: The movement speed of the auxiliary arm end is relatively slow, and the influence of inertial force during clamping can be ignored. The actual force measurement value of the force sensor under the current configuration of the auxiliary arm is obtained through the gravity compensation algorithm under static conditions, which is ∑ along the sensor body coordinate system. S Forces and moments in the x, y, and z axes (represented in this system ∑) S middle): S f TF =[f x f y f z ] T , S m TF =[m x m y m z ] T .

[0098] Step 7.2, obtain the system ∑ in the ROS2 transform tree. S Relative to the system ∑ B pose matrix B R S Through coordinate transformation B f TF= B R S · S f TF , B m TF = B R S · S m TF , Obtaining the representation in the base system ∑ B Forces in B f TF and torque B m TF .

[0099] Step 7.3: The auxiliary arm adjusts the movement of the robotic arm according to the applied force. Its underlying control mode is the same as in Step 5, and Σ is calculated. lt Linear velocity and angular velocity control quantities The formula is Among them, the coefficients k1, k2 > 0 are constant coefficients. B f TF(dz) , B m TF(dz) Each of the steps in step 7.2 B f TF , B m TF The results of each component after dead zone processing: Under the control law based on the force sensor data, the auxiliary arm remains stationary when the internal force is small, and adjusts its movement when it exceeds a certain range.

[0100] Step 7.4, set the desired speed at the end of the auxiliary arm. The six-dimensional velocity command is input in real time to the MoveIt! Servo node of the auxiliary arm, which drives the movement of the auxiliary arm through the same control method as in step 5.6, thereby increasing its end-effector linear velocity. angular velocity Along the force respectively B f TF(dz) torque B m TF(dz) The direction is adjusted to weaken the force. The auxiliary arm provides both compliant gripping and a certain degree of gripping position retention capability.

[0101] Step 8: In the core operation process control module, the main and auxiliary arms cooperate to perform screw assembly operations according to the preset state machine process.

[0102] like Figure 3As shown, the auxiliary arm state machine has 5 states. The IDLE state is the initial state of the auxiliary arm and the final state after the normal completion of the operation process, during which it is idle and stops moving. In the APPROACH_HOME state, the auxiliary arm moves based on the pose servo algorithm in step 6 and makes the tool system Σ lt Approaching the HOME configuration, this configuration represents the position of the auxiliary arm before and after tracking the target. In the TRACK_TARGET state, the auxiliary arm tracks the Apriltag target at the target handle based on the visual servoing algorithm in step 5 until the gripper reaches the pre-grasping pose, and closes the gripper at the appropriate time to hold the handle. In the COMPENSATED state, while holding the target handle, the auxiliary arm performs follow-up compensation for unwanted system movements based on the compliant gripping algorithm in step 7, ensuring that the gripping force is within a reasonable range. If the auxiliary arm experiences an operation timeout error, it enters the ERROR state and immediately stops moving. Table 1 shows the state transition conditions of the auxiliary arm's state machine. Under normal circumstances, the state transition process of the auxiliary arm is IDLE, APPROACH_HOME, TRACK_TARGET, COMPENSATED, APPROACH_HOME, IDLE.

[0103] like Figure 4 As shown, the main arm state machine has 10 states. In the IDLE, APPROACH_HOME, TRACK_TARGET, and ERROR states, the main arm behaves the same as the auxiliary arm. In the APPROACH_TOOLBOX state, the main arm moves based on the pose servo algorithm in step 6 and makes the tool system Σ rtThe toolbox on the base is positioned to a preset position, ensuring the screw cap is within the field of view of the end-effector camera. In GRASP_TOOL mode, the main arm tracks the Apriltag identifier on the top of the hexagonal screw cap using the visual servoing algorithm from step 5, causing the gripper to reach the pre-grabbing pose of the screw cap and closing the gripper at the appropriate time to grasp the screw. In LEAVE_TOOLBOX mode, the main arm moves using the pose servoing algorithm from step 6, raising the gripper vertically a certain distance to ensure the gripped screw is not affected by lateral friction and changes its axis, thus vertically leaving the base. In INS... In the ERT state, the main arm aligns the axis of the clamped screw with the axis of the screw hole on the target and keeps them concentric. At this point, based on the visual servoing algorithm from step 5, the end effector moves a short distance along the screw hole axis, ensuring the outer thread of the screw thread fits tightly against the inner wall of the screw hole. In the SCREW state, the main arm performs the screw-tightening operation, using its own movement to gradually screw the clamped screw into the screw hole. In the PULL state, the main arm opens its gripper to disengage from the screw cap, and then, based on the visual servoing algorithm from step 5, the end effector retracts a short distance along the screw hole axis to ensure that the gripper does not collide with the external screw cap during subsequent retraction of the main arm. Table 2 describes the state transition conditions of the main arm's state machine. Under normal circumstances, the main arm's state transition process is as follows: IDLE, APPROACH_TOOLBOX, GRASP_TOOL, LEAVE_TOOLBOX, APPROACH_HOME, TRACK_TARGET, INSERT, SCREW, PULL, APPROACH_HOME, IDLE.

[0104] The specific steps for tightening screws when the main boom is in the SCREW position are as follows:

[0105] Step 8.1, set the starting position of the main boom screw tightening operation in the workspace. rd pose matrix B R rD(ini) .

[0106] Step 8.2: The main arm stops moving.

[0107] Step 8.3: The main arm uses the camera image processing module to identify the target system Σ marked on the hexagonal screw cap. ro Calculate its corresponding coefficient ∑ rd pose matrix B R rD and sequentially put the system ∑ rd After rotating around its own x-axis by 60°, 120°, 180°, 240°, and 300°, a total of 6 alternative posture matrices for grasping the hexagonal screw cap are obtained, along with the initial values. B R rD (0) B RrD(1)_ , B R rD(2) , B R rD(3) , B R rD(4) , B R rD(5) .

[0108] Step 8.4: Calculate the matrix of each candidate pose sequentially. B R rD(i) (i=0, 1, 2, 3, 4, 5) and B R rD(ini) The difference is calculated using the following formula: Where tr(·) is the trace of the matrix, Ψ E (i)∈[0,2] is the attitude error function, which varies with the matrix In the axis-angle representation, the absolute value of the rotation angle increases monotonically with the increase of the rotation angle.

[0109] Step 8.5: Apply the visual servoing algorithm of the main arm from Step 5 to track the hexagonal screw cap identification code target, and use the function Ψ from Step 8.4. E (i) The minimum value corresponds to B R rD(i) As an instrumental expectation system Σ in the algorithm rd The attitude matrix is ​​used in the calculation.

[0110] Step 8.6, when the main arm tool system Σ rt The device is now stably positioned in the pre-grabbing pose of the hexagonal screw cap, and the main arm grippers are closed to hold the hexagonal screw cap. Steps 8.3-8.6 update the changed screw cap identifier target pose before each gripping of the hexagonal screw cap in the initial pose, and calculate the most suitable tool expectation system Σ. id Adjust the posture accordingly.

[0111] Step 8.7: After the main arm gripper is in the clamping state and holds stably, control the end of the main arm to move at an angular velocity ω. screw Around the tool system Σ rt The x-axis rotates in the positive direction, causing the screw to be screwed into the screw hole. Specifically, in step 5.3, the main arm tool system is set ∑. rt Speed ​​control quantity for: Where p screw This represents the current thread pitch of the screw. The x-component is the angular velocity when the screw is tightened. The x-component is the linear velocity along the screw axis calculated based on the screw pitch. Then, similar to step 5.4... The six-dimensional velocity command at the end of the arm is input to the MoveIt! Servo node of the main arm, which controls the main arm to perform the screwing motion.

[0112] Step 8.8: After the main arm gripper has been screwed into the screw in the forward direction for a period of time, stop the movement and open the gripper, then loosen the screw cap.

[0113] Step 8.9: After the main arm gripper is in the open state, similar to step 8.7, control the end effector of the main arm to move at an angular velocity ω. screw Around the tool system Σ rt After rotating the x-axis in the opposite direction for a period of time to restore the main arm to its initial position, proceed to step 8.2.

[0114] Repeat steps 8.2-8.9 until the screw is fully screwed into the screw hole, and the screw-tightening operation is complete.

[0115] Step 9: The operation task is completed, the system stops running, and the power is cut off. Table 1. Explanation of Auxiliary Arm State Transition Conditions State transition 1 The secondary arm has completed the operation flow and is always in the IDLE state. State transition 2 The secondary arm completes the state machine initialization and immediately switches to the APPROACH_HOME state. State transition 3 The secondary arm has run to the initial pose and exited the compliant gripping operation, switching to the IDLE state. State transition 4 The secondary arm is running to the initial pose. State transition 5 The secondary arm has a timeout error and immediately switches to the ERROR state. State transition 6 The secondary arm has run to the initial pose and immediately switches to the TRACK_TARGET state. State transition 7 The secondary arm tracks and approaches the handle of the operation target based on the visual servoing algorithm. State transition 8 The secondary arm has a timeout error and immediately switches to the ERROR state. State transition 9 The secondary arm switches to the COMPENSATE state after the tool gripper closes to grip the target handle. State transition 10 The secondary arm implements compliant gripping of the target based on end-effector force sensor data. State transition 11 The secondary arm exits the operation and immediately switches to the APPROACH_HOME state. Table 2 Explanation of Main Arm Status Transition Conditions State transition 1 The secondary arm state transition 1 is similar. State transition 2 Similar to the secondary arm state transition 2, immediately switch to the APPROACH_TOOLBOX state. State transition 3 The primary arm runs to the toolbox pose. State transition 4 The primary arm end-effector tool tracking error converges, immediately switching to the GRASP_TOOL state. State transition 5 The primary arm tracks and approaches the screw based on the visual servoing algorithm. State transition 6 The primary arm tool gripper closes to grab the screw and switches to the LEAVE_TOOLBOX state. State transition 7 The primary arm is holding the screw and lifting it a certain distance in the vertical direction. State transition 8 The primary arm end-effector tool tracking error converges, immediately switching to the APPROACH_HOME state. State transition 9 The primary arm is running to the initial pose. State transition 10 The primary arm begins to approach the operation target and switches to the TRACK_TARGET state. State transition 11 Similar to the secondary arm state transition 3. State transition 12 The primary arm tracks and approaches the screw hole of the operation target based on the visual servoing algorithm. State transition 13 The primary arm end-effector tool tracking error converges, immediately switching to the INSERT state. State transition 14 The primary arm performs screw pre-insertion. State transition 15 The primary arm end-screw pre-insertion error converges, immediately switching to the SCREW state. State transition 16 The primary arm gradually screws in the screw. State transition 17 The primary arm screws in the screw and immediately switches to the PULL state. State transition 18 The primary arm retracts the end-effector tool. State transition 19 The primary arm end-effector tool retraction error converges, immediately switching to the APPROACH_HOME state. Remaining state transitions The primary arm is in the current state and has a timeout error, immediately switching to the ERROR state.

Claims

1. A control system for a mobile dual-arm robot, characterized in that, It includes a mobile base subsystem, a main and auxiliary dual-arm subsystem, and a control subsystem. The mobile base subsystem includes a Mecanum wheel base chassis and a control box. The base chassis is driven by a set of four parallel Mecanum wheels, enabling omnidirectional planar movement. The control box under the base chassis houses a battery, a switching power supply, a Mecanum wheel motor driver, an Ethernet switch, an Ethernet-CAN protocol conversion module, a tool gripper control box, and an onboard computer. The main and auxiliary dual-arm subsystem includes two 6-DOF robotic arms (main and auxiliary), a vision camera, a six-dimensional force sensor, and a tool gripper. The main and auxiliary dual-arm bases are fixed to the base chassis, and their ends are respectively fixed to the vision camera, the six-dimensional force sensor, and the tool gripper via adapter flanges. The control subsystem is based on the ROS2 Iron development environment on the Ubuntu 22.04 platform, and includes a camera image processing module, a ROS2 controller framework, a MoveIt! Servo framework, a robotic arm servo drive module, and a core operation flow control module. The camera image processing module receives image data through the camera's ROS2 driver node and identifies and calculates the relative pose of Apriltag targets in the image through the Apriltag_ros image processing node. The ROS2 controller framework can publish the desired angle data for each joint of the robotic arm and simultaneously receive the actual state data of each joint. The MoveIt! Servo framework uses the six-dimensional velocity command data stream of the robotic arm's end effector as control input, generates smooth joint position commands, and sends them to the robotic arm joint motors to control the robotic arm's movement. The robotic arm servo drive module packages the desired angle data of each joint of the robotic arm into UDP packets for transmission and simultaneously parses the received UDP packets into the actual state data of each joint of the robotic arm. The core operation process control module uses a state machine mechanism to manage the behavior and state switching process of all motion modes of the main and auxiliary arms, ensuring that the two arms can work together to complete the screw assembly operation task.

2. A dual-arm master-slave operation coordination control method based on visual / force servoing, characterized in that, This invention comprises visual servo target tracking based on images from the end-effector camera and force-sensory servo compliant operation control based on information from a six-dimensional force sensor. It is used in the mobile main-auxiliary dual-arm robot control system of claim 1 to perform screw assembly operations on a moving target while the base is in motion. During the target pre-grabbing phase, both the main arm and the auxiliary arm are in visual servo motion mode, and the end-effector camera provides image information for the robot arm's target tracking and approach process. In the combined operation phase after the tool gripper grasps the target, the main arm and the auxiliary arm respectively perform force-sensory compliant assembly and compliant clamping operations based on the end-effector force sensor information. The process includes the following steps: Step 1: Perform kinematic modeling on the mobile dual-arm system, export the model file, and load it into ROS2 to build the robot model. Step 2: Power on the system and initialize the software and hardware. Step 3: In the camera image processing module, the end-effector camera driver node of the main and auxiliary dual arms acquires image data, and the Apriltag_ros image processing node calculates the relative pose of the Apriltag target based on the image data. Step 4: The moving base performs trajectory tracking motion, and feedback control is performed based on the two-dimensional position of the base and the trajectory of the desired position. Step 5: Move both arms to track moving targets using a visual servoing algorithm, and calculate the target system ∑. io 、Tool system ∑ it With the expected system of instruments ∑ id The pose is calculated and the tool system ∑ is generated based on the control law. it The six-dimensional speed commands, controlled by MoveIt! Servo, enable the robotic arm to move, thus enabling the tool system Σ it Tendency towards the expected system Σ id . Step 6: The dual arms move based on the pose servo algorithm, setting a static target pose in the workspace and using it as the tool expectation system ∑. id The pose is controlled using a method similar to step 5, which controls the movement of the robotic arm to make the tool system ∑ it Motion and approaching the system ∑ id . Step 7: After the auxiliary arm gripper closes and holds the target handle, the system enters the combined operation phase. Based on the measurement information from the end force sensor, the system performs compliant gripping control of the target to ensure that the contact force of the combined system is within a reasonable range. Step 8: In the core operation process control module, the main and auxiliary arms cooperate to perform screw assembly operations according to the preset state machine process. Step 9: The operation task is completed, the system stops running, and the power is cut off.

3. The dual-arm master-slave operation coordination control method based on visual / force servoing according to claim 2, characterized in that, Step 1, the process of modeling the mobile dual-arm system and loading it into ROS2 to establish a robot model, includes: Step 1.1: Use the DH method to perform kinematic modeling of the system. Step 1.2: Export the established mobile dual-arm model as URDF and the corresponding mesh file, and perform a series of robot configurations in the ROS2 environment. Step 1.3: Integrate the ROS2 controller functionality into the model file. Step 1.4: Configure and run MoveIt! to check if the robot system visualization model is working properly.

4. The dual-arm master-slave operation coordination control method based on visual / force servoing according to claim 2, characterized in that, In step 5, the process of moving the dual arms to track moving targets based on a visual servoing algorithm includes: Step 5.1, obtain the tool system ∑ it Relative to the base system Σ B pose matrix Tool System Σ it Relative to the expected system Σ id pose matrix Target system Σ io Compared to the tool system Σ it Position vector (represented in system Σ) it middle) Step 5.2, Calculate the matrix Axis-angle parameters (θ) i u i ). Step 5.3: Calculate the tool system Σ using the control law. it Linear velocity and angular velocity control quantities Step 5.4: Calculate the system Σ through coordinate transformation. it Linear velocity and angular velocity control quantities Composition system Σ B Lower series Σ it Six-dimensional generalized velocity expectation Step 5.5, will The six-dimensional velocity command at the end point is input in real time to the MoveIt! Servo node of arm i. Step 5.6: The main and auxiliary arms simultaneously execute steps 5.1-5.5 in a loop, controlling the movement of each joint through the independent MoveIt! Servo node running threads to achieve visual servoing function.

5. The dual-arm master-slave operation coordination control method based on visual / force servoing according to claim 2, characterized in that, In step 6, the process of the two arms moving based on the pose servo algorithm includes: Step 6.1, Obtain the current pose matrix Step 6.2, similar to step 5.2, calculate the matrix. Axis-angle parameters (θ) i u i ). Step 6.3, similar to step 5.3, calculates the system ∑ it Linear velocity and angular velocity control quantities Step 6.4, similar to step 5.4, calculates the system ∑. it Expected speed Step 6.5, similar to steps 5.5-5.6, sets the desired speed. As the end-effector's six-dimensional velocity command, it is input in real time to the MoveIt! Servo node of arm i to drive the movement of each joint.

6. The dual-arm master-slave operation coordination control method based on visual / force servoing according to claim 2, characterized in that, In step 7, the process of the auxiliary arm performing target compliant clamping control based on force sensor data includes: Step 7.1: Obtain the actual force measurement value of the force sensor under the current configuration of the auxiliary arm using the gravity compensation algorithm under static conditions. s f TF and torque measurement value s m TF (represented in this system Σ) S middle). Step 7.2, obtain the system Σ S Relative system Σ B pose matrix B R S The coordinate transformation is used to obtain the representation in the base system ∑ B Forces in B f TF and torque B m TF . Step 7.3: The auxiliary arm adjusts the movement of the robotic arm based on the measured value of the actual applied force. Its underlying control mode is consistent with that in Step 5, according to Step 7.

2. B f TF , B m TF The results of each component after dead zone processing B f TF(dz) , B m TF(dz) Calculate ∑ lt Linear velocity and angular velocity control quantities of the system Step 7.4, similar to steps 5.5-5.6, sets the desired speed. The six-dimensional velocity command at the end of the arm is input in real time to the MoveIt! Servo node of the auxiliary arm to drive the movement of the auxiliary arm, so that its end moves in the direction of the applied force / torque, thereby weakening the applied force.

7. The dual-arm master-slave operation coordination control method based on visual / force servoing according to claim 2, characterized in that, In step 8, the screw-tightening operation of the main arm is carried out in the following manner: Step 8.1, set the starting position of the main boom screw-tightening operation in the workspace. rd pose matrix B R rD(ini) . Step 8.2: The main arm stops moving. Step 8.3, the main arm identifies the target system Σ with the identification code on the hexagonal screw cap. ro Corresponding system Σ rd pose matrix B R rD , in turn, the system ∑ rd After rotating around its own x-axis by 60°, 120°, 180°, 240°, and 300°, a total of 6 alternative posture matrices for grasping the hexagonal screw cap are obtained. Step 8.4: Calculate the matrix of each candidate pose sequentially. B R rD(i) (i=0, 1, 2, 3, 4, 5) and B R rD(ini) The difference is expressed by the attitude error function Ψ E (i) is used to measure. Step 8.5, the function Ψ in step 8.4 E (i) The minimum value corresponds to B R rD(i) As a system ∑ rd The attitude matrix is ​​substituted into the visual servoing algorithm of the main arm in step 5 to perform visual tracking of the hexagonal screw cap identification code target. Step 8.6: Close the main arm jaws to hold the hexagonal nut. Step 8.7, similar to step 5.4, will The six-dimensional velocity command input from the end effector is given to the MoveIt! Servo node of the main arm, controlling the end effector to move at an angular velocity ω. screw Around the tool system ∑ rt The x-axis rotates in the positive direction, causing the screw to be screwed into the screw hole. Step 8.8: After the main arm gripper has been screwed into the screw in the forward direction for a period of time, stop the movement and open the gripper, then loosen the screw cap. Step 8.9, similar to step 8.7, controls the end effector of the main arm to move at an angular velocity ω. screw Around the tool system ∑ rt After rotating the x-axis in the opposite direction for a period of time to restore the main arm to its initial position, proceed to step 8.

2. Repeat steps 8.2-8.9 until the screw is fully screwed into the screw hole, and the screw-tightening operation is complete.

Images