A Trajectory Planning Method for Collaborative Assembly of Detonators and Fuse Bodies by Two Robots

Abstract

The present invention discloses a trajectory planning method for the collaborative assembly of detonators and fuse bodies by two robots, which adopts a hybrid coordination control strategy combining master-slave control and symmetric control. When the end effectors of the two-robot collaborative assembly system are outside the coordinated assembly area, the symmetric control strategy is adopted, and the two robots move independently without interference, and no coupling relationship is formed between the robots. When the end effectors of the two-robot collaborative assembly system enter the collaborative assembly area, in order to accurately perform the assembly operation, the master-slave control strategy is adopted, and a certain coupling constraint relationship is established between the robots through the end to make the system a redundant robot system, so as to complete the trajectory teaching and planning tasks of the two-robot collaborative assembly system.

Description

Technical Field

[0001] The present invention belongs to the technical field of fine assembly of military industrial equipment, and particularly relates to a trajectory planning method for collaborative assembly of an explosive transfer tube and a fuse by dual robots, which is applicable to the robot assembly trajectory planning for the assembly of the missile explosive transfer tube and the fuse body by dual robots. Background Art

[0002] The explosive transfer tube and the fuse body in a missile are dangerous articles that are flammable and explosive, and their assembly methods are restricted by many factors. At present, the assembly process of the fuse body and the explosive transfer tube still relies on manual assembly by operators, which has a large labor intensity, low efficiency, and a high risk factor, resulting in a low yield rate of products. Therefore, realizing the automatic assembly of the explosive transfer tube and the fuse body is the research focus of the current assembly automation industry.

[0003] When a dual-robot system performs an assembly task, two robots need to coordinate with each other, so the trajectory is complex and changeable, which makes it very difficult for dual robots in teaching. Traditional industrial robots use a teach pendant to specify a series of trajectory points or teach by dragging the end of the robot, which requires careful regulation of the details of each joint movement of the robot and is usually used for teaching single robots. If this method is applied to dual-robot collaboration, when teaching, not only the actions of each robot itself need to be considered, but also the cooperation timing of the two arms, whether the objects held by the two arms will collide during the movement, and problems such as reaching the dead point need to be considered. Therefore, applying the above-mentioned action-level teaching method to dual-robot coordinated assembly is not ideal in terms of efficiency and feasibility. Summary of the Invention

[0004] In order to solve the above problems existing in the prior art, the present invention provides a trajectory planning method for collaborative assembly of an explosive transfer tube and a fuse body by dual robots. For the collaborative assembly of the explosive transfer tube and the fuse body by dual robots, this method can solve the trajectory planning problem during dual-robot collaborative assembly.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] A trajectory planning method for collaborative assembly of an explosive transfer tube and a fuse body, which is applied to a dual-robot collaborative assembly system. The dual-robot collaborative assembly system includes a robot module and a machine vision system module; the robot module includes two six-axis industrial robots, and a gripper is installed at the end of each six-axis industrial robot, which are respectively used to grab and hold the explosive transfer tube and the fuse body; the machine vision module includes two monocular cameras, and the two monocular cameras are respectively fixed on the grippers of the two six-axis industrial robots;

[0007] The trajectory planning method includes:

[0008] S1. When the end effector of the six-axis industrial robot is outside the coordinated assembly area, a symmetric control strategy is adopted to perform trajectory planning for the two six-axis industrial robots respectively:

[0009] Step 1: Adopt the Eye_in_Hand hand-eye calibration layout form, install the monocular camera at the end of the six-axis industrial robot, and use the calibration board for calibration;

[0010] Step 2: After calibration, use the teach pendant to move the six-axis industrial robot above the workpiece to be assembled, and set this position as waypoint 1;

[0011] Step 3: Control the monocular camera to collect images. After image preprocessing, identify and determine the pose of the workpiece to be assembled, and establish the grasping point;

