Track planning method for optimal cooperative assembly of double mechanical arms

Abstract

A trajectory planning method for optimal cooperative assembly of double mechanical arms comprises the steps that S1, improved DH parameters of the mechanical arms are determined, and the coordinate range of a cooperative space is obtained through a Monte Carlo method; s2, the pose of the optimal assembly position of the double mechanical arms is solved through a particle swarm algorithm within the coordinate range of the cooperation space; s3, on the basis of pose information of end executors of the double mechanical arms, a B-spline interpolation method is adopted to calculate change parameters of all joints of the double mechanical arms, and speed and acceleration curves of all the joints are constructed; S4, an objective function is constructed, and a joint position sequence is obtained through calculation according to the pose of the optimal assembly position and the pose change curves of all the joints of the double mechanical arms; and then an NSGA-II optimization algorithm is adopted to select an optimal track meeting the objective function and the physical constraint condition of the mechanical arm from the joint position sequence. By introducing the operability, the time and energy consumption in the double-mechanical-arm assembling process are reduced, and the stability of the movement track is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm trajectory planning, and in particular to a trajectory planning method for optimal collaborative assembly of dual robot arms. Background Art

[0002] In the context of Industry 4.0, robotic arms are increasingly being used, and they play a vital role in various fields. In industrial manufacturing, many applications have proven that using single-arm robots to improve production efficiency is effective and feasible, but the flexibility and assembly efficiency of single robotic arms are difficult to meet the needs of greater intelligence. Dual-arm robots can not only ensure high flexibility and motion performance, but also achieve tasks that single-arm robots cannot accomplish, such as collaborative handling, object transfer, and collaborative assembly.

[0003] The existing method simplifies the process when performing optimal dual-arm assembly, and does not incorporate factors such as force feedback, visual tracking processing and obstacle avoidance to make the grasping assembly form a complete closed loop. It does not have enough grasp of global information and is unable to perform trajectory planning in complex environments. Summary of the invention

[0004] The present invention provides a trajectory planning method for optimal collaborative assembly of dual robotic arms to solve the technical problems mentioned in the background technology.

[0005] To achieve the above object, the technical solution of the present invention is achieved as follows:

[0006] The present invention provides a trajectory planning method for optimal collaborative assembly of dual robotic arms, comprising the following steps:

[0007] S1, determine the improved DH parameters of the robot arm and use the Monte Carlo method to obtain the coordinate range of the collaborative space;

[0008] S2, solving the optimal assembly position of the dual robotic arms by particle swarm algorithm within the coordinate range of the collaborative space;

[0009] S3. Based on the posture information of the end effectors of the dual robotic arms, the B-spline interpolation method is used to calculate the change parameters of each joint of the dual robotic arms, and the velocity and acceleration curves of each joint are constructed according to the change parameters of each joint;

[0010] S4. Construct the objective function and the physical constraints of the robot arm based on the velocity and acceleration curves of each joint, and calculate the joint position sequence based on the posture of the optimal assembly position and the posture change curves of each joint of the dual robot arm; then use the NSGA-II optimization algorithm to select the optimal trajectory that meets the objective function and the physical constraints of the robot arm from the joint position sequence.

[0011] Beneficial effects of the present invention:

[0012] The present invention reduces the time and energy consumption in the dual-manipulator assembly process and improves the stability of the motion trajectory by introducing operability and combining the NSGA-II optimization algorithm; it can effectively reduce the randomness and energy loss caused by the specified assembly position. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 It is a structural layout diagram of the dual mechanical arms in the present invention;

[0014] Figure 2 It is a layout diagram of each joint coordinate system in the present invention;

[0015] Figure 3 This is the layout diagram of the dual robotic arms in the platform coordinate system;

[0016] Figure 4 It is an operation block diagram of the particle swarm algorithm in the present invention;

[0017] Figure 5 It is a curve diagram of the speed and acceleration changes of each joint in the embodiment of the present invention;

[0018] Figure 6 It is a point cloud image of the dual-arm workspace in an embodiment of the present invention;

[0019] Figure 7 This is the NSGA-II trajectory optimization flow chart in the present invention. DETAILED DESCRIPTION

[0020] In order to facilitate the understanding of the present invention, the present invention will be described more fully below with reference to the relevant drawings. Preferred embodiments of the present invention are provided in the drawings. However, the present invention can be implemented in many other different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the understanding of the disclosure of the present invention more thorough and comprehensive.

