Double-mechanical-arm path planning method and device and double-mechanical-arm control system

Abstract

The invention provides a double-mechanical-arm path planning method and device and a double-mechanical-arm control system.The double-mechanical-arm path planning method comprises the steps that path planning is conducted on double mechanical arms based on a given starting point and a given target point, and a double-mechanical-arm motion path is generated; time information is added to the motion paths of the double mechanical arms, and time paths of the double mechanical arms are generated; performing a collision test on the double mechanical arms based on the time path of the double mechanical arms; if the collision test is passed, the double-mechanical-arm time path serves as a target path; and if the collision test is not passed, the time path of the slave mechanical arm is adjusted based on the time path of the master mechanical arm until the collision test is passed, and the time path of the master mechanical arm and the adjusted time path of the slave mechanical arm serve as target paths. According to the technical scheme provided by the invention, the rationality and feasibility of path planning are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arms, in particular to a dual-robot arm path planning method, device and dual-robot arm control system. BACKGROUND

[0002] Welding, as an indispensable processing technology in manufacturing, plays a key role in the manufacturing process of various products. In the field of welding of steel prefabricated pipe fittings, traditional welding methods mainly include manual welding and automatic welding based on single robot arm. Among them, manual welding relies on the experience and skills of welders, has high operation requirements, and has a poor working environment and high labor intensity. Although single robot arm automatic welding improves efficiency and accuracy to some extent, the working range and flexibility of single robot arm are limited when dealing with complex pipe fittings. With the progress of science and technology, machine vision, multi-axis linkage control and other technologies are gradually applied to the field of welding, providing a new direction for the development of welding automation. Machine vision technology can realize real-time monitoring and positioning of the weld, and multi-axis linkage control technology can make the robot move in a more complex space to complete diversified welding tasks.

[0003] In related technologies, welding is performed by using a single robot arm combined with a positioner. The single robot arm usually has multiple degrees of freedom and can move flexibly within a certain range to complete basic welding actions. The positioner is used to adjust the position and attitude of the pipe fitting, so that the weld is in a more convenient position for welding. Cooperating with single-axis or multi-axis positioners, some steel prefabricated pipe fittings with less complex shapes can be welded. During welding, the robot controls the welding torch to follow the weld according to the obtained weld information. The positioner adjusts the angle and position of the pipe fitting in a timely manner according to the shape of the pipe fitting and the welding requirements to ensure the welding quality. However, this scheme has obvious deficiencies when facing complex steel prefabricated pipe fittings.

[0004] Limited welding range and flexibility: The working space of the single robot arm is limited, and for some large or complex-shaped steel prefabricated pipe fittings, it is difficult to fully cover the welding area, resulting in some welds being unable to be welded or poor welding quality.

[0005] Low welding efficiency: When dealing with multi-layer and multi-pass welding of complex pipe fittings, the single robot arm needs to frequently change the welding position and attitude, resulting in long welding time and low efficiency. Moreover, the single robot arm can only weld at one place at the same time, and cannot weld multiple parts simultaneously, further limiting the overall welding efficiency.

[0006] It is difficult to find the weld seam and to correct the deviation in real time during welding by the single mechanical arm, a large amount of preliminary preparation and debugging work need to be performed manually, the labor intensity and production cost are increased, and the welding quality is still difficult to guarantee. SUMMARY

[0007] In view of the above problems, the embodiments of the present application provide a double mechanical arm path planning method, device and double mechanical arm control system to solve the problems of limited welding range and flexibility, low welding efficiency and difficulty in weld seam positioning and deviation correction in the prior art welding method using a single mechanical arm combined with a positioner.

[0008] In the first aspect, the embodiments of the present application provide a double mechanical arm path planning method, the double mechanical arm comprising a master mechanical arm and a slave mechanical arm, and the method comprising:

[0009] path planning for the double mechanical arm based on a given starting point and a target point to generate a double mechanical arm motion path;

[0010] adding time information to the double mechanical arm motion path respectively to generate a double mechanical arm time path;

[0011] collision testing for the double mechanical arm based on the double mechanical arm time path;

[0012] if the collision testing is passed, the double mechanical arm time path is taken as a target path;

[0013] if the collision testing is not passed, the slave mechanical arm time path is adjusted based on the master mechanical arm time path until the collision testing is passed, and the master mechanical arm time path and the adjusted slave mechanical arm time path are taken as the target path.

[0014] In a possible implementation manner, the path planning for the double mechanical arm based on the given starting point and the target point to generate the double mechanical arm motion path comprises:

[0015] initializing the starting point as a root node to establish a growth tree;

[0016] determining a distance between points and a steering angle according to a current sampling point, a minimum cost point, the starting point and the target point;

[0017] generating a growth node according to the distance between points, the steering angle, a preset first step length and a second step length;

[0018] updating the growth tree based on the growth node, and determining a double mechanical arm initial path according to the updated growth tree;

[0019] The initial path of the dual-robot arm is optimized by a path pruning algorithm and a B-spline curve fitting optimization algorithm to generate a dual-robot arm motion path.

[0020] In a possible implementation, before the point distance and the steering angle are determined according to the current sampling point, the minimum cost point, the starting point and the target point, the method further includes:

[0021] The configuration space of the robot arm is randomly divided to obtain a plurality of local regions;

[0022] A plurality of sampling points are randomly generated in the local regions, and a probability of the sampling points falling in each local region is directly proportional to a proportion of the local region in the configuration space;

[0023] The minimum cost point is determined from the growth tree by a cost function.

[0024] In a possible implementation, the growth node is generated according to the point distance, the steering angle, a preset first step length and a second step length, and includes:

[0025] It is judged whether the point distance is less than a preset step length, and the preset step length is a smaller value between the first step length and the second step length;

[0026] If it is judged that the point distance is greater than or equal to the preset step length, the current sampling point is taken as the growth node;