[0012] Step 4: Guide the six-axis industrial robot to the grasping point to grasp the workpiece through the vision system. After grasping, the six-axis industrial robot moves to waypoint 1 set in Step 2 by the teaching method;

[0013] Step 5: Use the teach pendant to control the two six-axis industrial robots to move the detonator and the fuse body to the collaborative assembly starting point respectively;

[0014] S2. When the end effector of the six-axis industrial robot is within the coordinated assembly area, a master-slave symmetric strategy is adopted to perform trajectory planning for the collaborative assembly of the two six-axis industrial robots:

[0015] Step 6: The two six-axis industrial robots move the detonator and the fuse body to the coordinated assembly starting position where their axes are aligned respectively;

[0016] Step 7: The master robot and the slave robot hold the detonator and the fuse body respectively and move to the contact position at speeds of v0 and v1;

[0017] Step 8: The master robot holds the detonator and continues to move at a speed of , and the slave robot holds the fuse body and rotates in place until the fuse body is tightened.

[0018] Furthermore, in Step 1, a multi-point calibration method is adopted to calibrate the six-axis industrial robot and the monocular camera vision sensor:

[0019] After the six-axis industrial robot drives the monocular camera to move to the specified waypoint, start the calibration. Place the nine-square calibration board in the area to be recognized, install the probe at the end of the six-axis industrial robot, and teach the calibration tip at the end of the robot to reach the pre-set spatial fixed point in the calibration board to obtain the transformation coordinates of the robot tool coordinate system in the robot base coordinate system And record the coordinates Q of this group of spatially fixed points in the robot base coordinate system; obtain the coordinates P of the spatially fixed points in the vision sensor, and according to the formula Solve the transformation matrix between the sensor coordinate system and the robot tool coordinate system That is, the calibration is completed.

[0020] Furthermore, in the third step, median filtering is used to filter the images collected by the monocular camera, filtering out isolated noise points and outliers in the background, and retaining the image edge information of the components to be assembled; the two-dimensional median filtering expression is:

[0021] g(x,y) = Med[f(x - i,y - i)]

[0022] In the formula, (i,j) ∈ M, where M is the coordinates of the pixels in the selected neighborhood; f(x,y) and g(x,y) are the images before and after the filtering process respectively.

[0023] Furthermore, in the fifth step, when guiding two six-axis industrial robots to move the detonator and the fuse body to the collaborative assembly starting point through the teach pendant, if there is a singular region in the known area, the singular value avoidance must be carried out first before optimizing the trajectory of the six-axis industrial robot trajectory dynamic curve.

[0024] Preferably, the singular value avoidance process of the trajectory dynamic curve before trajectory optimization is as follows:

[0025] The cyclic damping coefficient method is used to smooth the joint velocity curve. The calculation relationship between a certain joint velocity point of the robotic arm and the joint is:

[0026]

[0027] Judge And Whether the difference between them exceeds the threshold The order of magnitude of the maximum normal difference of the joint velocity between Δt intervals is 10 -3 ; if the difference exceeds Then make the damping coefficient λ act cyclically k times until the difference is less than Otherwise, continue to check the next pair of discrete velocities. The modified joint velocity after cycling is:

[0028]

[0029] Among them:

[0030]

[0031] After the singular value avoidance of the joint velocity curve, the joint velocity curve in the singular region becomes Subsequently, the corresponding discrete joint angle values also change. By integrating the joint velocity, the joint angle at the corresponding moment is obtained:

[0032]

[0033] Finally, the new joint angle value θ i (t)' is used to replace θ i (t), and the joint angle sequence after singularity avoidance is incorporated into the optimization algorithm to participate in the trajectory planning process.

