Visual servo control method and system for tightly coordinating and assembling multi-shaft-hole parts through multiple mechanical arms

Abstract

The invention discloses a visual servo control method and system for assembling multi-shaft-hole parts through multiple mechanical arms in a tight coordination mode, and belongs to the field of multi-robot cooperative control. The invention aims to solve the problems of visual feature extraction and collaborative assembly control in the task of tightly coordinating and assembling large-size multi-shaft-hole parts by multiple mechanical arms. The method is technically characterized in that a shaft hole assembly task image data set containing different hole shapes is established and used for training a positioning neural network and a relative state estimation neural network, and a state classification neural network is used for dividing the states of shafts and holes in images into three types of shielding, separation and overlapping; the positioning neural network is used for determining the positions of the shaft and the hole in the image. A multi-mechanical-arm visual feature fusion method based on the virtual acting force is provided, the virtual acting force acting on an object is calculated according to position information and relative state information of a shaft and a hole in an image, and the virtual acting force is input into a cooperative controller of the multiple mechanical arms to achieve cooperative assembly movement of the multiple mechanical arms. And natural combination of visual features and a multi-mechanical-arm cooperative controller is achieved. Finally, the method provided by the invention is verified in the task of tightly coordinating and assembling the double-shaft-hole parts by the double mechanical arms with the assembling clearance of 0.2 mm, the success rate reaches 85%, and the effectiveness of the method is verified.

Description

Technical Field

[0001] The present invention relates to a vision servo control framework for multi-robot collaborative assembly of multi-axis hole parts, belonging to the technical field of multi-robot collaborative control. Background Art

[0002] In recent years, robots have played an increasingly important role in automatic assembly. Multi-axis hole assembly is a basic operation among them, and robot multi-axis hole assembly technology is involved in tasks ranging from on-orbit assembly of large space structures to assembly of small electronic devices. Current multi-axis hole assembly methods usually focus on single-robot assembly of small parts. As the size of the parts to be assembled increases, it is difficult for a single robotic arm to simultaneously align multiple axles and holes that are far apart, and multi-robotic arm tightly coupled collaborative assembly is required.

[0003] Axis-hole assembly can generally be divided into two stages: hole searching and insertion. In the hole searching stage, it is necessary to eliminate positioning uncertainties to align the shaft and the hole. The insertion stage occurs after the shaft and the hole are aligned, and the assembly is completed compliantly while avoiding jamming. The present invention mainly focuses on the hole searching stage. For single-axis hole assembly tasks, there are already many methods for actively eliminating positioning errors, such as reinforcement learning, behavior cloning, finite state machine controllers, environmental attraction domains, etc. These methods usually need to guide the hole searching process based on the contact force information between the shaft and the hole. However, in the multi-axis hole assembly task of large-sized parts, the contact force information cannot provide guidance for hole searching. Compared with the above methods, blind search and vision servo do not require contact force to guide the search, but the blind search method is inefficient and time-consuming, and the classical vision servo method is sensitive to background, lighting, and shape.

[0004] For example, the prior art with the document number CN114918637B discloses a visual positioning method for a shaft-hole assembly robot. When the robot operates, it performs actions according to a matching template, adjusts its position according to the coordinates of the initial teaching, corrects the displacement and rotation angle differences with multiple coordinate systems, realizes the visual positioning guidance for the robot to automatically assemble the shaft hole, performs adaptive exposure adjustment, improves the operation efficiency and success rate, fully tracks and records data, and the quality inspection is traceable. The prior art with the document number CN118744426A discloses a human-computer interaction assembly method and system based on a multimodal large model and reinforcement learning, which includes the following steps: a data acquisition module acquires assembly environment information and text instructions, and the text instructions include text instructions generated by converting voice instructions and text instructions directly input by the user; a multimodal processing module performs multimodal fusion on the assembly environment information and text instructions to generate multimodal information, and inputs the multimodal information and the completed tasks into a vision-language large model; the vision-language large model outputs new tasks according to the multimodal information and the completed tasks, and then divides the new tasks into several subtasks; a computer control system uses a control instruction library to extract corresponding instructions according to the subtask order and sequentially execute the corresponding instructions. It can more comprehensively understand and process complex information in the assembly work environment, enabling the large model to meet the increasingly complex assembly task requirements.

[0005] In recent years, learning-based visual servoing has demonstrated strong generalization performance through domain randomization techniques. Compared with single robotic arm assembly tasks, the visual servoing system for multi-robotic arm multi-shaft-hole assembly tasks needs to solve the problem of fusing visual features of multiple robotic arms. In addition, during the hole searching process, the cooperative controller of multiple robotic arms not only needs to control the contact force between the shaft and the hole, but also needs to constrain the internal forces exerted by multiple robotic arms on the object. If the visual features of multiple robotic arms and the cooperative controller of multiple robotic arms can be combined, no one in the current prior art has proposed this problem, nor has a solution been given for the visual servo control framework for tightly coordinated multi-robotic arm assembly of multi-shaft-hole parts. Summary of the Invention

[0006] The technical problem to be solved by the present invention is:

[0007] Aiming at the problems of visual feature extraction and cooperative assembly control in the task of tightly coordinated multi-robotic arm assembly of large-size multi-shaft-hole parts, the present invention proposes a visual servo control method and system (multi-robotic arm tightly coordinated visual servo control framework) for tightly coordinated multi-robotic arm assembly of multi-shaft-hole parts based on virtual forces, and uses tightly coordinated multi-robotic arms to achieve high-precision assembly of multi-shaft-hole parts.

[0008] The technical solution adopted by the present invention to solve the above technical problems is:

[0009] A visual servo control method for tightly coordinated multi-robotic arm assembly of multi-shaft-hole parts, including the following steps:

[0010] Step 1: Use the image dataset of the shaft-hole assembly task to train the positioning neural network and the relative state estimation neural network respectively;

[0011] Step 2: Use the positioning neural network to extract the position information of the shaft / hole from the hand-eye camera images of each robotic arm, and use the relative state estimation neural network to extract the relative state information between the shaft and the hole from the hand-eye camera images of each robotic arm;

[0012] Step 3: Use the position information of the shaft / hole and the relative state information between the shaft and the hole to calculate the virtual force acting on the object;

[0013] Step 4: Input the virtual force into the cooperative controller of the multi-robotic arm to realize the cooperative assembly motion of the multi-robotic arm;

[0014] Step 5: Repeat Steps 2 to 4. The cooperative controller of the multi-robotic arm controls the multi-robotic arm to align the shaft and hole of the multi-shaft parts grasped with the multi-hole parts to be assembled one by one, and complete the assembly task.

[0015] Furthermore, the "image dataset of the shaft-hole assembly task" described in Step 1 contains holes of different shapes, and the positions of the shaft and the hole in the image coordinate system are stored in the form of a heatmap for training the positioning neural network;

[0016] Furthermore, the calculation formula of the "heatmap" is as follows:

[0017]

[0018] In the formula, p I represents any point in the image coordinate system, is the target point corresponding heatmap, and σ represents the standard deviation.

[0019] Furthermore, the content of the "relative state information between the shaft and the hole" described in Step 2 is as follows:

[0020] The relative states between the shaft and the hole are divided into three categories, namely occlusion, overlap, and separation; when in the occlusion state, the shaft is in front of the hole and the hole is partially or completely occluded by the shaft; when in the overlap state, the shaft is behind the hole and the hole is partially occluded by the shaft; when in the separation state, the hole is not occluded by the shaft at all.

[0021] Furthermore, the implementation process of the "calculating the virtual force acting on the object" described in Step 3 is as follows:

[0022] Step 3-1: According to the positions of the shaft and the hole in the image coordinate system recognized by the positioning neural network and Calculate the position vectors of the shaft and the hole in the camera coordinate system using the imaging model of the camera and

[0023] Step 3-2: Calculate and obtain and The unit direction vectors in the world coordinate system and

[0024] Step 3-3: Use to represent the position of the origin of the camera coordinate system in the world coordinate system. Assume that the shaft is fixedly connected to the end of the robotic arm, and estimate and obtain to the end point of the shaft The position vector Let the plane passing through and perpendicular to l be α i , and calculate the vectors and The intersection points with the plane α i respectively and l represents the unit vector of the axis direction of the hole;

[0025] Step 3-4: Calculate the deviation vector d of the shaft and the hole on the plane α i ; i ;

[0026] Step 3-5: Calculate the virtual acting forces F acting on each shaft i ;

[0027] Step 3-6: Calculate the virtual resultant force F and the resultant moment M acting on the multi-axis part;

[0028] Step 3-7: Combine the relative state information between the shaft and the hole, and calculate the virtual acting force (desired acting force) h acting on the multi-axis part d .

[0029] Furthermore, the calculation formulas for the "intersection points and of the vectors i with the plane α and " described in Step 3-3 are as follows:

[0030]

[0031] The calculation formula for the "deviation vector d i of the shaft and the hole on the plane α i " described in Step 3-4 is as follows:

[0032]

[0033] The "virtual acting force F acting on each axis" described in Step 35 i has the following calculation formula:

[0034] i represents the axis number;

[0035] The "virtual resultant force F and resultant moment M acting on the object" described in Step 36 have the following calculation formulas:

[0036]

[0037] In the formula, i represents the axis number; k M is the moment proportionality coefficient; F ε is a random term applied when the value of ∑F i is too small. The calculation formula of F ε is as follows;

[0038]

[0039] In the formula, x~Bernoulli(0.5), F s is the threshold for applying the random force, and k ε is used to adjust the magnitude of the random force;

[0040] The calculation process of the "desired acting force h d acting on the object" described in Step 37 is as follows:

[0041]

[0042] In the formula, 0 3 represents a 1x3 zero vector, represents the relative state of the axis hole in the image of the hand-eye camera of the robotic arm i; before contact occurs, if the images of the two hand-eye cameras are both in an overlapping state, then the object is moved along the axis direction for insertion, otherwise the axis is moved in the direction of reducing the error; after contact occurs, while moving the axis in the direction of reducing the error, a contact force of k F l T is maintained between the axis and the hole.

[0043] Furthermore, the "cooperative controller of multiple robotic arms" described in Step 4 consists of an object-level external force controller and an end-effector-level internal force controller. The object-level external force controller calculates the motion trajectory of the end-effector of the robotic arm according to the virtual acting force acting on the manipulated part, and the end-effector-level internal force control is used to achieve the compliant response of the internal force received by the manipulated part.

[0044] Furthermore, the specific process implemented in Step 4 is as follows:

[0045] Step Four One: Calculate the desired motion of the manipulated object using an object-level impedance controller. First, select a control reference point y fixed to the object, and calculate the reference acceleration of point y through the following control law:

[0046]

[0047] Where: represents the velocity of point y, is the reference acceleration of point y, respectively represent the desired object inertia matrix, damping matrix, and force feedback proportionality coefficient. All three matrices are selected as symmetric positive definite matrices, is the resultant force and resultant moment applied by the environment to the object; h c can be estimated by means of a force / torque sensor at the end of the robotic arm. Integrating gives the reference motion of the object consists of ;