[0027] If it is judged that the point distance is less than the preset step length, it is judged whether the steering angle is less than or equal to a preset steering angle threshold value;

[0028] If it is judged that the steering angle is less than or equal to the steering angle threshold value, the first step length is taken to grow in the direction of the target point to generate the growth node;

[0029] If it is judged that the steering angle is greater than the steering angle threshold value, the second step length is taken to grow in a random direction to generate the growth node.

[0030] In a possible implementation, the configuration space includes a free space, and the growth tree is updated based on the growth node, and the initial path of the dual-robot arm is determined according to the updated growth tree, and includes:

[0031] It is judged whether the growth node and a growth connection edge are located in the free space, and the growth connection edge is a connection edge generated by the growth node and the minimum cost point;

[0032] If it is judged that the growing node and / or the growing connection edge are not located in the free space, the growing node is discarded, and the step of generating a growing node according to the inter-point distance, the guide included angle, the preset first step length and the second step length is continuously executed;

[0033] If it is judged that the growing node and the growing connection edge are located in the free space, the growing tree is updated according to the growing node and the growing connection edge;

[0034] It is judged whether the updated growing tree enters the recognition range of the target point.

[0035] If it is judged that the updated growing tree enters the recognition range of the target point, the dual-robot initial path is determined according to the updated growing tree.

[0036] If it is judged that the updated growing tree does not enter the recognition range of the target point, the step of generating a growing node according to the inter-point distance, the guide included angle, the preset first step length and the second step length is continuously executed until the updated growing tree enters the recognition range of the target point.

[0037] In a possible implementation, the dual-robot initial path is a discrete path point set, and the dual-robot initial path is optimized by a path pruning algorithm and a B-spline curve fitting optimization algorithm to generate a dual-robot motion path, including:

[0038] A redundant node in the dual-robot initial path is determined by the path pruning algorithm.

[0039] The redundant node is used for path pruning to obtain a redundant optimization path.

[0040] The redundant optimization path is smoothed by the B-spline curve fitting optimization algorithm to generate the dual-robot motion path.

[0041] In a second aspect, an embodiment of the present application provides a dual-robot control system, the system including a master robot, a slave robot, an L-shaped robot base and a clamp;

[0042] The master robot and the slave robot are rigidly connected to corresponding clamps through the L-shaped robot base.

[0043] In a possible implementation, the L-shaped robot base includes a long side plate and a short side plate, the long side plate and the short side plate are vertically connected, the long side plate is rigidly connected to the clamp, and the short side plate is rigidly connected to the robot.

[0044] In a third aspect, an embodiment of the present application provides a dual-robot path planning device, the dual robot including a master robot and a slave robot, and the device including:

[0045] a planning module, configured to plan a path for the dual-robot arms based on a given starting point and a target point, and generate a dual-robot arm motion path;

[0046] an adding module, configured to add time information to the dual-robot arm motion path respectively, and generate a dual-robot arm time path;

[0047] a testing module, configured to perform a collision test on the dual-robot arms based on the dual-robot arm time path;

[0048] a determining module, configured to, if the collision test is passed, take the dual-robot arm time path as a target path;

[0049] an adjusting module, configured to, if the collision test is not passed, adjust the slave-robot arm time path based on the master-robot arm time path until the collision test is passed, and then take the master-robot arm time path and the adjusted slave-robot arm time path as the target path.

[0050] In a fourth aspect, an embodiment of the present application provides a computer readable storage medium, which comprises a stored program, wherein the program controls a device where the computer readable storage medium is located to perform the dual-robot arm path planning method in the first aspect or any possible implementation manner of the first aspect when the program is running.

[0051] In the technical scheme provided by the embodiment of the present application, the time information is added to the dual-robot arm motion path, and the same timeline is used to add the time information to the dual-robot arm motion path respectively, so as to ensure the time consistency of the dual-robot arm time path, and the master-robot arm time path is used to adjust the slave-robot arm time path, so as to avoid the collision between the master-robot arm and the slave-robot arm, and improve the rationality and feasibility of the path planning. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 A schematic structural diagram of a clamping structure is provided for the embodiment of the present application.

[0053] Figure 2 A schematic structural diagram of an L-shaped robot arm base is provided for the embodiment of the present application.

[0054] Figure 3 A schematic diagram of a symmetrically-arranged dual-robot arm control system is provided for the embodiment of the present application.

[0055] Figure 4 A flowchart of a dual-robot arm path planning method is provided for the embodiment of the present application.

[0056] Figure 5A flowchart of a double-robot arm path planning method provided by an embodiment of the present application is shown in the figure.

[0057] Figure 6 A comparison diagram of before and after path optimization provided by an embodiment of the present application is shown in the figure.

[0058] Figure 7 A flowchart of generating a growth node in a double-robot arm path planning method provided by an embodiment of the present application is shown in the figure.

[0059] Figure 8 A flowchart of determining an initial path in a double-robot arm path planning method provided by an embodiment of the present application is shown in the figure.

[0060] Figure 9 A structural diagram of a double-robot arm path planning device provided by an embodiment of the present application is shown in the figure.

[0061] Figure 10 A structural diagram of a planning module provided by an embodiment of the present application is shown in the figure. DETAILED DESCRIPTION

[0062] To make the objectives, technical solutions, and advantages of the present application clearer and more comprehensible, the present application is further described below in conjunction with the accompanying drawings and specific embodiments. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0063] In the prior art, there is a holding clamp as shown in Figure 1 Figure 1 A structural diagram of a holding clamp provided by an embodiment of the present application is shown in the figure, the holding clamp is circular, and the holding clamp has a circular motion side, a circumferential outer side, and a circumferential inner side. Figure 1 The holding clamp is integrated with a servo motor, and the holding clamp is driven to make circumferential motion around the pipe by the servo motor.