[0034] Furthermore, in step six, assume that (x0, y0, z0) is the coordinate system of the end gripper of the master robot, denoted as {T}; (x1, y1, z1) is the coordinate system of the end gripper of the slave robot, denoted as {T1}; v0 and v1 are the velocities of the end grippers of the master and slave robots when grasping an object, respectively, and v0 and v1 are equal in magnitude and opposite in direction; l is the distance between the opposite ends of the detonator and the fuse body; d is the distance between the origin points of the two end gripper coordinate systems, with an initial value of d0; the base coordinate system of the master robot is denoted as {B}; the base coordinate system of the slave robot is denoted as {B1}; is the transformation matrix from the coordinate system {B} to the coordinate system {T}, is the transformation matrix from the coordinate system {T} to the coordinate system {T1}, and so on; the transformation matrix between the coordinate systems {T} and {T1} is:

[0035]

[0036] Furthermore, in step seven, the transformation matrix from the coordinate system {B} to {T1} is:

[0037]

[0038] Multiply both sides of the equation on the left by to obtain the transformation relationship from the base coordinate system of the slave robot to the coordinate system of the end gripper of the slave robot as:

[0039]

[0040] where is the transformation matrix between the base coordinate systems of the master and slave robots, which does not change during the coordinated assembly process, When the coordinates of the end gripper of the master robot are given, the rotation degrees of each joint are calculated using inverse kinematics, and then obtained from the kinematic equation ;

[0041]

[0042] where \(d = d_0-(v_0 + v_1)t\), that is, the distance between the origin of the coordinate systems of the two end grippers at each moment.

[0043] Further, in the eighth step, the rotation angle \(\Delta\theta_6\) of the 6th joint of the robot for gripping the fuse body according to the pose constraint relationship is:

[0044]

[0045] where \(dis\) is the distance for the fuse body to rotate one week, that is, the pitch.

[0046] The present invention has the following advantages:

[0047] The present invention provides a trajectory planning method for the collaborative assembly of the booster tube and the fuse body by two robots for the assembly of the booster tube and the fuse body. A hybrid coordination control strategy combining master-slave control and symmetric control is adopted. When the end effectors of the two-robot collaborative assembly system are outside the coordinated assembly area (that is, the two robots respectively identify and grasp the booster tube and the fuse body and enter the assembly area), the symmetric control strategy is adopted, and the two robots move independently without interference, and no coupling relationship is formed between the robots. This can reduce the communication time between the robots and the computer and shorten the time of the assembly task of the booster tube and the fuse body.

[0048] When the end effectors of the two-robot collaborative assembly system enter the collaborative assembly area, in order to perform the assembly action accurately, the master-slave control strategy is adopted, and a certain coupling constraint relationship is established between the robots through the end to make the system a redundant robot system.

[0049] Classify the collaborative motion of the multi-robot system according to the relative motion form between the end poses of the collaborative robots. For each relative motion form, deduce the pose relationship between the end poses of the robots. Determine the trajectory teaching steps and teaching information for the two-robot collaborative assembly according to the deduction results, and transform the collaborative assembly motion trajectory problem of the two-robot system into a single-robot spatial trajectory planning problem, so as to complete the trajectory teaching and planning tasks of the multi-robot collaborative assembly system.

[0050] Since in the process of the robots collaborating to complete the assembly of the booster tube and the fuse body, the master and slave robots need to hold the booster tube and the fuse body respectively and perform actions similar to tightening screw holes in the collaborative workspace, and the slave robot has a certain superimposed motion relative to the master robot to cooperate with the master robot to complete the assembly process. The present invention is based on kinematics and analyzes and deduces the kinematic constraint relationship between the master and slave robots in the coordinated assembly process based on the Cartesian coordinate system transformation relationship. Description of the Drawings

[0051] Figure 1Flow chart of a trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots according to the present invention;

[0052] Figure 2 Block diagram of the trajectory planning principle according to the present invention. Detailed implementation manners

[0053] In order to enable those skilled in the art to better understand the technical solutions of the present invention, the present invention will be further described below with reference to the accompanying drawings.

[0054] Embodiment

[0055] This embodiment is a trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots, which is applied to a collaborative assembly system of two robots for assembling detonators and fuse bodies.