[0048] Step Four Two: Calculate the desired motion of the end effector of the robotic arm. Assume that there is no relative motion between the end effector of the robotic arm and the object. Then, the desired motion of the end effector of the robotic arm and the reference motion of the object satisfy the following constraint:

[0049]

[0050] Where r yi = r i - r y . Using to represent the desired velocity of the end effector of the robotic arm, to represent the desired acceleration of the end effector of the robotic arm, together form the desired motion of the end effector of the robotic arm;

[0051] Step Four Three: Use an end effector-level impedance controller to calculate the robotic arm joint angular velocity control command and control the internal force acting on the object. The following end effector-level impedance controller is adopted:

[0052]

[0053] Where: respectively represent the desired inertia matrix, damping matrix, and stiffness matrix of the end of the robotic arm, and are also selected as symmetric positive definite matrices; represents the difference between the actual pose and the desired pose of the end of the i-th robotic arm, represents the difference between the actual velocity and the desired velocity of the end of the i-th robotic arm, represents the difference between the reference acceleration and the desired acceleration of the end of the i-th robotic arm, It represents the partial components of the force / moment applied by the end of the robotic arm that generate the internal force / moment of the object. is the attitude transformation matrix. After transformation, the above variables are all represented in the desired pose coordinate system ; Using the internal force / moment component h Ii , the desired motion of the end coordinate system of the robotic arm The actual state information p of the end coordinate system of the robotic arm i , R i , ω i and the impedance parameters to calculate the reference acceleration of the end coordinate system of the robotic arm together For Integrate to obtain the reference velocity of the end coordinate system of the robotic arm Use the Jacobian matrix of the robotic arm to calculate the reference joint angular velocity:

[0054]

[0055] In the formula, q i ∈R 7 represents the joint angle vector of the robotic arm (taking a 7-degree-of-freedom robotic arm as an example), is the pseudo-inverse of the Jacobian matrix of the robotic arm; is used as the control command for the joint servo motor of robotic arm i.

[0056] A visual servo control method and system for tightly coordinating the assembly of multi-axis hole parts by multiple robotic arms. The system has program modules corresponding to the steps of the above method and executes the steps in the visual servo control method for tightly coordinating the assembly of multi-axis hole parts by multiple robotic arms when running.

[0057] A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, it implements the visual servo control method for tightly coordinating the assembly of multi-axis hole parts by multiple robotic arms.

[0058] The beneficial technical effects of the present invention are:

[0059] The present invention establishes an image dataset for shaft-hole assembly tasks containing different hole shapes, which is used to train a positioning neural network and a relative state estimation neural network. The state classification neural network is used to classify the states of the shaft and the hole in the image into three categories: occlusion, separation, and overlap. The positioning neural network is used to determine the positions of the shaft and the hole in the image. A multi-robot visual feature fusion method based on virtual forces is proposed. The virtual forces acting on the objects are calculated using the position information and relative state information of the shaft and the hole in the image, and the virtual forces are input into the cooperative controller of the multi-robot to achieve the cooperative assembly movement of the multi-robot, realizing the natural combination of multi-robot visual features and the multi-robot cooperative controller. It effectively solves the problems of visual feature extraction and cooperative assembly control in the task of multi-robot tightly coordinated assembly of large-size multi-shaft-hole parts. Finally, the method proposed in the present invention is verified in the task of tightly coordinated assembly of a double-shaft-hole part by a dual-robot with an assembly gap of 0.2 mm, and the success rate reaches 85%, verifying the effectiveness of the proposed method. Using tightly coordinated multi-robots can achieve high-precision assembly of multi-shaft-hole parts, controlling the multi-robots to insert a multi-shaft part onto a multi-hole part, and the shafts on the multi-shaft part are assembled with the holes on the multi-hole part one by one.

[0060] The visual servo control method (framework) for multi-robot tightly coordinated assembly of multi-shaft-hole parts proposed by the present invention calculates the virtual forces acting on the manipulated parts by extracting the feature information of the hand-eye cameras of each robot arm, and uses the virtual forces to guide the cooperative movement of the robots to complete the assembly. The visual servo control framework proposed by the present invention can quickly reduce the positioning error only by using the monocular hand-eye cameras of each robot arm, and has lower cost and better flexibility compared with the technology using multi-cameras for positioning. The present invention proposes a new set of picture datasets for shaft-hole assembly tasks, and improves the performance of the neural network by changing the shapes of the shafts and holes in the datasets. The visual servo control framework proposed by the present invention successfully realizes the sub-millimeter-level multi-robot multi-shaft-hole assembly task, breaks through the limitations of the existing technology in the high-precision assembly task of large-size multi-shaft-hole parts, and has significant technical advantages and application value in the field of robot automatic precision assembly. Brief Description of the Drawings

[0061] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings. The accompanying drawings, together with the following detailed description, are included in this specification and form a part of this specification, and are further used to illustrate the preferred embodiments of the present invention and explain the principles and advantages of the present invention.

[0062] Figure 1 is the flowchart of the present invention;

[0063] Figure 2 are the models of 14 holes in the shaft-hole assembly task dataset and the corresponding assembly scenarios;

[0064] Figure 3 Schematic diagram of the main vector in the present invention;

[0065] Figure 4 Control system block diagram of the multi-manipulator cooperative controller in the present invention;

[0066] Figure 5 Robots and related equipment used in the embodiments of the present invention;

[0067] Figure 6 Shaft and hole to be assembled in the embodiments of the present invention;

[0068] Figure 7 Contact force between the shaft and the hole and displacement of the end effector of the manipulator in the hole axis direction in the embodiments of the present invention;

[0069] Figure 8 Virtual force acting on the object to be manipulated in the embodiments of the present invention;