[0021] The terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0022] Reference Figure 1 The present application embodiment provides a dual-manipulator optimal collaborative assembly trajectory planning method, comprising the following steps:

[0023] S1, determine the improved DH parameters of the robot arm and use the Monte Carlo method to obtain the coordinate range of the collaborative space;

[0024] S2, solving the optimal assembly position of the dual robotic arms by particle swarm algorithm within the coordinate range of the collaborative space;

[0025] S3. Based on the posture information of the end effectors of the dual robotic arms, the B-spline interpolation method is used to calculate the change parameters of each joint of the dual robotic arms, and the velocity and acceleration curves of each joint are constructed according to the change parameters of each joint;

[0026] S4. Construct the objective function and the physical constraints of the robot arm based on the velocity and acceleration curves of each joint, and calculate the joint position sequence based on the posture of the optimal assembly position and the posture change curves of each joint of the dual robot arm; then use the NSGA-II optimization algorithm to select the optimal trajectory that meets the objective function and the physical constraints of the robot arm from the joint position sequence.

[0027] In some embodiments, the S1 specifically includes the following steps:

[0028] S11. First, determine the model of the dual robotic arm. In this embodiment, two 6DOF VSCR-6EUR3 model robotic arms are used as an example for description. Then, confirm the improved DH (Denavit-Hartenberg) parameters of the dual robotic arm. The improved DH parameters include the number of joints and connecting rods, the rotation angles of each joint, the offset of each connecting rod, the rod length of each connecting rod, and the rotation angle of each connecting rod. The improved DH parameters are specifically shown in Table 1.

[0029] Table 1. Improved DH parameter table;

[0030] joint Joint rotation angle (°) Connecting rod offset(mm) Rod length (mm) Connecting rod angle (°) 1 0 121.5 0 0 2 -90 0 0 90 3 0 0 -300 0 4 -90 110.5 -276 0 5 0 90 0 90 6 0 0 0 -90