[0064] As shown in the figure, the circumferential inner side of the holding clamp is provided with four fixing bolts, and the four fixing bolts are distributed at an interval of 90° along the ring.By rotating the adjusting knob at the end of the fixing bolt, the length of the fixing bolt extending into the circumferential inner side is changed, and the holding and fixing of pipe with different diameters are realized.The head part of the fixing bolt is wrapped with an arc-shaped rubber pad, the rubber pad has good anti-skid and wear-resistant performance, avoids damaging the surface of the pipe, at the same time, increases the friction force, improves the fixing reliability, and is suitable for pipes made of steel, plastic, and other materials. Figure 1

[0065] Figure 2A structural schematic diagram of an L-shaped mechanical arm base provided by an embodiment of the present application is shown in Figure 2 The L-shaped mechanical arm base includes a long side plate and a short side plate, the long side plate (vertical section) is vertically connected with the short side plate (horizontal section) to form a stable support structure. The long side plate is rigidly connected with the clamp, and the short side plate is rigidly connected with the mechanical arm. The L-shaped mechanical arm base is made of L-shaped integrated aluminum alloy (or steel) profile and is processed by a computer numerical control (CNC) device. The profile rigidity is used to ensure the attitude accuracy of the mechanical arm after installation, and to avoid the pose deviation caused by vibration and load during operation.

[0066] In the embodiment of the present application, based on the clamp, a mechanical arm base mounting surface is arranged on the annular moving side of the clamp, a plurality of screw holes are uniformly distributed on the long side plate of the L-shaped mechanical arm base, the plurality of screw holes are matched with the threaded holes on the mechanical arm base mounting surface (i.e. the hole spacing and hole diameter are matched), and the screw holes and the threaded holes are connected by bolts to realize the rigid connection of the long side plate and the clamp and to ensure the synchronous movement of the L-shaped mechanical arm base and the clamp. The mechanical arm base mounting surface is processed by milling, and the flatness error is ≤0.02mm, which can meet the application requirements of high-precision scenes.

[0067] In the embodiment of the present application, an array type threaded hole is arranged on the short side plate of the L-shaped mechanical arm base, and a mounting hole matched with the array type threaded hole is arranged on the mechanical arm, the array type threaded hole and the mounting hole are connected by bolts to realize the rigid connection of the short side plate and the mechanical arm. Moreover, the L-shaped mechanical arm base is processed by precise milling to ensure the flatness, so that the L-shaped mechanical arm base and the mechanical arm are quickly connected and have consistent attitudes, and the deviation of the mechanical arm movement track caused by the uneven mounting surface is avoided.

[0068] As an optional solution, the master mechanical arm and the slave mechanical arm are arranged symmetrically, i.e. the double mechanical arms are symmetrically installed around the pipeline. Figure 3 A schematic diagram of a symmetrically arranged double mechanical arm control system provided by an embodiment of the present application is shown in Figure 3 The double mechanical arm control system includes a master mechanical arm, a slave mechanical arm, an L-shaped mechanical arm base and a clamp; the master mechanical arm and the slave mechanical arm are rigidly connected with the corresponding clamps through the L-shaped mechanical arm base. The master mechanical arm and the slave mechanical arm are distributed in left and right mirror image around the pipeline (including flange and special-shaped pipe fitting). The master and slave mechanical arms are rigidly connected with the pipeline outer clamps through the L-shaped mechanical arm base to realize symmetric and cooperative operation, so as to realize synchronous welding of the pipeline weld and improve the operation efficiency and precision.

[0069] In the embodiment of the present application, the master robot and the slave robot are symmetrically distributed on both sides of the pipeline with the pipeline center axis as the symmetry reference, and the kinematics model of the "base-joint-end" is mirror symmetric. Through the motion control algorithm, the master robot and the slave robot are controlled to move synchronously along the pipeline axis and rotate in the circumferential direction during welding, so as to ensure that the welding parameters on both sides of the weld are consistent.

[0070] As another alternative, the master robot and the slave robot adopt an asymmetric layout. Among them, the master robot carries the welding gun to perform complex trajectory welding, and the slave robot assists in fixing the pipe fitting through a vacuum chuck or a gripper, and the cooperation of the double robots is realized through the master-slave motion control algorithm. At this time, the degrees of freedom of the slave robot can be simplified to 3-4 axes, which reduces the hardware cost and is suitable for straight seam welding scenes with low flexibility requirements.

[0071] Further, through the time delay compensation algorithm of master-slave control, the time delay between the master robot and the slave robot is reduced, and the real-time, smoothness and accuracy of the operation of the master robot and the slave robot are ensured. The end effector of the slave robot is integrated with multiple functions (such as clamping function and welding material conveying function), so as to improve the operation efficiency and flexibility of the master robot and the slave robot.

[0072] In the technical scheme provided by the embodiment of the present application, the double robots cooperate with the clamp to expand the working envelope to the full circumferential direction of the pipeline. For example, for complex pipe fittings with multiple branches, the double robots can work synchronously from both sides, covering the internal welds that cannot be reached by a single robot, greatly reducing the welding blind area, expanding the welding range, and improving the flexibility of the double robots.

[0073] In the embodiment of the present application, the double robots have parallel welding capability and can complete the welding of both sides of the girth seam at the same time. In combination with the improved Rapidly-exploring Random Tree (RRT) algorithm, the empty travel time is reduced, and compared with the welding scheme of a single robot combined with a positioner, the welding efficiency is greatly improved.

[0074] The dynamics research of the double robots is the premise of the cooperative operation of the double robots. At present, the main methods for establishing the dynamics model of the double robots are Newton-Euler equation method and Lagrange equation method. In the embodiment of the present application, the dynamics equation of the robot is established based on the Newton-Euler equation method.