[0056] The collaborative assembly system of two robots includes a robot module and a machine vision system module; the robot module includes two UR10E six-axis industrial robots, special explosion-proof grippers and a robot control box. The special robot grippers are respectively installed on the rotating mechanical hands at the ends of the two UR10E six-axis industrial robots, and are respectively used to grab and hold detonators and fuse bodies; the six-axis industrial robots and the special explosion-proof grippers are respectively connected to the robot control box; the machine vision module includes two monocular two-dimensional cameras, a light source, and an industrial computer integrated machine. The industrial computer integrated machine is built-in with a machine vision software system. The monocular two-dimensional monocular cameras are Hikvision monocular two-dimensional cameras. The two monocular two-dimensional cameras are respectively fixed on the two special explosion-proof grippers through brackets. The light sources are evenly distributed around the photographing area of the monocular two-dimensional cameras. The monocular two-dimensional cameras and the industrial computer integrated machine are respectively connected to the robot control box. The machine vision software converts the image pixel coordinates of the detonators and fuse bodies to be assembled collected into global coordinates by using the self-configured software and algorithms of Hikvision. For different poses of the detonators and fuse bodies, after analysis and calculation by the machine vision software system, the position coordinates of the detonators and fuse bodies to be assembled are respectively obtained, and the six-axis industrial robots are guided to grab the workpieces and hold them and move them to the area to be assembled.

[0057] This embodiment adopts the following technical solutions to perform trajectory planning for the collaborative assembly of detonators and fuse bodies by two robots:

[0058] For the assembly of the booster tube and the fuse body, a hybrid coordination control strategy combining master-slave control and symmetric control is adopted. When the end effectors of the dual-robot collaborative assembly system are outside the coordinated assembly area (i.e., the two robots respectively identify and grasp the booster tube and the fuse body and enter the assembly area), the control symmetry strategy is adopted, and the two robots move independently without interference, and no coupling relationship is formed between the robots. This can reduce the communication time between the robots and the computer and shorten the assembly task time of the booster tube and the fuse body. When the end effectors of the dual-robot collaborative assembly system enter the collaborative assembly area, in order to perform the assembly actions accurately, the master-slave control strategy is adopted, and a certain coupling constraint relationship is established between the robots through the end to make the system a redundant robot system, and each robot will be a dual-robot collaborative assembly system.

[0059] As Figure 1 shown, a trajectory planning method for a dual-robot collaborative assembly of a booster tube and a fuse body in this embodiment includes the following steps:

[0060] When the end effectors of the six-axis industrial robots are outside the coordinated assembly area, the symmetric control strategy is adopted to perform trajectory planning for the two six-axis industrial robots respectively:

[0061] Step 1: Adopt the Eye_in_Hand hand-eye calibration layout form, install the monocular two-dimensional camera at the end of the six-axis industrial robot, and use the calibration board for calibration;

[0062] In this embodiment, the multi-point calibration method is adopted to calibrate the robot and the visual sensor of the monocular two-dimensional camera:

[0063] After the robot drives the camera to move to waypoint 1, start the calibration. Place the nine-square grid calibration board in the area to be recognized, install the probe at the end of the six-axis robot, and teach the calibration tip at the end of the robot to reach the preset spatial fixed point in the calibration board, and calibrate to obtain the transformation coordinates of the robot tool coordinate system in the robot base coordinate system And record the coordinates Q of this set of spatial fixed points in the robot base coordinate system; obtain the coordinates P of the spatial fixed points in the visual sensor, and according to the formula Solve the transformation matrix between the sensor coordinate system and the robot tool coordinate system That is, the calibration is completed.

[0064] Step 2: After the calibration is completed, use the teach pendant to move the six-axis industrial robot above the workpiece to be assembled (booster tube and fuse body), and set this position as waypoint 1.

[0065] Step 3: Control the monocular two-dimensional camera to collect images, perform a series of image preprocessings, identify and determine the pose of the component to be assembled, and establish the grasping point.

[0066] In this embodiment, median filtering is used to filter the collected images, effectively removing isolated noise points and outliers in the background, and better retaining the image edge information of the components to be assembled. The standard two-dimensional median filtering expression is:

[0067] g(x,y) = Med[f(x - i,y - i)]

[0068] In the formula, (i,j) ∈ M, where M is the coordinate of the pixels in the selected neighborhood, and f(x,y) and g(x,y) are the images before and after the filtering process, respectively.

[0069] Step Four: After the camera recognizes the image and sets the grasping points, the vision system guides the six-axis industrial robot to the grasping points. The six-axis robot cooperates with the explosion-proof gripper to pick up the components to be assembled (explosion-proof tube, fuse body), and then moves the components to waypoint 1 set in Step Two through robot teaching respectively.

[0070] Step Five: Use the teach pendant to guide the two six-axis industrial robots to move the detonator tube and the fuse body to the collaborative assembly starting point respectively.

[0071] Use the teach pendant to move the workpiece to be assembled (fuse body and detonator tube) to the assembly starting point. During this process, if there are singular regions in the known area, and if there are singular regions in the known trajectory curve during the trajectory optimization process, it is bound to cause local mutations in the original normal trajectory dynamic curve, resulting in misidentification by the optimization algorithm based on the dynamic threshold and affecting the acquisition of the optimal result. Therefore, the singular value avoidance must be carried out before the trajectory dynamic curve is optimized. The movement of the UR robot is to input the 6 joint angle curves into the motion controller for motor drive control. The input joint angle curve is a discrete-time joint angle curve based on the minimum interval time △t. During the trajectory planning process, since the Cartesian space path is known and the overall running time and the joint angles at each moment are determined, the discrete joint angle curves required for control can be obtained. The specific block diagram is as Figure 2 shown.

[0072] Substitute the previous and the next three joint angles at the same moment into J 11 and J 22 (J 11 is called the front singularity, and J 22 is called the wrist singularity) and calculate their determinants. If the determinant of a group is 0, it means that the corresponding joint angle causes the front or wrist singularity. If the calculation result is near 0, the joint speed or differential motion speed of this group of results may be very large, and the inverse solution is often infeasible. In this case, reasonable means need to be taken to avoid the singular value region so that the robot can operate normally within the allowable range of the end error. Check J 11 and J in the entire trajectory22 Singular joint angle set θ, calculated using Δt There will be violent fluctuations in the joint velocity curve in this area. To avoid the singular area, a cyclic damping coefficient method is proposed to smooth the joint velocity curve. The calculation relationship between a certain joint velocity point of the manipulator and the joint is:

[0073]

[0074] Judge And Whether the difference exceeds the threshold The order of magnitude of the normal maximum difference in joint velocity during Δt is on the order of 10 -3 , when a singular value appears, the difference in joint velocity within the resulting Δt is much larger than the normal difference. Therefore, set the order of magnitude of to be 10 -1 . If the difference exceeds then make the damping coefficient λ act cyclically k times until the difference is less than Otherwise, continue to check the next pair of discrete velocities. The modified joint velocity is:

[0075]

[0076] Where:

[0077]

[0078] After avoiding the singular value of the joint velocity curve, the joint velocity curve in the singular area becomes Subsequently, the corresponding discrete joint angle values also change. The joint angle at the corresponding moment can be obtained by integrating the joint velocity

[0079]

[0080] Finally, use the new joint angle value θ i (t)' to replace θ i (t), and integrate the joint angle sequence after singular value avoidance into the optimization algorithm to participate in the trajectory planning process.

[0081] When the end effector of the six-axis industrial robot is in the coordinated assembly area, a master-slave symmetric strategy is adopted for the trajectory planning of the cooperative assembly of the dual six-axis industrial robots:

[0082] The principle of the master-slave symmetric strategy is:

[0083] Classify the cooperative motion of a multi-robot system according to the relative motion form between the end poses of the cooperative robots. For each relative motion form, deduce the pose relationship between the end poses of the robots. According to the deduction results, determine the trajectory teaching steps and teaching information for the cooperative assembly of two robots, and transform the cooperative assembly motion trajectory problem of the two-robot system into a single-robot spatial trajectory planning problem, so as to complete the trajectory teaching and planning tasks of the multi-robot cooperative assembly system.