[0070] Figure 9 Screenshot of the assembly process and outputs of the state classification neural network and the positioning neural network in the embodiments of the present invention. Detailed implementation manners

[0071] To enable those skilled in the art to better understand the solution of the present invention, the exemplary embodiments or examples of the present invention will be described below in conjunction with the appended Figures 1-9 Obviously, the described embodiments or examples are only part of the embodiments or examples of the present invention, rather than all of them. All other embodiments or examples obtained by those of ordinary skill in the art based on the embodiments or examples in the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0072] Step 1: Respectively train a positioning neural network and a relative state estimation neural network by using the image dataset of the shaft-hole assembly task;

[0073] Step 2: Extract the position information of the shaft / hole from the hand-eye camera images of each manipulator by using the positioning neural network, and extract the relative state information between the shaft and the hole from the hand-eye camera images of each manipulator by using the relative state estimation neural network. The position information of the shaft / hole is represented in the image coordinate system, and the relative state between the shaft and the hole is divided into three categories, namely occlusion, overlap, and separation. When in the occlusion state, the shaft is in front of the hole and the hole is partially or completely blocked by the shaft; when in the overlap state, the shaft is behind the hole and the hole is partially blocked by the shaft; when in the separation state, the hole is not blocked by the shaft at all;

[0074] Step 3: Calculate the virtual force acting on the object by using the position information of the shaft and the hole and the relative state information between the shaft and the hole;

[0075] Step 4: Input the virtual force into the cooperative controller of the multi-robot arms to achieve the cooperative assembly motion of the multi-robot arms. The cooperative controller of the multi-robot arms consists of two parts: an object-level external force controller and an end-effector-level internal force controller. The object-level external force controller calculates the motion trajectory of the end-effector of the robot arm according to the virtual force acting on the manipulated part, and the end-effector-level internal force control is used to achieve the compliant response of the internal force received by the manipulated part;

[0076] Step 5: Repeat Step 2 to Step 4. The cooperative controller of the multi-robot arms controls the multi-robot arms to align the axes and holes of the multi-axis parts grabbed with the multi-hole parts to be assembled one by one, and complete the assembly task.

[0077] The image dataset of the axis-hole assembly task described in Step 1 is rendered and generated in the simulation engine CoppeliaSim. To avoid the influence of the shape of the hole on the network performance during the assembly process, the dataset contains a total of 14 assembly scenarios with different hole models, as Figure 2 shown. By randomizing the color of the scene, the angle of the light, the pose of the camera, and the pose of the axis, 600 pictures are generated for each hole model, with 200 pictures in the occluded, overlapping, and separated states respectively. The positions of the axis and the hole in the image coordinate system can be automatically calculated and stored in the form of a heatmap for training the positioning neural network. If p I represents any point in the image coordinate system, then the heatmap corresponding to the target point can be expressed as:

[0078]

[0079] The specific content of the positioning neural network and the relative state estimation neural network described in Step 2 is as follows:

[0080] The positioning neural network selects the ResNetUNet network. The input processed by this network is an RGB image of 224×224×3, and the output is a double-layer heatmap. The mean square error loss function is used to train the neural network. If the positions of the axis and the hole in the image coordinate system are expressed as and then the output of the positioning neural network is expressed as a double-layer heatmap Finally, the estimated positions of the axis and the hole are:

[0081]

[0082] The state classification neural network selects the VGG16 network pre-trained on ImageNet. The input of this network is an RGB image of 224×224×3, and the output layer is changed from the original 1000 dimensions to 3 dimensions to adapt to the categories of the relative states of the shaft and hole. The neural network is trained using the cross-entropy loss function.

[0083] Step three includes the following steps:

[0084] Step 3-1: According to the positions of the shaft and hole in the image coordinate system recognized by the positioning neural network and Use the imaging model of the camera to calculate the position vectors of the shaft and hole in the camera coordinate system and The vectors described in the present invention are as Figure 3 shown;

[0085] Step 3-2: Calculate and obtain and The unit direction vectors in the world coordinate system and

[0086] Step 3-3: Use to represent the position of the origin of the camera coordinate system in the world coordinate system. Assuming that the shaft is fixedly connected to the end of the robotic arm, the position vector from to the shaft endpoint can be estimated as Set the plane passing through and perpendicular to l as α i , and calculate the intersection points of the vectors and with the plane α i respectively as and

[0087] Step 3-4: Calculate the deviation vectors of the shaft and hole on the plane α i :

[0088]

[0089] Step 3-5: Calculate the virtual force F acting on each shaft i :

[0090]

[0091] Step 3-6: Calculate the virtual resultant force F and resultant moment M acting on the object (the object refers to a multi-axis part grasped by multiple robotic arms and controlled by the cooperative controller of multiple robotic arms. The multi-axis part is used for insertion into a multi-hole part, and the shaft holes on the two correspond one by one):

[0092]

[0093] where k M is the torque proportionality coefficient. Since when ∑F i is too small, the object will no longer move under the action of friction. To solve this problem, when ∑F i is too small, a random term F ε is added to the resultant force.

[0094]

[0095] where x ~ Bernoulli(0.5), F s is the threshold for applying the random force, and k ε is used to adjust the magnitude of the random force.

[0096] Step 3: Combine the relative state information between the shaft and the hole to calculate the virtual acting force (desired acting force) h d on the multi-axis part. First, divide the assembly process into two stages: before the shaft-hole contact occurs and after the contact occurs. Use h d to represent the virtual resultant force and virtual resultant torque acting on the object, to represent the relative state of the shaft and the hole in the image of the hand-eye camera of the robotic arm i. Under different assembly stages and relative states of the shaft and the hole, the value of h d is as follows:

[0097]

[0098] 0 3 represents a 1x3 zero vector. Before the contact occurs, if the images of both hand-eye cameras are in an overlapping state, the object is inserted along the axis direction; otherwise, the shaft moves in the direction to reduce the error. After the contact occurs, while the shaft moves in the direction to reduce the error, a contact force of k F l T is maintained between the shaft and the hole.

[0099] Step 4 includes the following steps:

[0100] Step 4-1: Use the object-level impedance controller to calculate the desired motion of the manipulated object. First, select a control reference point y fixed to the object, and calculate the reference acceleration of point y through the following control law:

[0101]

[0102] where: represents the velocity of point y, is the reference acceleration of point y, respectively represent the desired object inertia matrix, damping matrix, and force feedback proportionality coefficient. All three matrices are selected as symmetric positive definite matrices. is the resultant force and resultant moment exerted by the environment on the object. h c can be estimated by means of a force / torque sensor at the end of the robotic arm. For integral calculation to obtain the reference motion of the object consists of .

[0103] Step Four Two: Calculate the desired motion of the end effector of the robotic arm. Assume that there is no relative motion between the end effector of the robotic arm and the object. Then the desired motion of the end effector of the robotic arm and the reference motion of the object satisfy the following constraints:

[0104]

[0105] where r yi = r i - r y . Use to represent the desired velocity of the end effector of the robotic arm, to represent the desired acceleration of the end effector of the robotic arm, which together constitute the desired motion of the end effector of the robotic arm.

[0106] Step Four Three: Use the end effector-level impedance controller to calculate the robotic arm joint angular velocity control command. To control the internal force exerted on the object, the following end effector-level impedance controller is adopted:

[0107]

[0108] where: respectively represent the desired inertia matrix, damping matrix, and stiffness matrix of the end of the robotic arm, and are also selected as symmetric positive definite matrices. represents the difference between the actual pose and the desired pose of the end of robotic arm i, represents the difference between the actual velocity and the desired velocity of the end of robotic arm i, represents the difference between the reference acceleration and the desired acceleration of the end of robotic arm i, represents the partial component of the force / torque exerted by the end of the robotic arm on the object that generates the internal force / torque of the object. is the attitude transformation matrix. After transformation, the above variables are all represented in the desired pose coordinate system . Using the internal force / torque component h Ii , the desired motion of the end coordinate system of the robotic arm the actual state information p i , R i , ω i and the impedance parameters Calculate the reference acceleration of the end - effector coordinate system of the robotic arm together For Integrate to obtain the reference velocity of the end - effector coordinate system of the robotic arm Calculate the reference joint angular velocity using the Jacobian matrix of the robotic arm:

[0109]

[0110] In the formula, q i ∈R 7 represents the joint angle vector of the robotic arm (taking a 7 - degree - of - freedom robotic arm as an example), is the pseudo - inverse of the Jacobian matrix of the robotic arm. As the control command of the joint servo motor of the i - th robotic arm.

[0111] In step five, repeat steps two to four. The cooperative controller of the multi - robotic arms controls the multi - robotic arms to align the axes and holes of the multi - axis parts grasped and the multi - hole parts to be assembled one by one, and complete the assembly task. When all the corresponding axes and holes are aligned, the assembly ends. If the assembly time exceeds the predetermined time or the internal and external forces on the object exceed the set threshold, human intervention or other means can be used for processing.

[0112] Embodiment:

[0113] The following combines the embodiment and the attached Figures 1-9 to illustrate the implementation process of the present invention. As shown in the flowchart Figure 1 The specific steps include:

[0114] Step one: Use the image dataset of the axis - hole assembly task to train the positioning neural network and the relative state estimation neural network respectively. The image dataset of the axis - hole assembly task is rendered and generated in the simulation engine CoppeliaSim. To avoid the influence of the shape of the hole on the network performance during the assembly process, the dataset contains a total of 14 assembly scenarios with different hole models, as Figure 2 shown. By randomizing the color of the scene, the angle of the light, the pose of the camera, and the pose of the axis, 600 pictures are generated for each hole model, among which 200 pictures are in the occluded, overlapping, and separated states respectively. The positions of the axis and the hole in the image coordinate system can be automatically calculated and stored in the form of a heatmap for training the positioning neural network. If p I represents any point in the image coordinate system, then the heatmap corresponding to the target point can be expressed as:

[0115]

[0116] Step 2: Use the positioning neural network to extract the position information of the shaft / hole from the hand-eye camera images of each robotic arm, and use the relative state estimation neural network to extract the relative state information between the shaft and the hole from the hand-eye camera images of each robotic arm. The position information of the shaft / hole is represented in the image coordinate system, and the relative state between the shaft and the hole is divided into three categories: occlusion, overlap, and separation. When in the occlusion state, the shaft is in front of the hole and the hole is partially or completely occluded by the shaft; when in the overlap state, the shaft is behind the hole and the hole is partially occluded by the shaft; when in the separation state, the hole is not occluded by the shaft at all.

[0117] The positioning neural network selects the ResNetUNet network. The input processed by this network is an RGB image of 224×224×3, and the output is a double-layer heat map. The neural network is trained using the mean squared error loss function. The positions of the shaft and the hole in the image coordinate system are represented as and Then the output of the positioning neural network is represented as a double-layer heat map Finally, the estimated positions of the shaft and the hole are:

[0118]

[0119] The state classification neural network selects the VGG16 network pre-trained on ImageNet. The input of this network is an RGB image of 224×224×3, and the output layer is changed from the original 1000 dimensions to 3 dimensions to adapt to the categories of the relative state between the shaft and the hole. The neural network is trained using the cross-entropy loss function.