[0075] First, single-link mechanical arm mechanics analysis. Generally, the mechanical arm includes joints and links, in the process of mechanical arm movement, in the specified direction on the link force to drive joint movement. Link is a mass, size (inertia tensor), not deformation of rigid body, suitable for rigid body dynamics. Rigid body dynamics for the study of a rigid body motion state and its relationship between the external force, its relationship is generally described by the Euler's law of motion. Newton's second law F = ma is the description of the abstract "object" or "particle", while the Euler's law of motion is a supplement to Newton's second law, which describes the volume of the rigid body composed of a large number of "particles" together.

[0076]

[0077] From the first law of Euler, the linear momentum of a rigid body is equal to the mass of the rigid body and its centroid linear velocity of the product, so when considering the linear motion of the rigid body, only need to consider the linear motion of the centroid. From the second law of Euler, the linear momentum of a single object is equal to the external force, that is, the rate of change of the angular momentum of the object is equal to the torque it receives, that is:

[0078]

[0079]

[0080]

[0081] And the collection of a large number of "particles" is:

[0082]

[0083] The inertia tensor I of the rigid body is:

[0084]

[0085] The angular momentum expression of the rigid body is:

[0086]

[0087] The torque of the rigid body is defined as the rate of change of the angular momentum of the rigid body, and the derivative of both ends can be obtained:

[0088]

[0089] Where τ represents the torque of the rigid body, where The angular acceleration of the rigid body The relationship between torque and angular velocity, angular acceleration is:

[0090]

[0091] For the dynamics of the robot arm, the end effector of the robot arm runs according to the specified trajectory through the force of each joint motor on its link; the dynamics of the robot arm can be divided into reverse recursive force transmission (i.e. from the external force on the end effector, the torque of each joint is calculated in turn) and forward recursive velocity and acceleration (i.e. from the base, the velocity and acceleration transmitted to each link are calculated in turn, until the end acceleration).

[0092] Secondly, the inverse dynamics of the robot arm is analyzed. For the inverse kinematics algorithm, the Newton-Euler equation method is the most widely used and has high computational efficiency, therefore, in the embodiment of the application, the Newton-Euler equation method is selected as the inverse kinematics algorithm for calculation. The Newton-Euler equation method includes two parts, one part is the outward recursion of the motion quantity, and the other part is the inward recursion of the force / torque.

[0093] Among them, the outward recursion of the motion quantity, i.e. the forward recursive calculation of the velocity and acceleration, as known from the foregoing analysis, if the inertia force and inertia moment of each link are required, the angular acceleration and linear velocity need to be known. The angular velocity of the current link is equal to the angular velocity of the previous link and the angular velocity caused by the rotation of the joint of the current link:

[0094]

[0095] Taking the derivative of both sides of the formula, the transmission formula of the angular acceleration is further obtained:

[0096]

[0097] Among them, θ represents the angle of the rotary joint, and Z represents the rotation axis of the joint.

[0098] For the linear velocity, the linear velocity of each link is equal to the sum of the linear velocity of the previous link, the linear velocity caused by the rotation of the previous link and the linear velocity of the translational joint:

[0099] v i+1 =v i +ω i ×p i+1 +d i+1 Z i+1

[0100] Taking the derivative of both sides of the formula, the transmission formula of the linear acceleration is further obtained:

[0101]

[0102] Among them, d represents the joint position, Z represents the direction of the joint translation, and p represents the position vector of the center of mass of the previous link to the current link; by solving the above two formulas, the inertia force and inertia moment of each link can be obtained:

[0103]

[0104]

[0105] where, the inward recursion of force / torque, namely, inverse recursion of force / torque.f i represents the force of each connecting rod received from the previous connecting rod, n i represents the torque it receives. The force balance equation and the torque balance equation are:

[0106] F i = f i-i+1 R·f i+1

[0107] N i = n i -n i-1 +(-p c )×f i +(p i+1 -p c )×(-f i+1 )

[0108] Then the force / torque of each joint is:

[0109]

[0110] According to the movement mode of the joint, the joint is divided into a rotating joint and a moving joint. Among them, the rotating joint is used to realize the relative rotational movement between two parts, and the moving joint is used to realize the relative linear movement between two parts.

[0111] The dynamics equation of the double robot arm is:

[0112]

[0113] where, q, respectively represent the joint angle, angular velocity and angular acceleration of each robot arm, represents the inertia matrix of the robot arm, which has the characteristics of positive definite symmetry, M(q) = [M1(q) M2(q)]; C(q, q) represents the Coriolis force and centrifugal force matrix of the robot arm; G(q) represents the gravity vector of the robot arm; J T represents the transpose of the Jacobian matrix of the robot arm end effector; F represents the force at the contact point of the end effector and the rigid body; τ represents the driving torque of each joint.

[0114] In the embodiment of the present application, the double-robot arm dynamics equation containing forward recursive kinematics and inverse dynamics is established, the mapping relationship between joint torque and end pose is solved by Newton-Euler equation method, the precise trajectory control of the robot arm end effector is realized, and the attitude adjustment demand of complex weld is met.

[0115] Figure 4 A flowchart of a double-robot arm path planning method provided by the embodiment of the present application is shown in FIG. 1, which comprises the following steps. Figure 4

[0116] In step 101, the double-robot arm is path planned based on the given starting point and target point, and a double-robot arm motion path is generated.

[0117] As an optional solution, the double-robot arm is path planned by A* algorithm, and a double-robot arm motion path is generated. The A* algorithm generates a double-robot arm motion path by using a heuristic function (such as Euclidean distance) for global path search based on the given starting point and target point. Compared with the improved RRT algorithm, the A* algorithm can shorten the path planning time and reduce the occupation of computing resources, and is suitable for scenes of high real-time requirement of flow line operation. In actual application, considering the welding process parameter constraint, the heuristic function can be customized to further improve the path planning efficiency, which is not limited in the embodiment of the present application.