[0084] According to the relative motion of the end of the cooperative robot, the cooperative motion of the multi-robot system is divided into two categories:

[0085] 1. Coupled motion: Robots with a cooperative relationship start the same form of motion at the same time at a certain moment, and the relative pose relationship between the end poses remains unchanged during the motion. The typical representative of the coupled motion described above is the two-robot handling system. Since the slave robot follows the master robot's motion, the slave robot does not require a separate trajectory teaching instruction, and the teaching and planning of the entire system can be completed in the base coordinate system of the master robot.

[0086] 2. Superimposed motion: Robots with a cooperative relationship start to move at the same time at a certain moment. Among them, the end motion trajectory of the master robot is independent, and the slave robot generates motion relative to the tool hand coordinate system of the master robot. The motion trajectory of the slave robot is the superposition of the relative motion of the slave robot's end with respect to the master robot's end and the motion trajectory of the master robot's end. The superimposed motion described above is mostly used to handle situations where robots need to cooperate with each other during the motion. The path of the slave robot is planned under the tool of the master robot, and the motion trajectory of the slave robot's end relative to the master robot's end is considered during the motion.

[0087] 3. Since the robots cooperate to complete the assembly process of the detonator and the fuse body, the master and slave robots need to hold the detonator and the fuse body respectively and perform actions similar to tightening screw holes in the cooperative workspace. The slave robot has a certain superimposed motion relative to the master robot and cooperates with the master robot to complete the assembly process. The present invention is based on kinematics and analyzes and deduces the kinematic constraint relationship between the master and slave robots during the coordinated assembly process based on the Cartesian coordinate system transformation relationship.

[0088] 4. Step six: Two six-axis industrial robots respectively grasp the fuse body and the detonator, and move the workpiece to the coordinated assembly starting position where their axes are aligned.

[0089] 5. Suppose \((x_0, y_0, z_0)\) is the coordinate system of the end gripper of the master robot, denoted as \(\{T\}\); \((x_1, y_1, z_1)\) is the coordinate system of the end gripper of the slave robot, denoted as \(\{T1\}\); \(v_0\) and \(v_1\) are the speeds of the end grippers of the master and slave robots respectively when grasping and moving an object, and \(v_0\) and \(v_1\) are equal in value and opposite in direction; \(l\) is the distance between the opposite ends of the detonator and the fuse body; \(d\) is the distance between the origins of the two end gripper coordinate systems, with an initial value of \(d_0\); the base coordinate system of the master robot is denoted as \(\{B\}\); the base coordinate system of the slave robot is denoted as \(\{B1\}\). In the text, is the transformation matrix from the coordinate system \(\{B\}\) to the coordinate system \(\{T\}\), is the transformation matrix from the coordinate system \(\{T\}\) to the coordinate system \(\{T1\}\), and so on. From Figure 2 it can be seen that the transformation relationship from the coordinate system \(\{T\}\) of the end gripper of the master robot to the coordinate system \(\{T1\}\) of the slave robot only requires moving the coordinate system \(\{T\}\) along the \(y\)-axis by a distance of \(d_0\), and then rotating it counterclockwise by \(90^{\circ}\) around the \(X\)-axis to obtain the transformation matrix between the coordinate systems \(\{T\}\) and \(\{T1\}\) which is:

[0090]

[0091] Step 7: The master and slave robots move the detonator and the fuse body to the contact position at speeds \(v_0\) and \(v_1\) respectively while clamping them.

[0092] The transformation matrix from the coordinate system \(\{B\}\) to \(\{T1\}\) can be expressed as:

[0093]

[0094] Multiply both sides of the equation on the left by to obtain the transformation relationship from the base coordinate system of the slave robot to the coordinate system of the end gripper of the slave robot as:

[0095]