[0120] Step 3: Use the position information of the shaft / hole and the relative state information between the shaft and the hole to calculate the virtual force acting on the object. The specific steps are as follows:

[0121] Step 3-1: Assume that the number of robotic arms is m. According to the positions of the shaft and the hole in the image coordinate system recognized by the positioning neural network and (i = 1, 2, …, m), use the imaging model of the camera to calculate the position vectors of the shaft and the hole in the camera coordinate system and The said vectors are as Figure 3 shown. The calculation formula is as follows:

[0122]

[0123]

[0124] In the formula, α x and α y are proportionality coefficients, through which the pixel coordinates can be converted into metric units.

[0125] Step 3-2: Calculate and unit direction vector in the world coordinate system and The calculation formula is as follows:

[0126]

[0127] In the formula, is the attitude rotation matrix of the camera coordinate system relative to the world coordinate system, and the unit vector in the direction of the axis of the hole is denoted as l.

[0128] Step 33: Use to represent the position of the origin of the camera coordinate system in the world coordinate system. Assuming that the axis and the end of the robotic arm are fixedly connected, the position vector from to the end point of the axis can be estimated. Let the plane passing through and perpendicular to l be α i , and calculate the intersection points and of the vectors i and with the plane α respectively. The calculation formula is as follows:

[0129]

[0130] Step 34: Calculate the deviation vector of the axis and the hole on the plane α i :

[0131]

[0132] Step 35: Calculate the virtual force F i acting on each axis respectively:

[0133]

[0134] Step 36: Calculate the virtual resultant force F and resultant moment M acting on the object:

[0135]

[0136] k M is the moment proportionality coefficient. Since when the value of ∑F i is too small, the object will no longer move under the action of friction. To solve this problem, when the value of ∑F i is too small, a random term F ε is added to the resultant force.

[0137]

[0138] where \(x\sim Bernoulli(0.5)\), \(F\) s is the threshold for applying the random force, and \(k\) ε is used to adjust the magnitude of the random force. represents the direction of the random force. Since the random force is usually applied in the occluded state, set to be the direction of motion in front of the axial field of view, that is:

[0139]

[0140] where can be regarded as the projection of the vector on the plane \(\alpha\) i .

[0141] Step 3: Combine the relative state information between the shaft and the hole to calculate the expected force acting on the object. First, divide the assembly process into two stages: before the shaft-hole contact occurs and after the contact occurs. Let \(h\) d represent the virtual resultant force and virtual resultant moment expected to act on the object, represent the relative state of the shaft and the hole in the image of the hand-eye camera of the robotic arm \(i\). In different assembly stages and relative states of the shaft and the hole, the value of \(h\) d is as follows:

[0142]

[0143] 0 3 represents a \(1\times3\) zero vector. Before the contact occurs, if the images of both hand-eye cameras are in an overlapping state, the object is inserted along the axis direction; otherwise, the shaft moves in the direction to reduce the error. After the contact occurs, while the shaft moves in the direction to reduce the error, a contact force of \(k\) F \(l\) T is maintained between the shaft and the hole.

[0144] Step 4: Input the virtual force into the cooperative controller of the multi-robotic arms to achieve the cooperative assembly motion of the multi-robotic arms. The cooperative controller of the multi-robotic arms consists of two parts: the external force controller at the object level and the internal force controller at the end effector level. The external force controller at the object level calculates the motion trajectory of the end effector of the robotic arm according to the virtual force acting on the manipulated part, and the internal force control at the end effector level is used to achieve the compliant response of the internal force received by the manipulated part. The system block diagram of the cooperative controller of the multi-robotic arms is as Figure 4 shown. The specific steps are as follows:

[0145] Step 4.1: Use the impedance controller at the object level to calculate the expected motion of the manipulated object. First, select a control reference point \(y\) fixed to the object, and calculate the reference acceleration of point \(y\) through the following control law:

[0146]

[0147] In the formula: represents the velocity of point y, is the reference acceleration of point y, respectively represent the desired object inertia matrix, damping matrix, and force feedback proportionality coefficient. All three matrices are selected as symmetric positive definite matrices, is the resultant force and resultant moment exerted on the object by the environment. h c can be estimated by means of a force / torque sensor at the end of the robotic arm. For Integrating and calculating gives the reference motion of the object consists of .

[0148] Step Four Two: Calculate the desired motion of the end effector of the robotic arm. Assume that there is no relative motion between the end effector of the robotic arm and the object. Then the desired motion of the end effector of the robotic arm and the reference motion of the object satisfy the following constraints:

[0149]

[0150] In the formula, r yi = r i - r y . Using to represent the desired velocity of the end effector of the robotic arm, represents the desired acceleration of the end effector of the robotic arm, together constitute the desired motion of the end effector of the robotic arm.

[0151] Step Four Three: Use the end effector level impedance controller to calculate the robotic arm joint angular velocity control command. In order to control the internal force received by the object, the following end effector level impedance controller is adopted:

[0152]

[0153] In the formula: respectively represent the desired end inertia matrix, damping matrix, and stiffness matrix of the robotic arm, and are also selected as symmetric positive definite matrices. represents the difference between the actual pose and the desired pose of the end of robotic arm i, represents the difference between the actual velocity and the desired velocity of the end of robotic arm i, represents the difference between the reference acceleration and the desired acceleration of the end of robotic arm i, represents the partial component of the force / torque exerted by the end of the robotic arm on the object that generates the internal force / moment of the object. is the attitude transformation matrix. After transformation, the above variables are all represented in the desired pose coordinate system . The calculation formula is as follows:

[0154]

[0155] Where: represents the corresponding Euler angles. Using the internal force / moment component h Ii , the desired motion of the end - effector coordinate system of the robotic arm the actual state information p of the end - effector coordinate system of the robotic arm i , R i , ω i and the impedance parameters to calculate the reference acceleration of the end - effector coordinate system of the robotic arm together Integrate to obtain the reference velocity of the end - effector coordinate system of the robotic arm Calculate the reference joint angular velocity using the Jacobian matrix of the robotic arm:

[0156]

[0157] Where, q i ∈R 7 represents the joint angle vector of the robotic arm (taking a 7 - degree - of - freedom robotic arm as an example), is the pseudo - inverse of the Jacobian matrix of the robotic arm. is used as the control command for the joint servo motor of the i - th robotic arm.

[0158] Step Five: Repeat Step Two to Step Four until the assembly task is completed, aligning all the corresponding shafts and holes.

[0159] Verify the beneficial effects of the present invention with the following embodiments:

[0160] 1) Experimental task

[0161] Use two kinova jaco2 robotic arms to conduct an assembly experiment. As Figure 5 shown, a 6 - dimensional force / torque sensor and a monocular hand - eye camera are installed at the end of each robotic arm. The shaft and hole to be assembled are as Figure 6 shown. The inner diameter of the hole is 20.2 mm, and the diameter of the shaft is 20 mm. At a height of 1 cm from the surface of the hole, two initial positions of the shaft are randomly selected within the range of the hole center, generating a total of 20 initial poses of the object. If the assembly cannot be completed within the time of 40 actions (about 100 s), the image dataset of the shaft - hole assembly task or a replacement dataset needs to be used to retrain the positioning neural network and the relative state estimation neural network respectively, or adjust the relative position relationship of the relevant hardware.

[0162] 2) Training process

[0163] Using the images of 7 holes as the training set, and the remaining images of 7 holes for testing. Select 300 images from the 600 images of each scenario for training (100 images for occlusion, separation, and overlap respectively), and the remaining images for testing. Finally, a total of 2100 images are used for training. To avoid the influence of background clutter on the model, during the training process, natural images are randomly selected from the MSCOCO dataset, and then the natural images are randomly rotated by an angle and superimposed on the alpha channel of the simulation image for enhancement. On this basis, ISO noise, Gaussian noise, and Gaussian blur are added to the image, and the brightness, contrast, and saturation of the image are randomly changed, and the image is randomly horizontally flipped with a probability of 50%. Each model is trained for 30 epochs, the batch size is 8, the maximum learning rate is 0.001, the weight decay is 0.0001, and the optimizer is Adam. The training is carried out on an NVIDIA GeForce RTX 3090. The training time of the state classification network is about 18 minutes, and the training time of the localization network is about 15 minutes.

[0164] 3) Experimental results and analysis

[0165] The success rate and average time of 20 assembly experiments are statistically analyzed. The assembly success rate is 85% and the average time is 48.18 s. It can be seen that the method of the present invention can complete the assembly with a high success rate in a short time. In Figure 7 shows the contact force between the shaft and the hole during one assembly process, as well as the displacement of the end effector of the robotic arm in the direction of the hole axis, and stable and compliant contact force control can be achieved during the assembly process. Figure 8 shows the virtual force acting on the object during the assembly process. Since the axis of the hole coincides with the z-axis of the world coordinate system, the virtual torque is only applied in the direction around the z-axis. The images during the assembly process and the outputs of the state classification neural network and the localization neural network are as shown in Figure 9 It can be seen that the positioning accuracy of the shaft and the hole is relatively high during the assembly process. Although there are some classification errors, the impact on the assembly is limited.

[0166] Although the present invention has been described in terms of a limited number of embodiments, those skilled in the art in this technical field will understand that, within the scope of the present invention thus described, other embodiments can be envisaged. For the scope of the present invention, the disclosure of the present invention is illustrative rather than restrictive, and the scope of the present invention is defined by the appended claims.

Claims

1. A visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple robotic arms, characterized in that: The following steps are involved: Step 1: Use the image dataset of the shaft-hole assembly task to train the positioning neural network and the relative state estimation neural network respectively; Step 2: Use the positioning neural network to extract the position information of the axis / hole from the hand-eye camera images of each robot arm, and use the relative state estimation neural network to extract the relative state information between the axis and the hole from the hand-eye camera images of each robot arm; Step 3: Calculate the virtual force acting on the object using the position information of the shaft / hole and the relative state information between the shaft and the hole; Step 4: Input the virtual force into the collaborative controller of the multi-manipulator to realize the collaborative assembly motion of the multi-manipulator; Step 5: Repeat steps 2 to 4. The collaborative controller of the multi-manipulator controls the multi-manipulator to match the grasped multi-axis parts with the axial holes of the porous parts to be assembled one by one and align the axial holes to complete the assembly task.

2. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple manipulators according to claim 1 is characterized in that: The "image dataset for the shaft-hole assembly task" described in step 1 contains holes of different shapes, and the positions of the shaft and the hole in the image coordinate system are stored in the form of a heat map for training the positioning neural network.

3. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple manipulators according to claim 2 is characterized in that: The calculation formula of the "heat map" is as follows: In the formula, p I represents any point in the image coordinate system, Target point Corresponding heat map, σ represents the standard deviation.

4. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple manipulators according to claim 1, 2 or 3, characterized in that: The contents of "relative status information between shaft and hole" described in step 2 are as follows: The relative states between the shaft and the hole are divided into three categories: blocking, overlapping and separation; when in the blocking state, the shaft is in front of the hole and the hole is partially or safely blocked by the shaft; When in the overlapping state, the shaft is behind the hole and the hole is partially blocked by the shaft; when in the separated state, the hole is not blocked by the shaft at all.

5. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple manipulators according to claim 4 is characterized in that: The implementation process of "calculating the virtual force acting on the object" described in step 3 is as follows: Step 31: Position the axis and hole in the image coordinate system identified by the positioning neural network and Use the camera's imaging model to calculate the position vectors of the axis and hole in the camera coordinate system and Step 32: Calculate and Unit direction vector in world coordinates and Step 33: Use represents the position of the origin of the camera coordinate system in the world coordinate system. Assuming that the axis and the end of the robot arm are fixedly connected, it is estimated that To axis endpoint The position vector will pass And the plane perpendicular to l is set to α i , respectively calculate the vector and With plane α i The intersection of and l represents the unit vector in the direction of the hole axis; Step 34: Calculate the axis and hole in plane α i The deviation vector d on i ; Step 35: Calculate the virtual force F acting on each axis separately i ; Step 36: Calculate the virtual resultant force F and resultant moment M acting on the multi-axis part; Step 37: Combine the relative state information between the shaft and the hole to calculate the virtual force (expected force) h acting on the multi-axis part d .

6. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple robotic arms according to claim 5, characterized in that: Step 33: "Vector and With plane α i The intersection of and The calculation formula of ” is as follows: Steps 3 and 4 described "the axis and the hole are on the plane α i The deviation vector d on i The calculation formula of ” is as follows: The virtual force F acting on each axis described in step 35 i The calculation formula of ” is as follows: i represents the axis number; The calculation formula for the "virtual resultant force F and resultant moment M acting on the object" described in step 36 is as follows: In the formula, i represents the number of the axis; k M is the torque proportionality coefficient; F ε When ∑F i A random term is imposed when the value is too small, F ε The calculation formula is as follows; Where x~Bernoulli(0.5), F s is the threshold for applying random force, k ε Used to adjust the size of random force; The expected force h acting on the object described in step 37 d The calculation process of ” is as follows: In the formula, 03 represents a 1x3 zero vector, represents the relative state of the shaft and hole in the image of the hand-eye camera of robot i; before contact occurs, if the images of the two hand-eye cameras are in an overlapping state, the object is inserted along the axis direction, otherwise the axis is moved in the direction of reducing the error; after contact occurs, while the axis is moved in the direction of reducing the error, a k is maintained between the shaft and the hole. F l T contact force.

7. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple robotic arms according to claim 6 is characterized in that: The "multi-robot collaborative controller" described in step 4 consists of two parts: an object-level external force controller and an end-effector-level internal force controller. The object-level external force controller calculates the motion trajectory of the end-effector of the robot according to the virtual force acting on the manipulated part, and the end-effector-level internal force control is used to achieve a compliant response of the internal force exerted on the manipulated part.

8. The visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple manipulators according to claim 7 is characterized in that: The specific process of step 4 is as follows: Step 41: Use the object-level impedance controller to calculate the desired motion of the controlled object; first select a control reference point y fixed to the object, and calculate the reference acceleration of point y using the following control rate: Where: represents the velocity of point y, is the reference acceleration of point y, They represent the desired object inertia matrix, damping matrix and force feedback proportional coefficient respectively. All three matrices are selected as symmetric positive definite matrices. h is the resultant force and torque exerted by the environment on the object; c The force / torque sensor at the end of the robot arm can be used to estimate The reference motion of the object is obtained by integration Depend on composition; Step 42: Calculate the expected motion of the end effector of the robot arm: Assuming that there is no relative motion between the end effector of the robot arm and the object, the expected motion of the end effector of the robot arm is The following constraints are satisfied with the object reference motion: In the formula, r yi =r i -r y ,use represents the desired velocity of the end effector of the robot arm, represents the desired acceleration of the end effector of the robot arm, Together they form the desired motion of the end effector of the robot arm; Step 43: Use the end-effector-level impedance controller to calculate the robot arm joint angular velocity control command to control the internal force of the object. The following end-effector-level impedance controller is used: Where: They represent the desired inertia matrix, damping matrix and stiffness matrix of the end of the manipulator, respectively, and are also selected as symmetric positive definite matrices; represents the difference between the actual and expected end pose of robot arm i, represents the difference between the actual speed and the expected speed of the end of robot i, represents the difference between the reference acceleration and the expected acceleration of the end of robot i, It represents the component of the force / torque applied by the end of the robot arm to the object that generates the internal force / torque of the object; is the attitude transformation matrix. After transformation, the above variables are expressed in the desired attitude coordinate system. middle; Using the internal force / torque component h Ii , the expected motion of the robot end coordinate system The actual state information p of the robot end coordinate system i ,R i , ω i And impedance parameters Calculate the reference acceleration of the robot end coordinate system together right Integrate to get the reference velocity of the robot end coordinate system The reference joint angular velocity is calculated using the Jacobian matrix of the robotic arm: In the formula, q i ∈R 7 Represents the joint angle vector of the robot arm (taking a 7-DOF robot arm as an example), is the pseudo-inverse of the Jacobian matrix of the robot; As the control instructions for the joint servo motors of robot arm i.

9. A visual servo control method system for closely coordinating the assembly of multi-axis hole parts by multiple robotic arms, characterized in that: The system has a program module corresponding to the steps of any one of claims 1 to 8 above, and executes the steps in the visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple robotic arms during operation.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the visual servo control method for tightly coordinated assembly of multi-axis hole parts by multiple robotic arms as described in any one of claims 1 to 8 is implemented.

Images