[0118] As another optional solution, the double-robot arm is path planned by the improved RRT algorithm based on the given starting point and target point, and a double-robot arm motion path is generated.

[0119] In step 102, time information is added to the double-robot arm motion path respectively, and a double-robot arm time path is generated.

[0120] In the embodiment of the present application, the motion path planning process of a single robot arm is as follows: given the starting point and the target point, a collision-free feasible path from the starting point to the target point is searched in the pose space of the robot arm. For the loose coordination path planning of the double-robot arm, the pose space of the master robot arm is C1, and the pose space of the slave robot arm is C2, the dimension of each pose space is 5, that is, each point in the pose space is a 5-dimensional state sequence of each robot arm. If the path planning of the double-robot arm is considered, the pose space C is represented as C=C1*C2, and the dimension of the pose space C is 10, that is, each point in the space represents a state sequence of the double-robot arm. For the loose coordination path planning of the double-robot arm, the improved RRT algorithm can be directly deployed to the pose space C of the double-robot arm for centralized and unified planning in theory, but the increase of dimension will greatly increase the time and difficulty of solving constraints.

[0121] ​In the embodiment of the present application, time information is added to the motion path of the dual robot arms, specifically, a timestamp is assigned to each path point, a corresponding relationship between the path points and the time is established, so that the robot arm reaches the path point corresponding to the time at the time, and then reaches each path point in turn according to the timestamps. Wherein, the same timeline is used to add time information to the motion path of the dual robot arms to ensure the time consistency of the dual robot arms.

[0122] In step 103, collision test is performed on the dual robot arms based on the dual robot arm time path; if the collision test is passed, step 104 is performed; if the collision test is not passed, step 105 is performed.

[0123] In this step, if the collision test is passed, it indicates that the dual robot arm time path is reasonable and feasible, and when the master and slave robot arms move along the respective time paths, physical collision will not occur between the master and slave robot arms and between the robot arms and the surrounding environment (such as workbench, workpiece, etc.). If the collision test is not passed, it indicates that the determined time path has collision hazards, and when the master and slave robot arms move along the respective time paths, physical collision may occur between the master and slave robot arms or between the robot arms and the surrounding environment.

[0124] In step 104, the dual robot arm time path is taken as the target path.

[0125] In step 105, the slave robot arm time path is adjusted based on the master robot arm time path until the collision test is passed, and then the master robot arm time path and the adjusted slave robot arm time path are taken as the target path.

[0126] In this step, the path information of the master robot arm time path is added to the obstacle space of the slave robot arm, and the obstacle space and the free space of the slave robot arm are updated. Based on the updated free space, the slave robot arm time path is adjusted, and the adjusted motion path of the slave robot arm will not fall into the obstacle space, thereby avoiding the collision between the master and slave robot arms.

[0127] In the embodiment of the present application, a six-dimensional force control sensor is integrated at the end of the robot arm, and forms redundant detection with the binocular vision sensor. The force control sensor realizes weld seam positioning through contact force feedback under strong arc light interference (such as full position welding), and the binocular vision sensor provides global positioning under normal illumination. In the scene of deep groove, blind hole and other visual blind areas, the deviation correction reliability is improved, and the dependence on a single sensor is reduced. Further, through a space-time registration algorithm, the force control signal collected by the force control sensor and the vision data collected by the binocular vision sensor are synchronized, and the unity between the data is ensured.

[0128] Further, the decision logic of the sensor is set through a confidence switching strategy. For example, the weights of different sensors are set according to the reliability, importance and other factors of the sensors, the confidence of the sensor is weighted, the comprehensive confidence under the current decision logic is obtained, and it is judged whether the decision logic needs to be switched according to the comprehensive confidence. When the comprehensive confidence is low, the decision logic is switched to improve the decision confidence and precision. In the embodiment of the present application, the precise positioning and real-time correction of the weld are realized, the manufacturing error of the pipe fitting is adaptively compensated, the welding qualification rate is improved, the manual debugging cost is reduced, the welding precision is improved, and the stability and reliability of the welding quality are improved.

[0129] In the technical scheme provided by the embodiment of the present application, time information is added to the motion paths of the two mechanical arms, the same timeline is used to add time information to the motion paths of the two mechanical arms, the time consistency of the motion paths of the two mechanical arms is ensured, the motion path of the master mechanical arm is used to adjust the motion path of the slave mechanical arm, the collision between the master mechanical arm and the slave mechanical arm is avoided, and the rationality and feasibility of the path planning are improved.

[0130] In the embodiment of the present application, the welding range is expanded through the cooperative work of the two mechanical arms, the welding flexibility is improved, the welding requirements of the steel prefabricated pipe fitting with various complex shapes and sizes can be met by the double mechanical arm control system. The welding efficiency is significantly improved by using the parallel operation capability of the double mechanical arms, and the welding time is reduced.

[0131] Figure 5 In the double mechanical arm path planning method provided by the embodiment of the present application, the flowchart of generating the motion paths of the two mechanical arms by the improved RRT algorithm is as shown in Figure 5 The method comprises the following steps.

[0132] In step 1011, the starting point is initialized as the root node, and a growing tree is established.

[0133] In step 1012, the distance between points and the steering angle are determined according to the current sampling point, the minimum cost point, the starting point and the target point.

[0134] In this step, the distance between points is the distance between the minimum cost point and the current sampling point, and the steering angle is the angle formed by the starting point, the current node and the target point.

[0135] The calculation formula of the steering angle is as follows:

[0136]

[0137] Wherein, β represents the steering angle, ‖q init q‖2 represents the distance between the starting point and the current state point, ‖qq goal ‖2 represents the distance between the current node and the target point; ‖qinit q goal ‖2 distance between the start point and the target point.