[0031] S12, establish a platform coordinate system in the middle of the platform where the dual robotic arms are located (i.e. Figure 1 The dual manipulators are set up around the platform coordinate system; then the base coordinate system (i.e. Figure 1 The two bases on the dual robot are symmetrically arranged on both sides of the base, and the base coordinate system is flush with the two bases of the dual robot; then the coordinate systems are established on the base, each joint and the end effector of the robot in turn. The coordinate system on the end effector is called the end coordinate system; the coordinate system of each joint Figure 2 As shown; the specific positions of the platform coordinate system and the base coordinate system are as follows Figure 1 As shown;

[0032] Reference Figure 3 It can be seen that in this embodiment, the two robotic arms adopt an anthropomorphic robot structure, and the base distance is 1.3m, so the base coordinates of the main robotic arm in the platform coordinate system are (-0.65, 0, 0.55), and the base coordinates of the slave robotic arm in the platform coordinate system are (0.65, 0, 0.55).

[0033] S13, according to the improved DH parameters of the dual robotic arms and the displacement parameters of each switch and connecting rod of the dual robotic arms, and according to forward kinematics, solving the transformation matrix of the end effector relative to the platform coordinate system;

[0034] S14, determining the rotation angles of each joint of the dual manipulators through inverse kinematics based on the transformation matrix of the end effector relative to the platform coordinate system and the known end effector position and direction;

[0035] S15. Use the Monte Carlo method to sample the workspace of the dual robotic arms and draw its point cloud image. In this embodiment, the point cloud image is shown in Figure 6 As shown, the joint combination is generated by the random value method to obtain the coordinate set of the two robot end effectors;

[0036] S16. Finally, the coordinate range in the collaboration space is calculated by using the coordinate sets of the two robot end effectors. Specifically, the coordinate range in the collaboration space in this embodiment is calculated as follows:

[0037]

[0038]

[0039]

[0040] In some embodiments, the calculation formula of the transformation matrix of the end effector relative to the platform coordinate system in S13 is specifically as follows:

[0041]

[0042] in, represents the transformation matrix from the end effector to the platform coordinate system; S represents the transformation matrix from the platform coordinate system to the robot base coordinate system; Represents the transformation matrix from the robot end effector coordinate system to the dual robot base coordinate system; transformation matrix The following relationship is satisfied:

[0043]

[0044] in, The transformation matrix representing the coordinate system of the first joint of the robot arm to the coordinate system of the base of the robot arm; Represents the transformation matrix from the second joint coordinate system of the robot arm to the first joint coordinate system of the robot arm; Represents the transformation matrix from the third joint coordinate system of the robot arm to the second joint coordinate system of the robot arm; Represents the transformation matrix from the fourth joint coordinate system of the robot arm to the third joint coordinate system of the robot arm; Represents the transformation matrix from the fifth joint coordinate system of the robot arm to the fourth joint coordinate system of the robot arm; Represents the transformation matrix from the sixth joint coordinate system of the robot to the fifth joint coordinate system of the robot.

[0045] Specifically, and They satisfy the following relationships respectively:

[0046]

[0047]

[0048]

[0049] Among them, θ1 and d1 represent the first joint angle and the offset of the first link respectively; θ2 and d2 represent the second joint angle and the offset of the second link respectively; θ3 and d3 represent the third joint angle and the offset of the third link respectively; θ4 and d4 represent the fourth joint angle and the offset of the fourth link respectively; θ5 and d5 represent the fifth joint angle and the offset of the fifth link respectively; θ6 and d6 represent the sixth joint angle and the offset of the sixth link respectively.

[0050] in The following relationship is satisfied:

[0051]

[0052] Among them, n x 、n y 、n z Represents the direction vector of the x-axis of the robot end coordinate system in the platform coordinate system; x , o y , o z Represents the direction vector of the y-axis of the robot end coordinate system in the platform coordinate system; a x 、a y 、a z p represents the direction vector of the Z axis of the robot end coordinate system in the platform coordinate system; x 、p y 、p z Represents the coordinates of the end of the robot arm in the platform coordinate system; and the above parameters satisfy the following relationship:

[0053]

[0054] -where c1 = cosθ1; c 234 =cos(θ2+θ3+θ4); c5 = cosθ5; c6 = cosθ6; c 23 =cos(θ2+θ3); c2 = cosθ2; s1 = sinθ1; s234 =sin(θ2+θ3+θ4); s5 = sinθ5; s6 = sinθ6; s 23 =sin(θ2+θ3); s2=sinθ2.

[0055] In some embodiments, the calculation formula of each joint angle of the dual robotic arms in S14 is as follows:

[0056] The calculation formula of the first joint angle is:

[0057]

[0058] Where θ1 represents the first joint rotation angle; p x 、p y They represent the x-axis and y-axis coordinates of the end effector of the robot arm in the platform coordinate system respectively; d4 represents the offset of the fourth link; the calculation formula of the second joint angle is:

[0059]

[0060] Where θ2 represents the second joint rotation angle; A, B and C are three different intermediate variables; and A, B and C satisfy the following relationship respectively:

[0061] A=-2B2a2

[0062] B=2B1a2

[0063] C=B1 2 +B2 2 +a2 2 -a3 2

[0064] B1=c1p x +s1p y -d5s 234

[0065] B2=p z -d1+d5c 234

[0066] Where a2 is the length of the second link; a3 is the length of the third link; c1 is the cosine value of the first joint rotation angle; s1 is the sine value of the first joint rotation angle; p z represents the z-axis coordinate of the end effector of the robot arm in the platform coordinate system; d1 represents the offset of the first link; d5 represents the offset of the fifth link; s 234 represents the sine value of the sum of the second joint rotation angle, the third joint rotation angle and the fourth joint rotation angle; c 234 represents the cosine value of the sum of the second joint angle, the third joint angle and the fourth joint angle; B1 and B2 are different intermediate variables;

[0067] By solving the above formula, only in A 2 +B 2 -C 2 ≥0, there is a solution, and then substituting the data into the above formula, we can get the rotation angles of the third and fourth joints;

[0068] The calculation formula for the third and fourth joint angles is:

[0069] θ3=θ 23 -θ2

[0070] θ4=θ 234 -θ 23

[0071] Where θ3 represents the third joint angle; θ4 represents the fourth joint angle; θ 23 represents the sum of the second joint angle and the third joint angle; θ 234 represents the sum of the second joint rotation, the third joint rotation and the fourth joint rotation;

[0072] The calculation formula of the fifth joint angle is:

[0073] θ5=atan2(s5,s1a x -c1a y )

[0074] Where θ5 represents the fifth joint rotation angle; a x 、a y They represent the direction vectors of the z-axis in the robot end coordinate system on the x-axis and y-axis in the platform coordinate system respectively; s5 is the sine value of the fifth joint angle;

[0075] The calculation formula of the sixth joint angle is:

[0076] θ6=Atan2(c1n y -s1n x , c1o y -s1o x )-Atan2(-s5,0)

[0077] Among them, n x 、n y They represent the direction vectors of the x-axis of the robot end coordinate system in the x-axis and y-axis directions in the platform coordinate system respectively; x , o y They respectively represent the direction vectors of the y-axis of the robot end coordinate system in the x-axis and y-axis directions in the platform coordinate system.

[0078] Specifically, the first joint rotation angle, the fifth joint rotation angle and the sixth joint rotation angle are deduced by the following equations;

[0079]

[0080] The second joint rotation angle, the third joint rotation angle and the fourth joint rotation angle are deduced by the following equations;

[0081]

[0082] The above equations are all obtained by having the same elements in the same position in the equations.

[0083] In this embodiment, it can be known through calculation that θ1 has two groups of solutions, θ5 and θ6 have four groups of solutions, and θ2, θ3 and θ4 have eight groups of solutions.

[0084] In some embodiments, reference Figure 4 , the S2 specifically comprises the following steps:

[0085] S21. First, set the initial parameters in the particle swarm algorithm: specifically, set the learning factor C1=C2=2, the number of iterations to 200, the population size to 100, the maximum particle speed to 0.5, and the minimum particle speed to -0.5; and set the initial value of the individual extreme value of each particle to zero;

[0086] S22, randomly initialize the positions and velocities of multiple particles, each particle adjusts its own speed according to the individual extreme value and the group extreme value of all particles and outputs the position variable;

[0087] Among them, the individual extreme value represents the best position of the particle itself; the group extreme value is the optimal solution selected from the individual extreme values ​​of all particles, which is used to guide the entire particle group to search for a better solution space;

[0088] S23, inversely solving the position variables of each particle through the ikine function in the MATLAB robot toolbox; if the inverse kinematics solution exists, solving the Jacobian matrix of the dual manipulator through the jacob0 function, substituting it into the mathematical model of maneuverability, and obtaining the maneuverability value of the calculated particle; if the inverse solution does not exist, performing the inverse kinematics solution for each particle again;

[0089] S24, comparing the current operability value of the calculated particle with the individual extreme value of the calculated particle, if the current operability value is higher, updating the individual extreme value of the calculated particle to the current operability value, and updating the position of the individual extreme value of the calculated particle; if the individual extreme value is higher, no update is performed;

[0090] S25, iterate S22 to S24 until the set conditions are met, compare the individual extreme values ​​of all particles, and obtain the individual extreme value with the highest operability value; use the position corresponding to the individual extreme value obtained in S25 as the position of the optimal assembly position. In this embodiment, the assembly position with the highest fitness in the collaborative space obtained after iteration is (-0.402, -0.006, 0.332).

[0091] In some embodiments, the operability mathematical model in S23 is specifically as follows:

[0092]

[0093] Among them, ω D Indicates the operability value; L , R Represents the weight coefficient of the left and right arms in the dual robotic arm; ω L ,ω R is the maneuverability value of the left and right robotic arms.

[0094] In some embodiments, S3 specifically includes the following steps:

[0095] S31, obtaining the pose information of the start point and the end point of the end effector, and obtaining the poses of multiple control points located between the start point and the end point, wherein the pose information of the start point and the end point of the end effector and the poses of the multiple control points constitute the pose information of the end effector;

[0096] S32, converting the end effector posture information into the angle value of each joint of the dual robotic arms;

[0097] Specifically, inverse kinematics (IK) is used to convert the pose information of the end effector into the angle value (i.e., joint parameter) of each joint of the dual manipulator. This step can find the corresponding joint angle for each key moment of the end pose. Inverse kinematics can be solved by a variety of methods, such as using MATLAB's inverse kinematics function or using numerical methods;

[0098] S33, determining the order and node vector of the B-spline curve;

[0099] Specifically, in order to achieve smooth interpolation of joint angles, the B-spline order is selected as 7. Next, a node vector is generated according to the number of points in the joint angle list and the spline order to determine the number of segments of the interpolation curve and the node distribution of each segment;

[0100] S34, constructing a B-spline interpolation of joint parameters according to the angle value of each joint of the dual robotic arms;

[0101] Specifically, the B-spline interpolation function in MATLAB (such as spap2 or csapi) is used to perform B-spline interpolation on the angle sequence of each joint. An independent B-spline curve is constructed for each joint of the dual manipulator to ensure the continuity and smoothness of each joint in time;

[0102] S35, calculating and extracting the posture parameters of each joint change curve;

[0103] Specifically, according to the known time parameters (i.e., time nodes), the spline object is used at these time nodes to find the angle value of each joint, thereby obtaining the joint change curve. For each time node, the posture change of each joint is calculated to obtain the posture parameters of each joint change curve;

[0104] S36. Calculate and construct the velocity and acceleration curves of each joint based on the posture parameters of the joint change curve.

[0105] Specifically, the calculated joint angles at each time node are input into the simulation system of the dual robotic arms, and the first-order and second-order derivatives of the B-spline interpolation curves of each joint are calculated to obtain the velocity and acceleration change curves of each joint.

[0106] The velocity and acceleration variation curves of each joint obtained in this embodiment are as follows: Figure 5 shown.

[0107] In some embodiments, the S31 specifically includes the following steps:

[0108] S311, define uniform time nodes, and obtain the position and posture information of the dual robotic arm end effectors at the beginning and end; the position and posture information of the dual robotic arm end effectors at the beginning and end is expressed by the formula as follows:

[0109]

[0110]

[0111] Among them, T start and T end Respectively represent the location information of the starting point and the location information of the end point; R start and R end The rotation matrices representing the attitudes of the starting point and the end point, P star and P end The vectors representing the starting and ending positions respectively;

[0112] S312: Divide the time period between the starting point and the end point into N uniform time nodes t i , time node t i The calculation formula is as follows:

[0113]

[0114] Among them, t start and t end Respectively represent the time of the starting point and the time of the end point;

[0115] S313, using linear interpolation to generate evenly distributed position nodes P between the starting point and the end point i , location node P i The calculation formula is as follows:

[0116] P i =(1-α i ) start +α i P end , α i ∈[0, 1]

[0117] Among them, α i is the time node t i The scaling factor of i Satisfies the following relationship;

[0118]

[0119] S314, generating uniformly distributed posture nodes q between the starting point and the end point on the spherical surface by linear interpolation i Specifically, first, R start and R end Convert to quaternion, use SLERP to interpolate between the start and end quaternion, get the attitude corresponding to each position, so far, get the attitude node q i ;

[0120] Posture node q i The calculation formula is as follows:

[0121]

[0122] Where θ is R start and R end The angle between the converted quaternions; q start and q end The posture nodes representing the starting and ending positions respectively;

[0123] S315: Set the position node P i , attitude node q i The poses of multiple control points located between the starting point and the end point are obtained by merging to ensure that the path is uniform and smooth in space and time. The pose information of the end effector's starting point and end point and the poses of multiple control points constitute the end effector pose information.

[0124] In some embodiments, reference Figure 7 , the S4 specifically comprises the following steps:

[0125] S41. Create an objective function for trajectory planning based on the speed and acceleration curves of each joint and the physical constraints of the robot arm; the objective function is as follows:

[0126] F(x)=[minQ1, minQ2, minQ3]

[0127] h j (t) = 0, j = 1, 2, ..., m

[0128] g l (t)≤0, l=1, 2, ..., n

[0129] Where F(x) represents the objective function; h j (t) and g l (t) represents the physical constraints of the manipulator constructed by the equality and inequality constraints respectively; Q1 is the total time of the manipulator's initial and final position movement; Q2 is the average acceleration of the joint movement; Q3 is the manipulator's joint pulsation;

[0130] S42, using the posture of the optimal assembly position obtained in S2 and the posture change curves of each joint of the dual manipulator obtained in S3, and calculating each joint path in the joint space by inverse kinematics to obtain a joint position sequence;

[0131] S43, initialize relevant parameters, and randomly generate the initial parent population G from the joint position sequence r , set the population size, and generate a descendant population M with the same population size as the parent by simulating hybridization and mutation r ;

[0132] S44, perform population fusion on the current parent population and the current child population to obtain a fused population W r , using the fast non-dominated sorting strategy, the fusion population W r Perform stratification to obtain non-dominated sets; calculate and sort the local crowding of all sets;

[0133] S45, fill the non-dominated set into the next parent population G in layers r+1 If the population size exceeds the set population size when filling all individuals of a certain level of non-dominated set, some individuals are filled into the parent population G in the order of individual crowding degree. r+1 , until the parent population G r+1 It terminates when the size of the population is equal to the set population size;

[0134] S46, parent population Gr+1 After conventional genetic operations, the offspring population M is generated r+1 ; Population generation number r = r + 1;

[0135] S47, loop S44 to S46 until the number of iterations reaches the set value or a solution set that meets the requirements is found;

[0136] S48, outputting the optimal solution set of the frontier surface that satisfies the objective function and the physical constraints of the robot arm; then selecting the optimal solution from the optimal solution set of the frontier surface according to the operability mathematical model and the optimal selection principle, so as to obtain the required optimal trajectory.

[0137] In some embodiments, Q1, Q2, and Q3 in S41 respectively satisfy the following relationships:

[0138]

[0139] Among them, j i is the joint pulsation shock; T is the movement time; a i represents the acceleration of the i-th joint; h is the number of trajectory segments between path points.

[0140] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be covered within the protection scope of the present invention. In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the ability of ordinary technicians in the field to implement. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A trajectory planning method for optimal collaborative assembly of dual robotic arms, characterized in that: The steps include: S1, determine the improved DH parameters of the robot arm and use the Monte Carlo method to obtain the coordinate range of the collaborative space; S2, solving the optimal assembly position of the dual robotic arms by particle swarm algorithm within the coordinate range of the collaborative space; S3. Based on the posture information of the end effectors of the dual robotic arms, the B-spline interpolation method is used to calculate the change parameters of each joint of the dual robotic arms, and the velocity and acceleration curves of each joint are constructed according to the change parameters of each joint; S4. Construct the objective function according to the velocity and acceleration curves of each joint and the physical constraints of the robot arm, and calculate the joint position sequence according to the posture of the optimal assembly position and the posture change curves of each joint of the dual robot arm; then use the NSGA-II optimization algorithm to select the optimal trajectory that meets the objective function and the physical constraints of the robot arm from the joint position sequence.

2. The trajectory planning method according to claim 1, characterized in that: The S1 specifically includes the following steps: S11, first determine the model of the dual robotic arm, and then confirm the improved DH parameters of the dual robotic arm, the improved DH parameters including the number of joints and connecting rods, the rotation angles of each joint, the offset of each connecting rod, the rod length of each connecting rod, and the rotation angle of each connecting rod; S12, establishing a platform coordinate system in the middle of the platform where the dual robotic arms are located, and the dual robotic arms are arranged around the platform coordinate system; then establishing a base coordinate system on the base on the top of the platform, and the two bases on the dual robotic arms are symmetrically arranged on both sides of the base, and the base coordinate system is flush with the two bases of the dual robotic arms; Then, coordinate systems are established on the base, each joint and the end effector of the robot arm in turn; the coordinate system on the end effector is called the end coordinate system; S13, according to the improved DH parameters of the dual robotic arms and the displacement parameters of each switch and connecting rod of the dual robotic arms, and according to forward kinematics, solving the transformation matrix of the end effector relative to the platform coordinate system; S14, determining the rotation angles of each joint of the dual manipulators through inverse kinematics based on the transformation matrix of the end effector relative to the platform coordinate system and the known end effector position and direction; S15, using the Monte Carlo method to sample the workspace of the dual robotic arms and draw its point cloud image, generating joint combinations through a random value method, and obtaining a coordinate set of the end effectors of the two robotic arms; S16. Finally, the coordinate range in the collaborative space is calculated through the coordinate sets of the two robot end effectors.

3. The trajectory planning method according to claim 2, characterized in that: The calculation formula of the transformation matrix of the end effector relative to the platform coordinate system in S13 is as follows: in, represents the transformation matrix from the end effector to the platform coordinate system; S represents the transformation matrix from the platform coordinate system to the robot base coordinate system; Represents the transformation matrix from the robot end effector coordinate system to the dual robot base coordinate system; where the transformation matrix The following relationship is satisfied: in, The transformation matrix representing the coordinate system of the first joint of the robot arm to the coordinate system of the base of the robot arm; Represents the transformation matrix from the second joint coordinate system of the robot arm to the first joint coordinate system of the robot arm; Represents the transformation matrix from the third joint coordinate system of the robot arm to the second joint coordinate system of the robot arm; Represents the transformation matrix from the fourth joint coordinate system of the robot arm to the third joint coordinate system of the robot arm; Represents the transformation matrix from the fifth joint coordinate system of the robot arm to the fourth joint coordinate system of the robot arm; Represents the transformation matrix from the sixth joint coordinate system of the robot to the fifth joint coordinate system of the robot.

4. The trajectory planning method according to claim 3, characterized in that: The calculation formula of each joint angle of the dual robotic arm in S14 is as follows: The calculation formula of the first joint angle is: Where θ1 represents the first joint rotation angle; p x 、p y They represent the x-axis and y-axis coordinates of the end effector of the robot arm in the platform coordinate system respectively; d4 represents the offset of the fourth link; The calculation formula of the second joint angle is: Where θ2 represents the second joint rotation angle; A, B and C are three different intermediate variables; and A, B and C satisfy the following relationship respectively: A=-2B2a2 B=2B1a2 <h2 style=";text-align:left;direction:ltr">C=B1<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +B2<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +a2<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> -a3<h2 style=";text-align:left;direction:ltr"> 2 B1=c1p x +s1p y -d5s 234 <h2 style=";text-align:left;direction:ltr">B2=p<h2 style=";text-align:left;direction:ltr"> z <h2 style=";text-align:left;direction:ltr"> -d1+d5c<h2 style=";text-align:left;direction:ltr"> 234 Where a2 is the length of the second link; a3 is the length of the third link; c1 is the cosine value of the first joint rotation angle; s1 is the sine value of the first joint rotation angle; p z represents the z-axis coordinate of the end effector of the robot arm in the platform coordinate system; d1 represents the offset of the first link; d5 represents the offset of the fifth link; s 234 represents the sine value of the sum of the second joint rotation angle, the third joint rotation angle and the fourth joint rotation angle; c 234 represents the cosine value of the sum of the second joint angle, the third joint angle and the fourth joint angle; B1 and B2 are different intermediate variables; The calculation formula for the rotation angle of the third joint and the fourth joint is: θ3=θ 23 -θ2 θ4=θ 234 -θ 23 Where θ3 represents the third joint angle; θ4 represents the fourth joint angle; θ 23 represents the sum of the second joint angle and the third joint angle; θ 234 represents the sum of the second joint rotation, the third joint rotation and the fourth joint rotation; The calculation formula of the fifth joint angle is: θ5=atan2(s5,s1a x -c1a y ) Where θ5 represents the fifth joint rotation angle; a x 、a y They represent the direction vectors of the z-axis in the robot end coordinate system on the x-axis and y-axis in the platform coordinate system respectively; s5 is the sine value of the fifth joint angle; The calculation formula of the sixth joint angle is: θ6=Atan2(c1n y -s1n x ,c1o y -s1o x )-Atan2(-s5,0) Among them, n x 、n y They represent the direction vectors of the x-axis of the robot end coordinate system in the x-axis and y-axis directions in the platform coordinate system respectively; x , o y They respectively represent the direction vectors of the y-axis of the robot end coordinate system in the x-axis and y-axis directions in the platform coordinate system.

5. The trajectory planning method according to claim 4, characterized in that: The S2 specifically includes the following steps: S21. First, set the initial parameters in the particle swarm algorithm: and set the initial value of the individual extreme value of each particle to zero; S22, randomly initialize the positions and velocities of multiple particles, each particle adjusts its own speed according to the individual extreme value and the group extreme value of all particles and outputs the position variable; Among them, the individual extreme value represents the best position of the particle itself; the group extreme value is the optimal solution selected from the individual extreme values ​​of all particles, which is used to guide the entire particle group to search for a better solution space; S23, inversely solving the position variables of each particle through the ikine function in the MATLAB robot toolbox; if the inverse kinematics solution exists, solving the Jacobian matrix of the dual manipulator through the jacob0 function, substituting it into the mathematical model of maneuverability, and obtaining the maneuverability value of the calculated particle; if the inverse solution does not exist, performing the inverse kinematics solution for each particle again; S24, comparing the current operability value of the calculated particle with the individual extreme value of the calculated particle, if the current operability value is higher, updating the individual extreme value of the calculated particle to the current operability value, and updating the position of the individual extreme value of the calculated particle; if the individual extreme value is higher, no update is performed; S25, iterate the loop S22 to S24 until the set conditions are met, compare the individual extreme values ​​of all particles, and obtain the individual extreme value with the highest operability value; use the position corresponding to the individual extreme value obtained in S25 as the posture of the optimal assembly position.

6. The trajectory planning method according to claim 5, characterized in that: The operability mathematical model in S23 is specifically as follows: Among them, ω D Indicates the operability value; L , R Represents the weight coefficient of the left and right arms in the dual robotic arm; ω L ,ω R is the maneuverability value of the left and right robotic arms.

7. The trajectory planning method according to claim 6, characterized in that: The S3 specifically includes the following steps: S31, obtaining the pose information of the start point and the end point of the end effector, and obtaining the poses of multiple control points located between the start point and the end point, wherein the pose information of the start point and the end point of the end effector and the poses of the multiple control points constitute the pose information of the end effector; S32, converting the end effector posture information into the angle value of each joint of the dual robotic arms; S33, determining the order and node vector of the B-spline curve; S34, constructing a B-spline interpolation of joint parameters according to the angle value of each joint of the dual robotic arms; S35, calculating and extracting the posture parameters of the joint change curve; S36. Calculate and construct the velocity and acceleration curves of each joint based on the posture parameters of the joint change curve.

8. The trajectory planning method according to claim 7, characterized in that: The S31 specifically includes the following steps: S311, define uniform time nodes, and obtain the position and posture information of the dual robotic arm end effectors at the beginning and end; the position and posture information of the dual robotic arm end effectors at the beginning and end is expressed by the formula as follows: Among them, T start and T end Respectively represent the location information of the starting point and the location information of the end point; R start and R end The rotation matrices representing the attitudes of the starting point and the end point, P star and P end The vectors representing the starting and ending positions respectively; S312: Divide the time period between the starting point and the end point into N uniform time nodes t i , time node t i The calculation formula is as follows: Among them, t start and t end Respectively represent the time of the starting point and the time of the end point; S313, using linear interpolation to generate evenly distributed position nodes P between the starting point and the end point i , location node P i The calculation formula is as follows: P i =(1-a i )P start +a i P end ,a i ∈[0,1] Among them, α i is the time node t i The scaling factor of i Satisfies the following relationship; S314, generating uniformly distributed posture nodes q between the starting point and the end point on the spherical surface by linear interpolation i , posture node q i The calculation formula is as follows: Where θ is R start and R end The angle between the converted quaternions; q start and q end The posture nodes representing the starting and ending positions respectively; S315: Set the position node P i , attitude node q i The poses of multiple control points located between the starting point and the end point are obtained by merging to ensure that the path is uniform and smooth in space and time. The pose information of the end effector's starting point and end point and the poses of multiple control points constitute the end effector pose information.

9. The trajectory planning method according to claim 8, characterized in that: The S4 specifically includes the following steps: S41. Create an objective function for trajectory planning based on the velocity and acceleration curves of each joint and the physical constraints of the robot arm; the objective function is as follows: F(x)=[minQ1, minQ2, minQ3] h j (t)=0,j=1,2,...,m g l (t)≤0,l=1,2,...,n Where F(x) represents the objective function; h j (t) and g l (t) represents the physical constraints of the manipulator constructed by the equality and inequality constraints respectively; Q1 is the total time of the manipulator's initial and final position movement; Q2 is the average acceleration of the joint movement; Q3 is the manipulator's joint pulsation; S42, using the posture of the optimal assembly position obtained in S2, and calculating each joint path in the joint space by inverse kinematics to obtain a joint position sequence; S43, initialize relevant parameters, and randomly generate the initial parent population G from the joint position sequence r , set the population size, and generate a descendant population M with the same population size as the parent by simulating hybridization and mutation r ; S44, perform population fusion on the current parent population and the current child population to obtain a fused population W r , using the fast non-dominated sorting strategy, the fusion population W r Perform stratification to obtain non-dominated sets; calculate and sort the local crowding of all sets; S45, fill the non-dominated set into the next parent population G in layers r+1 If the population size exceeds the set population size when filling all individuals of a certain level of non-dominated set, some individuals are filled into the parent population G in the order of individual crowding degree. r+1 , until the parent population G r+1 It terminates when the size of the population is equal to the set population size; S46, parent population G r+1 After conventional genetic operations, the offspring population M is generated r+1 ; Population generation number r = r + 1; S47, loop S44 to S46 until the number of iterations reaches the set value or a solution set that meets the requirements is found; S48, outputting the optimal solution set of the frontier surface that satisfies the objective function and the physical constraints of the robot arm; then selecting the optimal solution from the optimal solution set of the frontier surface according to the operability mathematical model and the optimal selection principle, so as to obtain the required optimal trajectory.

10. The trajectory planning method according to claim 8, characterized in that: Q1, Q2, and Q3 in S41 respectively satisfy the following relationships: Among them, j i is the joint pulsation shock; T is the movement time; a i represents the acceleration of the i-th joint; h is the number of trajectory segments between path points.

Images