[0096] where, is the transformation matrix between the base coordinate systems of the master and slave robots and will not change during the coordinated assembly process, can calculate the rotation degrees of each joint using inverse kinematics when the coordinates of the end gripper of the master robot are given, and then be obtained from the kinematic equation ;

[0097]

[0098] where, \(d = d_0-(v_0 + v_1)t\), that is, the distance between the origins of the two end gripper coordinate systems at each moment.

[0099] Step 8: The master robot clamps the detonator at Continue to move at the speed, and rotate in place with the robot gripping the fuse body. From the characteristics of each joint of the industrial robot, it can be seen that the slave robot only needs to grip the fuse body and rotate its 6th joint by the corresponding angle, while the other joints remain in their original states unchanged. According to the pose constraint relationship, the rotation angle Δθ6 of the 6th joint of the slave robot gripping the fuse body is:

[0100]

[0101] Among them, dis is the distance for the fuse body to rotate one week, that is, the pitch. The master robot continues to move while clamping, and the slave robot grabs the fuse body and rotates until the fuse body is tightened.

[0102] In the above-described specific embodiments, the purpose, technical solutions, and beneficial effects of the present invention have been further described in detail. It should be understood that the above are only specific embodiments of the present invention and are not used to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A trajectory planning method for collaborative assembly of detonators and fuse bodies by dual robots, which is applied to a dual-robot collaborative assembly system. The dual-robot collaborative assembly system includes a robot module and a machine vision system module; the robot module includes two six-axis industrial robots, and a gripper is installed at the end of each six-axis industrial robot, which are respectively used to grasp and hold the detonator and the fuse body; the machine vision system module includes two monocular cameras, and the two monocular cameras are respectively fixed on the grippers of the two six-axis industrial robots; It is characterized in that The trajectory planning method includes: S1. When the end effector of the six-axis industrial robot is outside the coordinated assembly area, a symmetric control strategy is adopted to perform trajectory planning on the two six-axis industrial robots respectively: Step 1. Adopt the Eye_in_Hand hand-eye calibration layout form, install the monocular camera at the end of the six-axis industrial robot, and use the calibration board for calibration; Step 2. After the calibration is completed, move the six-axis industrial robot above the workpiece to be assembled through the teach pendant, and set this position as waypoint 1; Step 3. Control the monocular camera to collect images. After image preprocessing, identify and determine the pose of the workpiece to be assembled, and establish the grasping point; Step 4. Guide the six-axis industrial robot to the grasping point to grasp the workpiece through the vision system. After the grasping is completed, the six-axis industrial robot moves to waypoint 1 set in Step 2 by the teaching method; Step 5. Control the two six-axis industrial robots respectively through the teach pendant to move the detonator and the fuse body to the collaborative assembly starting point; S2. When the end effector of the six-axis industrial robot is within the coordinated assembly area, a master-slave symmetric strategy is adopted to perform trajectory planning for the collaborative assembly of the two six-axis industrial robots: Step 6. The two six-axis industrial robots respectively move the detonator and the fuse body to the coordinated assembly starting position where their axes are aligned; Step 7. The master robot and the slave robot respectively hold the detonator and the fuse body and move to the contact position between them at speeds of v0 and v1; Step VIII. The master robot holds the detonator transfer tube and continues to move at a speed of, and the slave robot holds the fuse body and rotates in place until the fuse body is tightened.

2. The trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots according to claim 1, wherein, In Step 1, a multi-point calibration method is adopted to calibrate the six-axis industrial robot and the monocular camera vision sensor: After the six-axis industrial robot drives the monocular camera to move to the specified waypoint, calibration begins. Place the nine-square grid calibration board in the area to be recognized, install the probe at the end of the six-axis industrial robot, and teach the calibration tip at the end of the robot to reach the preset spatial fixed point in the calibration board. The transformation coordinates of the robot tool coordinate system in the robot base coordinate system are obtained through calibration And record the coordinates Q of this set of spatial fixed points in the robot base coordinate system; obtain the spatial fixed point coordinates P in the visual sensor, and according to the formula Solve the transformation matrix between the sensor coordinate system and the robot tool coordinate system That is, the calibration is completed.