[0138] In the embodiment of the present application, before step 1012, the method further comprises: randomly dividing the configuration space of the robot arm to obtain a plurality of local regions; randomly generating a plurality of sampling points in the local regions, the probability of the sampling points falling in each local region being proportional to the proportion of the local region in the configuration space; and determining the minimum cost point from the growing tree by using the cost function. The minimum cost point is determined according to a start cost and a target cost, the start cost being used to indicate the cost from the start point to the current state point, and the target cost being used to indicate the cost from the current state point to the target point.

[0139] Step 1013: generating a growing node according to the distance between the points, the guide angle, a preset first step length and a preset second step length.

[0140] Step 1014: updating the growing tree based on the growing node, and determining the initial path of the dual robot arms according to the updated growing tree.

[0141] Step 1015: optimizing the initial path of the dual robot arms by using a path pruning algorithm and a B-spline curve fitting optimization algorithm to generate a motion path of the dual robot arms.

[0142] In this step, the path pruning algorithm is used to determine redundant nodes in the initial path of the dual robot arms, the path pruning is performed based on the redundant nodes to obtain a redundant optimized path, and the B-spline curve fitting optimization algorithm is used to perform smoothing processing on the redundant optimized path to generate the motion path of the dual robot arms. For example, the B-spline curve fitting optimization algorithm is a cubic B-spline curve fitting optimization algorithm. In the embodiment of the present application, the tree structure of the RRT algorithm itself leads to a zigzag path, and the path optimization algorithm is used to ensure the feasibility and necessity of the path, thereby improving the path planning efficiency.

[0143] In the embodiment of the present application, the initial path of the dual robot arms is represented as a series of discrete path point sets. For example, the initial path is {p1, p2, p3…, p n}, wherein p1 represents the start point, p nThe target point is represented. For example, two path points on the initial path are randomly selected, it is judged whether a straight line path exists between the two path points and whether the straight line path is in free space, if it is judged that a straight line path exists between the two path points and the straight line path is in free space, it is indicated that other path points between the two path points are redundant nodes. At this time, the original path between the two path points is replaced by the straight line path, that is, other path points between the two path points are deleted, so as to realize path pruning. For example, the two path points randomly selected are p3 and p5, a straight line path exists between p3 and p5, and the straight line path is in free space, so p4 is a redundant node, and the path p3p4 and the path p4p5 are replaced by the straight line path p3p5.

[0144] For another example, the values of i and j are respectively set as 2 and n, p i and p j are connected, that is, the path point p2 and the target point p n are connected, it is judged whether the connection edge formed by p i and p j is in free space; if it is judged that the connection edge formed by p i and p j is not in free space, the next path point is sequentially connected with the target point until the connection edge formed by the path point and the target point is in free space, and then the key path point is added to the key point set; if it is judged that the connection edge formed by p i and p j is in free space, p i is added to the key point set. It is judged whether p j is located after the path point p3, if it is judged that p j is located after the path point p3, the values of i and j are respectively set as 2 and 3, and the step of connecting p i and p j is continuously executed. The starting point p1 and the target point p n are added to the key point set, at this time, the starting point, the target point and the key path point are included in the key point set. In the initial path, the points other than the key point set are redundant nodes. In actual application, other path pruning algorithms can be selected for path pruning, and the embodiment of the present application is not limited in this aspect.

[0145] Figure 6 A comparison schematic diagram before and after path optimization provided by the embodiment of the present application is shown in FIG. 1. Figure 6As shown, the effect comparison before and after path optimization is shown through the simulation map, and the size of the simulation map is 1000*1000. A pink solid circle is used to represent the starting point (0, 0), an orange solid circle is used to represent the target point (1000, 1000), and a green rectangle represents an obstacle. The red path represents the initial path of the mechanical arm; the cyan path represents the initial path of the mechanical arm after path pruning by the path pruning algorithm; and the black path represents the motion path of the mechanical arm after smoothing the redundant optimization path by the curve fitting optimization algorithm. Compared with the initial path of the mechanical arm, the motion path of the mechanical arm is smoother, and the feasibility and rationality are higher.

[0146] In the technical scheme provided by the embodiment of the application, the improved RRT algorithm is used to generate the motion path of the dual mechanical arm, and the path optimization algorithm is used to optimize the initial path of the dual mechanical arm, thereby ensuring the feasibility and necessity of the path and improving the path planning efficiency.

[0147] Figure 7 A flowchart for generating a growth node in the dual mechanical arm path planning method provided by the embodiment of the application is shown in Figure 7 The method comprises the following steps.

[0148] In step 10131, it is determined whether the distance between the points is less than the preset step length. If it is determined that the distance between the points is greater than or equal to the preset step length, step 10132 is performed; if it is determined that the distance between the points is less than the preset step length, step 10133 is performed.

[0149] In this step, the preset step length is the smaller value between the first step length and the second step length.

[0150] In step 10132, the current sampling point is taken as a growth node.

[0151] In step 10133, it is determined whether the guide angle is less than or equal to the preset guide angle threshold. If it is determined that the guide angle is less than or equal to the guide angle threshold, step 10134 is performed; if it is determined that the guide angle is greater than the guide angle threshold, step 10135 is performed.

[0152] In step 10134, the first step length is used to grow in the direction of the target point to generate a growth node.

[0153] In step 10135, the second step length is used to grow in a random direction to generate a growth node.

[0154] In the embodiment of the application, the growth node is generated in different ways based on different situations of the distance between the points and the guide angle, so that the process of generating the growth node is more flexible.

[0155] Figure 8A flowchart of a process for determining an initial path in a dual-robot path planning method provided by an embodiment of the present application is shown in FIG. 10, and the method comprises the following steps: Figure 8

[0156] In step 10141, it is determined whether the growth node and the growth connection edge are located in the free space. If it is determined that the growth node and / or the growth connection edge are not located in the free space, the growth node is discarded, and step 1013 is continued. If it is determined that the growth node and the growth connection edge are located in the free space, step 10142 is performed.