3. The trajectory planning method for collaborative assembly of detonator and fuse body by two robots according to claim 1, characterized in that, In Step 3, median filtering is used to filter the images collected by the monocular camera, filter out isolated noise points and outliers in the background, and retain the image edge information of the components to be assembled; the two-dimensional median filtering expression is: g(x,y) = Med[f(x - i,y - i)] where (i,j) ∈ M, and M is the coordinate of the pixels in the selected neighborhood; f(x,y) and g(x,y) are the images before and after the filtering process respectively.

4. A trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots according to claim 1, characterized in that In Step 5, when guiding the two six-axis industrial robots to move the detonator and the fuse body to the collaborative assembly starting point respectively through the teach pendant, if there is a singular area in the known area, the singular value avoidance must be performed first before the trajectory dynamic curve of the six-axis industrial robot is optimized.

5. A trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots as described in claim 4, characterized in that, The singular value avoidance process of the trajectory dynamic curve before trajectory optimization is: The cyclic damping coefficient method is used to smooth the joint velocity curve. The calculation relationship between a joint velocity point of the robotic arm and the joints is: Judge and whether the difference exceeds the threshold The order of magnitude of the maximum difference in joint speed between Δt is on the order of 10 -3 ; If the difference exceeds then apply the damping coefficient λ cyclically k times until the difference is less than Otherwise, continue to check the next pair of discrete speeds, and the modified joint speed after cycling is: [[ID= After avoiding singularities in the joint velocity curve, the joint velocity curve in the singular region becomes Subsequently, the corresponding discrete joint angle values also change. By integrating the joint velocity, the joint angle at the corresponding moment can be obtained: Finally, use the new joint angle value θ i (t)' to replace θ i (t), and incorporate the joint angle sequence after singularity avoidance into the optimization algorithm to participate in the trajectory planning process.

6. The trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots according to claim 1, characterized in that, In Step 6, assume that (x0, y0, z0) is the coordinate system of the end effector of the master robot, denoted as {T}; (x1, y1, z1) is the coordinate system of the end effector of the slave robot, denoted as {T1}; v0 and v1 are the speeds of the end effectors of the master and slave robots when grasping an object, respectively, and v0 and v1 are equal in value and opposite in direction; l is the distance between the opposite ends of the detonator and the fuse body; d is the distance between the origins of the two end effector coordinate systems, and the initial value is d0; the base coordinate system of the master robot is denoted as {B}; the base coordinate system of the slave robot is denoted as {B1}; is the transformation matrix from the coordinate system {B} to the coordinate system {T}, is the transformation matrix from the coordinate system {T} to the coordinate system {T1}, and so on; the transformation matrix between the coordinate systems {T} and {T1} is:

7. A trajectory planning method for collaborative assembly of detonators and fuse bodies by two robots according to claim 6, characterized in that, In step 7, the transformation matrix from coordinate system {B} to {T1} is as follows: Multiply both sides of the equation on the left by The transformation relationship from the robot base coordinate system to the slave coordinate system to the slave robot end-effector coordinate is obtained as follows: Among them, is the transformation matrix between the base coordinate systems of the master and slave robots and will not change during the coordinated assembly process. When the coordinates of the end gripper of the master robot are given, the rotation degrees of each joint are calculated using inverse kinematics, and then obtained from the kinematic equation obtained; where d = d0 - (v0 + v1)t, that is, the distance between the origin points of the coordinate systems of the two end grippers at each moment.

8. The trajectory planning method for collaborative assembly of detonator and fuse body by two robots according to claim 1, characterized in that, In the eighth step, the rotation angle Δθ6 of the sixth joint of the robot for gripping the fuse body according to the pose constraint relationship is: where dis is the distance for one full rotation of the fuse body, that is, the pitch.

Images