[0157] In this step, the configuration space comprises the free space and the obstacle space. The growth connection edge is a connection edge generated by the growth node and the minimum cost point. The growth node located in the free space is added to the node set, and the connection edge located in the free space is added to the connection edge set.

[0158] In step 10142, the growth tree is updated according to the growth node and the growth connection edge.

[0159] In this step, the growth node and the growth connection edge are added to the growth tree to update the growth tree.

[0160] In step 10143, it is determined whether the updated growth tree enters the identification range of the target point. If it is determined that the updated growth tree enters the identification range of the target point, step 10144 is performed. If it is determined that the updated growth tree does not enter the identification range of the target point, step 1013 is continued until the updated growth tree enters the identification range of the target point.

[0161] In this step, if it is determined that the updated growth tree enters the identification range of the target point, it indicates that a feasible path from the starting point to the target point has been determined, and the update and expansion of the growth tree can be terminated. If it is determined that the updated growth tree does not enter the identification range of the target point, it indicates that a feasible path from the starting point to the target point has not been determined, and the update and expansion of the growth tree needs to be continued to determine the feasible path.

[0162] In step 10144, the initial path of the dual robot is determined according to the updated growth tree.

[0163] In this step, the node in the growth tree that enters the identification range of the target point is connected to the target point, the parent node is traced back to the starting point through the target point, the complete path is extracted, and the complete path is taken as the initial path of the dual robot. In the embodiment of the present application, the rationality and feasibility of the path are ensured by discarding the unreasonable growth node, and the path planning efficiency is improved.

[0164] Figure 9 A structure diagram of a dual-robot path planning device provided by an embodiment of the present application is shown in FIG. 11, and the device comprises the following components: Figure 9 ​As shown, the device comprises a planning module 11, an adding module 12, a testing module 13, a determining module 14 and an adjusting module 15. The planning module is used for path planning of the dual-robot arm based on a given starting point and target point, generating a dual-robot arm motion path; the adding module is used for adding time information to the dual-robot arm motion path respectively, generating a dual-robot arm time path; the testing module is used for collision testing of the dual-robot arm based on the dual-robot arm time path; the determining module is used for taking the dual-robot arm time path as the target path if the collision testing is passed; and the adjusting module is used for adjusting the slave robot arm time path based on the master robot arm time path if the collision testing is not passed, and taking the master robot arm time path and the adjusted slave robot arm time path as the target path after the collision testing is passed.

[0165] Figure 10 A structural schematic diagram of a planning module provided by an embodiment of the present application is shown in the figure. Figure 10 As shown, the planning module 11 comprises an establishing submodule 111, a determining submodule 112, a generating submodule 113, an updating submodule 114 and an optimizing submodule 115. The establishing submodule 111 is used for initializing the starting point as a root node and establishing a growth tree; the determining submodule 112 is used for determining the distance between points and the guiding angle according to the current sampling point, the minimum cost point, the starting point and the target point; the generating submodule 113 is used for generating a growth node according to the distance between points, the guiding angle, the preset first step length and the second step length; the updating submodule 114 is used for updating the growth tree based on the growth node, and determining the dual-robot arm initial path according to the updated growth tree; and the optimizing submodule 115 is used for optimizing the dual-robot arm initial path through a path pruning algorithm and a B-spline curve fitting optimization algorithm, and generating a dual-robot arm motion path.

[0166] In the embodiment of the present application, the planning module 11 further comprises a dividing submodule 116, a sampling submodule 117 and a cost submodule 118. The dividing submodule 116 is used for randomly dividing the pose space of the robot arm to obtain a plurality of local regions; the sampling submodule 117 is used for randomly generating a plurality of sampling points in the local regions, and the probability of the sampling points falling in each local region is proportional to the proportion of the local region in the pose space; and the cost submodule 118 is used for determining the minimum cost point from the growth tree through a cost function.

[0167] In the embodiment of the present application, the growing submodule 113 is specifically configured to determine whether the distance between points is less than a preset step length, the preset step length being a smaller value between the first step length and the second step length; if it is determined that the distance between points is greater than or equal to the preset step length, the current sampling point is taken as a growing node; if it is determined that the distance between points is less than the preset step length, it is determined whether the guide included angle is less than or equal to a preset guide included angle threshold; if it is determined that the guide included angle is less than or equal to the guide included angle threshold, the first step length is taken to grow in the direction of the target point to generate a growing node; if it is determined that the guide included angle is greater than the guide included angle threshold, the second step length is taken to grow in a random direction to generate a growing node.

[0168] In the embodiment of the present application, the conformation space includes a free space. The updating submodule 114 is specifically configured to determine whether the growing node and a growing connecting edge are located in the free space, the growing connecting edge being a connecting edge generated between the growing node and the minimum cost point; if it is determined that the growing node and / or the growing connecting edge are not located in the free space, the growing node is discarded, and the step of generating a growing node according to the distance between points, the guide included angle, the preset first step length and the preset second step length is continuously executed; if it is determined that the growing node and the growing connecting edge are located in the free space, the growing tree is updated according to the growing node and the growing connecting edge; it is determined whether the updated growing tree enters the recognition range of the target point; if it is determined that the updated growing tree enters the recognition range of the target point, the initial path of the dual-robot arm is determined according to the updated growing tree; if it is determined that the updated growing tree does not enter the recognition range of the target point, the step of generating a growing node according to the distance between points, the guide included angle, the preset first step length and the preset second step length is continuously executed until the updated growing tree enters the recognition range of the target point.

[0169] In the embodiment of the present application, the optimization submodule 115 is specifically configured to determine a redundant node in the initial path of the dual-robot arm through a path pruning algorithm; perform path pruning based on the redundant node to obtain a redundant optimized path; and perform smoothing processing on the redundant optimized path through a B-spline curve fitting optimization algorithm to generate a dual-robot arm motion path.

[0170] In the technical scheme provided by the embodiment of the present application, time information is added to the dual-robot arm motion path, the same timeline is used to add time information to the dual-robot arm motion path, the time consistency of the dual-robot arm time path is ensured, the master robot arm time path is used to adjust the slave robot arm time path, the collision between the master robot arm and the slave robot arm is avoided, and the rationality and feasibility of path planning are improved.

[0171] The embodiment of the present application provides a computer readable storage medium, which includes a stored program, wherein when the program runs, the device where the computer readable storage medium is located performs each step of the embodiment of the dual-robot arm path planning method, and the specific description can be referred to the above-mentioned embodiment of the dual-robot arm path planning method.

[0172] The above description is only the preferred embodiment of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art can easily think of changes or replacements within the technical range disclosed by the present application, which should be covered in the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A dual robot path planning method, characterized by, The double robot arm includes a master robot arm and a slave robot arm, and the method includes: path planning for the double robot arm based on a given starting point and a target point, to generate a double robot arm motion path; adding time information to the double robot arm motion path respectively, to generate a double robot arm time path; collision testing for the double robot arm based on the double robot arm time path; if the collision testing is passed, taking the double robot arm time path as a target path; if the collision testing is not passed, adjusting the slave robot arm time path based on the master robot arm time path, until the collision testing is passed, and taking the master robot arm time path and the adjusted slave robot arm time path as the target path.

2. The method of claim 1, wherein, The path planning for the double robot arm based on a given starting point and a target point, to generate a double robot arm motion path, includes: initializing the starting point as a root node, to establish a growth tree; determining a point distance and a steering angle according to a current sampling point, a minimum cost point, the starting point and the target point; generating a growth node according to the point distance, the steering angle, a preset first step length and a preset second step length; updating the growth tree based on the growth node, and determining a double robot arm initial path according to the updated growth tree; optimizing the double robot arm initial path through a path pruning algorithm and a B-spline curve fitting optimization algorithm, to generate a double robot arm motion path.

3. The method of claim 2, wherein, Before the determining of the point distance and the steering angle according to the current sampling point, the minimum cost point, the starting point and the target point, the method further includes: randomly dividing a robot arm configuration space to obtain a plurality of local regions; randomly generating a plurality of sampling points in the local regions, a probability of the sampling points falling in each local region being proportional to a proportion of the local region in the configuration space; determining a minimum cost point from the growth tree through a cost function.

4. The method of claim 2, wherein, The generating of the growth node according to the point distance, the steering angle, the preset first step length and the preset second step length includes: judging whether the point distance is less than a preset step length, the preset step length being a smaller value between the first step length and the second step length; if it is judged that the point distance is greater than or equal to the preset step length, taking the current sampling point as the growth node; if it is judged that the point distance is less than the preset step length, judging whether the steering angle is less than or equal to a preset steering angle threshold value; if it is judged that the steering angle is less than or equal to the preset steering angle threshold value, growing in the target point direction at the first step length to generate the growth node; if it is judged that the steering angle is greater than the preset steering angle threshold value, growing in a random direction at the second step length to generate the growth node.

5. The method of claim 2, wherein, The configuration space includes a free space, and the updating of the growth tree based on the growth node, and the determining of the double robot arm initial path according to the updated growth tree includes: judging whether the growth node and a growth connection edge are located in the free space, the growth connection edge being a connection edge generated between the growth node and the minimum cost point; If it is judged that the growing node and / or the growing connection edge are not located in the free space, the growing node is discarded, and the step of generating a growing node according to the distance between points, the guide angle, the preset first step length and the preset second step length is continuously executed; If it is judged that the growing node and the growing connection edge are located in the free space, the growing tree is updated according to the growing node and the growing connection edge; It is judged whether the updated growing tree enters the recognition range of the target point; If it is judged that the updated growing tree enters the recognition range of the target point, the double-robot initial path is determined according to the updated growing tree; If it is judged that the updated growing tree does not enter the recognition range of the target point, the step of generating a growing node according to the distance between points, the guide angle, the preset first step length and the preset second step length is continuously executed until the updated growing tree enters the recognition range of the target point.

6. The method of claim 2, wherein, The double-robot initial path is a discrete path point set, and the double-robot initial path is optimized to generate a double-robot motion path through a path pruning algorithm and a B-spline curve fitting optimization algorithm, including: A redundant node in the double-robot initial path is determined through the path pruning algorithm; A redundant optimization path is obtained through path pruning based on the redundant node; The double-robot motion path is generated through the B-spline curve fitting optimization algorithm for smoothing the redundant optimization path.

7. A dual robot control system, characterized by, The system includes a master robot, a slave robot, an L-shaped robot base and a clamp; The master robot and the slave robot are rigidly connected to the corresponding clamps through the L-shaped robot base.

8. The system of claim 7, wherein, The L-shaped robot base includes a long side plate and a short side plate, and the long side plate and the short side plate are vertically connected. The long side plate is rigidly connected to the clamp, and the short side plate is rigidly connected to the robot.

9. A dual robot path planning device, characterized by, The double-robot includes a master robot and a slave robot, and the device includes: A planning module for planning a path for the double-robot based on a given starting point and a target point to generate a double-robot motion path; An adding module for adding time information to the double-robot motion path to generate a double-robot time path; A testing module for testing the double-robot based on the double-robot time path for collision; A determining module for determining the double-robot time path as a target path if the collision test is passed; An adjusting module for adjusting the slave robot time path based on the master robot time path until the collision test is passed, and then determining the master robot time path and the adjusted slave robot time path as the target path.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored program, wherein the program controls the device where the computer-readable storage medium is located to execute the double-robot path planning method according to any one of claims 1-6 when the program is running.

Images