A method and system for optimizing the motion trajectory of a robotic arm loading and unloading of a lens module

Abstract

The present invention discloses a method and system for optimizing the motion trajectory of a robot arm loading and unloading of a lens module, which relates to the technical field of robot arm trajectory optimization, and comprises the following steps: receiving robot arm related parameters, obtaining a robot arm rod coordinate system according to the robot arm related parameters, performing kinematic analysis on the robot arm rod coordinate system, and obtaining the robot arm end coordinates; wherein the robot arm related parameters are D_H parameters, including: connecting rod length a i‑1 , connecting rod torsion angle α i‑1 , connecting rod offset d i and the joint angle θ i ; Obtain the target path point and the robot arm joint angle, generate the robot arm motion trajectory according to the robot arm joint angle and the robot arm end coordinates combined with the target path point; Use the improved algorithm to optimize the robot arm motion trajectory.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm trajectory optimization, and in particular to a method and system for optimizing the robot arm loading and unloading motion trajectory of a lens module. Background Art

[0002] The loading and unloading parts of most focusing machines are completed manually or by guide rails, and the level of automation is insufficient, which affects production efficiency; the slide rail system is usually only suitable for specific materials. For materials of different shapes and sizes, customized design and manufacturing are required, which will reduce efficiency and require a certain amount of installation space. It may not be suitable for factories with limited space. Summary of the invention

[0003] In order to solve the deficiencies mentioned in the above background technology, the purpose of the present invention is to provide a method and system for optimizing the motion trajectory of a robot arm loading and unloading of a lens module.

[0004] In a first aspect, the purpose of the present invention can be achieved by the following technical solution: a method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module, the method comprising the following steps:

[0005] Receive the relevant parameters of the robot arm, obtain the robot arm rod coordinate system according to the relevant parameters of the robot arm, perform kinematic analysis on the robot arm rod coordinate system, and obtain the end coordinates of the robot arm; wherein the relevant parameters of the robot arm are D_H parameters, including: connecting rod length a i-1 , connecting rod torsion angle α i-1 , connecting rod offset d i and the joint angle θ i ;

[0006] Obtain the target path point and the robot arm joint angle, and generate the robot arm motion trajectory based on the robot arm joint angle and the robot arm end coordinates combined with the target path point;

[0007] The improved algorithm is used to optimize the motion trajectory of the robotic arm.

[0008] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the pre-established robot arm member coordinate system is established by adopting a modified D_H parameter modeling method.

[0009] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the process of inputting the robot arm related parameters into a pre-established robot arm rod coordinate system for kinematic analysis is as follows:

[0010]

[0011] Where: s i is sinθi , c i is cosθ i , sα i-1 is sinα i-1 , cα i-1 is cosα i-1 ;

[0012] a i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i distance;

[0013] α i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i The angle of

[0014] d i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The distance; θ i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The angle of

[0015] is the transformation matrix from coordinate system i-1 to coordinate system i;

[0016] The position coordinates and posture matrix of the end of the robot arm can be obtained by continuous right multiplication of the above formula, as shown in the following formula:

[0017]

[0018] In combination with the first aspect, in certain implementations of the first aspect, the method further includes: the motion trajectory of the robotic arm includes a loading trajectory, an unloading trajectory and a reset trajectory, and the motion trajectory of the robotic arm needs to be divided into three processes of lifting, carrying and lowering.

[0019] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the process of obtaining the target path point, combining the target path point with the robot arm joint angle and the robot arm end coordinates, and outputting the robot arm motion trajectory is as follows:

[0020] Four interpolation points are inserted between the starting point and the end point of each trajectory. The lifting process between the starting point and the first interpolation point is a quintic polynomial trajectory, the carrying process through the four middle interpolation points is a quartic polynomial trajectory, and the descending process through the last interpolation point and the end point is a quintic polynomial trajectory. The formula for the robot arm motion trajectory is a 54445 polynomial function, as follows:

[0021] S j1 (t) = a j1 t1 5 +a j2 t1 4 +a j3 t1 3 +a j4 t1 2 +a j5 t1+a j6 #(2-1)

[0022] S j2 (t) = a j7 t2 4 +a j8 t2 3 +a j9 t2 2 +a j10 t2+a j11 #(2-2)

[0023] S j3 (t) = a j12 t3 4 +a j13 t3 3 +a j14 t3 2 +a j14 t3+a j16 #(2-3)

[0024] S j4 (t) = a j17 t4 4 +a j18 t4 3 +a j19 t4 2 +a j20 t4+a j21 #(2-4)

[0025] S j5 (t) = a j22 t5 5 +a j23 t5 4 +a j24 t5 3 +a j25 t52 +a j26 t5+a j27 #(2-5)

[0026] Where: S j1 (t), S j2 (t), S j3 (t),S j4 (t),S j5 (t) are the quintic, quartic, quartic, quartic, and quintic polynomials of joint j, respectively. jn is the coefficient of each segment function, n is the coefficient number of each segment function, and n=1, 2, 3, ..., 27, t1 to t5 are the interpolation times of each segment trajectory planning respectively.

[0027] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the process of optimizing the trajectory of the robot arm motion trajectory using the improved algorithm is performed by combining a non-dominated sorting method with an elite strategy and a coati algorithm, wherein the motion time of each polynomial is used as a decision variable, time and energy are used as optimization targets, and a mathematical model of the multi-objective optimization target is established under the premise of satisfying parameter constraints as shown in formula (3-1):

[0028] F(x)=[minf1(x),minf2(x)]#(3-1)

[0029] stg ji (x)≥0,i=1,…,p#(3-2)

[0030] In formula (3-1), F(x) is the mathematical model of the optimization objective, x is the decision variable, f1(x) and f2(x) are sub-objective functions, g ji (x) is the constraint condition, p = 6; f1(x) is the total running time; f2(x) is the average acceleration of the joint; g j1 (x) is the speed constraint, g j2 (x) is the acceleration constraint.

[0031] In a second aspect, in order to achieve the above-mentioned purpose, the present invention discloses a robot arm loading and unloading motion trajectory optimization system for a lens module, comprising:

[0032] The data calculation module is used to receive the relevant parameters of the robot arm, obtain the robot arm rod coordinate system according to the relevant parameters of the robot arm, perform kinematic analysis on the robot arm rod coordinate system, and obtain the end coordinates of the robot arm, wherein the relevant parameters of the robot arm are D_H parameters, including: the length of the connecting rod a i-1 , connecting rod torsion angle α i-1 , connecting rod offset d i and the joint angle θ i;

[0033] The trajectory acquisition module is used to obtain the target path point and the robot arm joint angle, and generate the robot arm motion trajectory according to the robot arm joint angle and the robot arm end coordinates combined with the target path point;

[0034] The trajectory optimization module is used to optimize the motion trajectory of the robot arm using an improved algorithm.

[0035] In conjunction with the second aspect, in certain implementations of the second aspect, the system further includes: the mechanical arm rod coordinate system pre-established in the data calculation module is established by adopting a modfied D_H parameter modeling method;

[0036] Or the process of inputting the robot arm related parameters into the pre-established robot arm rod coordinate system for kinematic analysis in the data calculation module is as follows:

[0037]

[0038] Where: s i is sinθ i , c i is cosθ i , sα i-1 is sinα i-1 , cα i-1 is cosα i-1 ;

[0039] a i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i distance;

[0040] α i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i The angle of

[0041] d i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The distance; θ i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The angle of

[0042] is the transformation matrix from coordinate system i-1 to coordinate system i;

[0043] The position coordinates and posture matrix of the end of the robot arm can be obtained by continuous right multiplication of the above formula, as shown in the following formula:

[0044]

[0045] Preferably, the motion trajectory of the robot arm in the trajectory acquisition module includes a loading trajectory, a unloading trajectory and a reset trajectory, and the motion trajectory of the robot arm needs to be divided into three processes: lifting, carrying and descending;

[0046] Preferably, the target path point is obtained in the trajectory acquisition module, and the process of outputting the motion trajectory of the robotic arm is as follows:

[0047] Four interpolation points are inserted between the starting point and the end point of each trajectory. The lifting process between the starting point and the first interpolation point is a quintic polynomial trajectory, the carrying process through the four middle interpolation points is a quartic polynomial trajectory, and the descending process through the last interpolation point and the end point is a quintic polynomial trajectory. The formula for the robot arm motion trajectory is a 54445 polynomial function, as follows:

[0048] S j1 (t) = a j1 t1 5 +a j2 t1 4 +a j3 t1 3 +a j4 t1 2 +a j5 t1+a j6 #(2-1)

[0049] S j2 (t) = a j7 t2 4 +a j8 t2 3 +a j9 t2 2 +a j10 t2+a j11 #(2-2)

[0050] S j3 (t) = a j12 t3 4 +a j13 t3 3 +a j14 t3 2 +a j14 t3+a j16 #(2-3)

[0051] S j4 (t) = a j17 t44 +a j18 t4 3 +a j19 t4 2 +a j20 t4+a j21 #(2-4)

[0052] S j5 (t) = a j22 t5 5 +a j23 t5 4 +a j24 t5 3 +a j25 t5 2 +a j26 t5+a j27 #(2-5)

[0053] Where: S j1 (t), S j2 (t), S j3 (t),S j4 (t),S j5 (t) are the quintic, quartic, quartic, quartic, and quintic polynomials of joint j, respectively. jn is the coefficient of each segment function, n is the coefficient number of each segment function, and n=1, 2, 3, ..., 27, t is the interpolation time of each segment trajectory planning;

[0054] Preferably, the process of optimizing the trajectory of the robot arm using the improved algorithm in the trajectory optimization module is carried out by combining the non-dominated sorting with the elite strategy and the coati algorithm, wherein the motion time of each polynomial is used as the decision variable, and time and energy are used as the optimization objectives. The mathematical model of this multi-objective optimization objective is established under the premise of satisfying the parameter constraints as shown in formula (3-1):

[0055] F(x)=[minf1(x),minf2(x)]#(3-1)

[0056] stg ji (x)≥0,i=1,…,p#(3-2)

[0057] In formula (3-1), F(x) is the optimization result, x is the decision variable, f1(x) and f2(x) are sub-objective functions, g ji (x) is the constraint condition, p = 6; f1(x) is the total running time; f2(x) is the average acceleration of the joint; g j1 (x) is the speed constraint, g j2 (x) is the acceleration constraint.

[0058] In another aspect of the present invention, in order to achieve the above-mentioned purpose, a terminal device is disclosed, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the memory stores a computer program capable of running on the processor, and when the processor loads and executes the computer program, the method for optimizing the loading and unloading motion trajectory of a robotic arm of a lens module as described above is adopted.

[0059] In another aspect of the present invention, in order to achieve the above-mentioned purpose, a computer-readable storage medium is disclosed, in which a computer program is stored, and it is characterized in that when the computer program is loaded and executed by a processor, a method for optimizing the loading and unloading motion trajectory of a robotic arm of a lens module as described above is adopted.

[0060] Beneficial effects of the present invention:

[0061] The present invention adopts a 5-4-4-4-5 piecewise polynomial trajectory for the robot arm operation trajectory with a deep material tray, a certain limit on the movement space, and a certain lifting and lowering action during the movement process. The quintic polynomial is used as the movement trajectory during lifting and lowering, and the lifting trajectory and the lowering trajectory are connected with a quartic polynomial. In addition to the starting point and the end point, four interpolation points are used in the middle to limit the operation trajectory of the robot arm. The 5-4-4-4-5 piecewise polynomial trajectory has good smoothness, small jitter, high path planning flexibility, and can meet the high-precision requirements of the optical lens module loading and unloading trajectory.

[0062] The proposed piecewise polynomial trajectory combines the quintic polynomial and the quartic polynomial, which has the following advantages in robot trajectory planning: 1. It makes full use of the advantages of the quintic polynomial and the quartic polynomial: the quintic polynomial can provide a smoother trajectory, while the quartic polynomial has a simpler calculation and implementation method. By combining the two, the calculation amount and implementation complexity can be reduced while ensuring the smoothness of the trajectory; 2. Smoothness: The combination of the quintic polynomial and the quartic polynomial can provide a smoother trajectory. The quintic polynomial can fully consider the continuity of velocity and acceleration to achieve a smooth transition. The quartic polynomial can be used to connect different segments to avoid discontinuity; 3. Real-time performance: Due to the simplicity of the quartic polynomial, it is easier to calculate and update in real time. In applications that require real-time or fast planning of robot trajectories, this combination can provide higher efficiency and response speed; 4. Control performance: The piecewise polynomial combined with the quintic polynomial and the quartic polynomial can also provide better control performance. By reasonably selecting the connection method and transition conditions between different segments, the speed and acceleration of the robot can be better controlled to meet the requirements of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative work.

[0064] Figure 1 It is a schematic flow chart of the method of the present invention;

[0065] Figure 2 It is a schematic diagram of the algorithm optimization process of the present invention;

[0066] Figure 3 It is a schematic diagram of the Pareto optimal frontier for optimizing the trajectories of joint 1 and joint 2 of the robot of the present invention;

[0067] Figure 4 It is a schematic diagram of the running trajectory curve of the robot arm before and after optimization of the present invention;

[0068] Figure 5 Schematic diagram of the robot arm speed trajectory curve before and after optimization of the present invention;

[0069] Figure 6 Schematic diagram of the acceleration trajectory curve of the robot arm before and after optimization of the present invention;

[0070] Figure 7 It is a schematic diagram of the DH parameter coordinate system of the present invention;

[0071] Figure 8 It is a schematic diagram of the system structure of the present invention. DETAILED DESCRIPTION

[0072] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0073] Embodiment 1:

[0074] The following is an introduction to the relevant terms involved in the embodiments of the present application:

[0075] DH parameters: DH parameters (Denavit-Hartenberg parameters) are parameters used in manipulators and robotics to describe the relative position and orientation between links and joints. It simplifies the description of the manipulator by defining four variables.

[0076] Kinematic analysis of the robot arm Kinematic analysis of the robot arm needs to derive the robot arm's position, velocity, acceleration and other motion parameters from the perspective of geometry and vector algebra. Among them, the robot arm's position and posture parameters include position and posture. The position parameter represents the coordinates of the end of the robot arm in space, and the posture represents the direction of the robot arm in space.

[0077] The posture parameters of the robot are generally described in the form of Euler angles, quaternions or rotation matrices. Among them, Euler angles are a commonly used description method, which decomposes the posture of the robot into rotation angles around three coordinate axes. However, the limitations of Euler angles are that they have problems such as universal lock and singularity. Therefore, in practical applications, quaternions and rotation matrices are often more commonly used. The movement speed and acceleration of the robot can be calculated using kinematic equations. The kinematic equations describe the relationship between the velocity and acceleration of the end of the robot and the angles and velocities of each joint of the robot, and are generally solved using gradient equations or inverse dynamics equations.

[0078] Robotic arm motion trajectory planning: The motion trajectory planning of the robotic arm refers to determining the motion trajectory of the robotic arm through preset planning points to achieve the automated motion of the robotic arm. The motion trajectory planning needs to combine the kinematic characteristics of the robotic arm and the motion control strategy to select the appropriate path planning algorithm and control strategy.

[0079] like Figure 1 As shown, a method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module comprises the following steps:

[0080] Receive relevant parameters of the robot arm, obtain the robot arm rod coordinate system according to the relevant parameters of the robot arm, perform kinematic analysis on the robot arm rod coordinate system, and obtain the end coordinates of the robot arm;

[0081] Among them, the robot arm related parameters are D_H parameters, including: connecting rod length a i-1 , connecting rod torsion angle α i-1 , connecting rod offset d i and the joint angle θ i ;

[0082] Specifically, the present invention is further described below by way of embodiments:

[0083] Table 1 Six-axis industrial robotic arm D_H parameter table

[0084]

[0085] The pre-established robot arm member coordinate system is established by adopting the modfied D_H parameter modeling method;

[0086] The process of inputting the relevant parameters of the robot arm into the pre-established robot arm rod coordinate system for kinematic analysis: According to the D_H parameter table given in the previous section, the parameters are substituted into the general formula of the translation and rotation homogeneous change matrix of the two adjacent link coordinate systems i-1 to the coordinate system i, as shown in formula (1-1):

[0087]

[0088] Where: s i is sinθ i , c i is cosθ i , sα i-1 is sinα i-1 , cα i-1 is cosα i-1 ;

[0089] a i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i distance;

[0090] α i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i The angle of

[0091] d i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The distance; θ i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The angle of

[0092] is the transformation matrix from coordinate system i-1 to coordinate system i;

[0093] The position coordinates and posture matrix of the end of the robot arm can be obtained by continuous right multiplication of the above formula, as shown in the following formula:

[0094]

[0095] Obtain the target path point and the robot arm joint angle, and generate the robot arm motion trajectory based on the robot arm joint angle and the robot arm end coordinates combined with the target path point;

[0096] Among them, it is required to plan an ideal operation trajectory for the collaborative robot to load and unload the optical lens module while satisfying the motion parameter constraints of each joint of the robotic arm.

[0097] The robot arm's running trajectory can be divided into loading trajectory, unloading trajectory and reset trajectory. Since there is a certain depth in the processing position of the tray and the lens module, each trajectory needs to be divided into three processes: lifting, transporting and descending. Four interpolation points are inserted between the starting point and the end point of each trajectory. The lifting process between the starting point and the first interpolation point is a 5th-order polynomial trajectory, the transport process through the four middle interpolation points is a 4th-order polynomial trajectory, and the descent process through the last interpolation point and the end point is a quintic polynomial trajectory. The entire trajectory is continuous and smooth. The planned trajectory meets the constraints of each joint parameter and can accurately pass the key points of the taught target path, and needs to meet the optimal optimization of the two optimization goals of time and energy consumption.

[0098] The 5-4-4-4-5 polynomial function is constructed as shown in the following equations (2-1)-(2-2):

[0099] S j1 (t) = a j1 t1 5 +a j2 t1 4 +a j3 t1 3 +a j4 t1 2 +a j5 t1+a j6 #(2-1)

[0100] S j2 (t) = a j7 t2 4 +a j8 t2 3 +a j9 t2 2 +a j10 t2+a j11 #(2-2)

[0101] S j3 (t) = a j12 t3 4 +a j13 t3 3 +a j14 t3 2 +a j14 t3+a j16 #(2-3)

[0102] S j4 (t) = a j17 t4 4 +aj18 t4 3 +a j19 t4 2 +a j20 t4+a j21 #(2-4)

[0103] S j5 (t) = a j22 t5 5 +a j23 t5 4 +a j24 t5 3 +a j25 t5 2 +a j26 t5+a j27 #(2-5)

[0104] Where: S j1 (t), S j2 (t), S j3 (t),S j4 (t),S j5 (t) are the quintic, quartic, quartic, quartic, and quintic polynomials of joint j, respectively. j etc. are the coefficients of each segment function. The value of j ranges from 1 to 6. t is the interpolation time of each segment trajectory planning. j0 , interpolate the intermediate point x j1 With x j2 and the final point x j3 The constructed polynomial trajectory function should satisfy the following two conditions: 1. The positions of the starting point and the final point should be at x j0 and x j3 Corresponding and the velocity and acceleration are zero; 2. The displacement, velocity and acceleration of the two middle points are continuous. Through the above constraints, we can find a j1 to a j27 There are 27 unknown polynomial coefficients, and the derivation formulas of the polynomial coefficients are given by equations (2-6)-(2-8).

[0105]

[0106] X=[X0 X1 X2 X3 X4 X5 X6] T #(2-7)

[0107] X0=[x j0 0 0]

[0108] X1=[x j1 x j1 0 0]

[0109] X2=[xj2 x j2 0 0]

[0110] X3=[x j3 x j3 0 0]

[0111] X4=[x j4 x j4 0 0]

[0112] X5=[x j5 x j5 0 0]

[0113] X6=[x j6 0 0]

[0114] a=A -1 X=[a1 a2 a3 a4 a5] T

[0115] a1=[a j1 a j2 a j3 a j4 a j5 a j6 ]

[0116] a2=[a j7 a j8 a j9 a j10 a j11 ]

[0117] a3=[a j12 a j13 a j14 a j15 a j16 ]

[0118] a4=[a j17 a j18 a j19 a j20 a j21 ]

[0119] a5=[a j22 a j23 a j24 a j25 a j26 a j27 ]

[0120] Formula (2-6) is the time matrix in the polynomial matrix, formula (2-7) is the displacement matrix of joint j, and formula (2-8) is the coefficient matrix in the polynomial matrix.

[0121] The improved algorithm is used to optimize the motion trajectory of the robotic arm.

[0122] The process of optimizing the trajectory of the robot arm using the improved algorithm is carried out by combining the non-dominated sorting with the elite strategy and the coati algorithm. The motion time of each polynomial is used as the decision variable, and time and energy are the optimization targets. The mathematical model of this multi-objective optimization target is established under the premise of satisfying the parameter constraints as shown in formula (3-1):

[0123] F(x)=[minf1(x),minf2(x)]#(3-1)

[0124] stg ji (x)≥0,i=1,…,p#(3-2)

[0125] In formula (3-1), F(x) is the mathematical model of the optimization objective, x is the decision variable, f1(x) and f2(x) are sub-objective functions, g ji (x) is the constraint condition, p = 6; f1(x) is the total running time; f2(x) is the average acceleration of the joint; g j1 (x) is the speed constraint, g j2 (x) is the acceleration constraint.

[0126]

[0127] g j1 (x) = V jmax -|V j |#(3-5)

[0128] g j2 (x) = a jmax -|a j |#(3-6)

[0129] The updating strategy of population individuals in the multi-objective raccoon algorithm is mainly divided into the exploration phase and the exploitation phase.

[0130] In the exploration phase, it is assumed that half of the raccoons go up the tree and the other half stay underground and wait. The position update formula for the raccoons that go up the tree is:

[0131]

[0132] Where: is the updated position of the i-th raccoon individual in the j-th dimension, r is a random number between 0 and 1, Iguana j is the position of the iguana, I is a positive integer of 1 or 2, N is the population size, and m is the number of decision variables.

[0133] The underground raccoon explores the solution space based on the position of the iguana. This process is divided into two formulas for simulation.

[0134]

[0135] Where: is the ground position of the iguana in the jth dimension.

[0136]

[0137] Where: is the target value of the iguana in the jth dimension, F i,j is the target value of the i-th raccoon in the j-th dimension.

[0138] If the new raccoon individual is in a better position, update its position, otherwise keep the original position.

[0139]

[0140] Where: is the objective function value of the i-th raccoon in the new position, F i is the objective function value of the i-th raccoon in its original position.

[0141] In the exploitation phase, a safe position close to the current position of each raccoon is randomly generated near the current position as shown in formula (3-11):

[0142]

[0143] Where: is the local lower bound; is the local upper bound; t is the current number of iterations; T is the maximum number of iterations.

[0144]

[0145] Where: is the safe position of the i-th raccoon in the j-th dimension. If the new raccoon individual position is better, update its position, otherwise keep the original position.

[0146]

[0147] Where: is the objective function value of the i-th raccoon in a safe position.

[0148] The evolution process of this optimization algorithm is as follows:

[0149] Initialize the population P0 of the raccoon algorithm, the number of individuals in the initial population is N;

[0150] Acquire new species through exploration and exploitation and The number of individuals in each new population is N. and Merge to get a new population R i ;

[0151] Use non-dominated sorting to sort the new population R i Divide into levels, each level corresponds to a non-dominated set F i , for each newly generated non-dominated set F i Sort by congestion level;

[0152] Sort the non-dominated sets of each level in sorting order to obtain a new population R' i , take the new population R' i The first half of the individuals form the new generation population P i+1 ;

[0153] Determine whether the number of iterations of the raccoon algorithm has been reached. If so, stop the iteration and proceed to the next step. If not, return to step 2.

[0154] The flowchart of the specific optimization algorithm is as follows Figure 2 shown.

[0155] Initialize the population S pop is 100, and the number of iterations is 100. Given the path points in trajectory planning, the joint angles of the robot arm are shown in Table 2 below:

[0156] Table 2 Robotic arm path points

[0157]

[0158] The time required for the initial whole path planning is 6 seconds. Substituting each joint angle into the algorithm, the Pareto frontier of trajectory optimization can be obtained as follows: Figure 3 shown.

[0159] The optimal result is obtained by the algorithm in this paper. Then the 5-4-4-4-5 polynomial trajectory is analyzed to obtain the trajectory of each joint of the robot arm. The position, velocity, and acceleration change curves of the six joints before and after are as follows: Figure 4 , Figure 5 , Figure 6 shown.

[0160] Embodiment 2: In the second aspect, as Figure 8 As shown, in order to achieve the above-mentioned purpose, the present invention discloses a robot arm loading and unloading motion trajectory optimization system for a lens module, comprising:

[0161] The data calculation module is used to receive the relevant parameters of the robot arm, obtain the robot arm rod coordinate system according to the relevant parameters of the robot arm, perform kinematic analysis on the robot arm rod coordinate system, and obtain the end coordinates of the robot arm, wherein the relevant parameters of the robot arm are D_H parameters, including: the length of the connecting rod a i-1 , connecting rod torsion angle α i-1 , connecting rod offset d i and the joint angle θ i ;

[0162] The trajectory acquisition module is used to obtain the target path point and the robot arm joint angle, and generate the robot arm motion trajectory according to the robot arm joint angle and the robot arm end coordinates combined with the target path point;

[0163] The trajectory optimization module is used to optimize the motion trajectory of the robot arm using an improved algorithm.

[0164] In conjunction with the second aspect, in certain implementations of the second aspect, the system further includes: the mechanical arm rod coordinate system pre-established in the data calculation module is established by adopting a modfied D_H parameter modeling method;

[0165] Or the process of inputting the robot arm related parameters into the pre-established robot arm rod coordinate system for kinematic analysis in the data calculation module is as follows:

[0166]

[0167] Where: s i is sinθ i , c i is cosθ i , sα i-1 is sinα i-1 , cα i-1 is cosα i-1 ;

[0168] a i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i distance;

[0169] α i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i The angle of

[0170] d i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The distance; θi : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The angle of

[0171] is the transformation matrix from coordinate system i-1 to coordinate system i;

[0172] The position coordinates and posture matrix of the end of the robot arm can be obtained by continuous right multiplication of the above formula, as shown in the following formula:

[0173]

[0174] Preferably, the motion trajectory of the robot arm in the trajectory acquisition module includes a loading trajectory, a unloading trajectory and a reset trajectory, and the motion trajectory of the robot arm needs to be divided into three processes: lifting, carrying and descending;

[0175] Preferably, the target path point is obtained in the trajectory acquisition module, and the process of outputting the motion trajectory of the robotic arm is as follows:

[0176] Four interpolation points are inserted between the starting point and the end point of each trajectory. The lifting process between the starting point and the first interpolation point is a quintic polynomial trajectory, the carrying process through the four middle interpolation points is a quartic polynomial trajectory, and the descending process through the last interpolation point and the end point is a quintic polynomial trajectory. The formula for the robot arm motion trajectory is a 54445 polynomial function, as follows:

[0177] S j1 (t) = a j1 t1 5 +a j2 t1 4 +a j3 t1 3 +a j4 t1 2 +a j5 t1+a j6 #(2-1)

[0178] S j2 (t) = a j7 t2 4 +a j8 t2 3 +a j9 t2 2 +a j10 t2+a j11 #(2-2)

[0179] S j3 (t) = a j12 t3 4 +aj13 t3 3 +a j14 t3 2 +a j14 t3+a j16 #(2-3)

[0180] S j4 (t) = a j17 t4 4 +a j18 t4 3 +a j19 t4 2 +a j20 t4+a j21 #(2-4)

[0181] S j5 (t) = a j22 t5 5 +a j23 t5 4 +a j24 t5 3 +a j25 t5 2 +a j26 t5+a j27 #(2-5)

[0182] Where: S j1 (t), S j2 (t), S j3 (t),S j4 (t),S j5 (t) are the quintic, quartic, quartic, quartic, and quintic polynomials of joint j, respectively. jn is the coefficient of each segment function, n is the coefficient number of each segment function, and n=1, 2, 3, ..., 27, t1 to t5 are the interpolation times of each segment trajectory planning respectively.

[0183] Preferably, the process of optimizing the trajectory of the robot arm using the improved algorithm in the trajectory optimization module is carried out by combining the non-dominated sorting with the elite strategy and the coati algorithm, wherein the motion time of each polynomial is used as the decision variable, and time and energy are used as the optimization objectives. The mathematical model of this multi-objective optimization objective is established under the premise of satisfying the parameter constraints as shown in formula (3-1):

[0184] F(x)=[minf1(x),minf2(x)]#(3-1)

[0185] stg ji (x)≥0,i=1,…,p#(3-2)

[0186] In formula (3-1), F(x) is the mathematical model of the optimization objective, x is the decision variable, f1(x) and f2(x) are sub-objective functions, g ji (x) is the constraint condition, p = 6; f1(x) is the total running time; f2(x) is the average acceleration of the joint; g j1 (x) is the speed constraint, g j2 (x) is the acceleration constraint.

[0187] Based on the same inventive concept, the present invention also provides a computer device, which includes: one or more processors, and a memory for storing one or more computer programs; the program includes program instructions, and the processor is used to execute the program instructions stored in the memory. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is used to implement one or more instructions, specifically for loading and executing one or more instructions in a computer storage medium to implement the above method.

[0188] It needs to be further explained that, based on the same inventive concept, the present invention also provides a computer storage medium, on which a computer program is stored, and the computer program is executed by a processor to execute the above method. The storage medium can adopt any combination of one or more computer-readable media. The computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electrical, magnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program, which can be used by an instruction execution system, device or device or used in combination with it.

[0189] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0190] The above shows and describes the basic principles, main features and advantages of the present disclosure. Those skilled in the art should understand that the present disclosure is not limited by the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present disclosure. Without departing from the spirit and scope of the present disclosure, the present disclosure may have various changes and improvements, and these changes and improvements fall within the scope of the present disclosure to be protected.

Claims

1. A method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module, characterized in that: The method comprises the following steps: Receive the relevant parameters of the robot arm, obtain the robot arm rod coordinate system according to the relevant parameters of the robot arm, perform kinematic analysis on the robot arm rod coordinate system, and obtain the end coordinates of the robot arm; wherein the relevant parameters of the robot arm are D_H parameters, including: connecting rod length a i-1 , connecting rod torsion angle α i-1 , connecting rod offset d i and the joint angle θ i ; Obtain the target path point and the robot arm joint angle, and generate the robot arm motion trajectory based on the robot arm joint angle and the robot arm end coordinates combined with the target path point; The process of obtaining the target path point and outputting the robot arm motion trajectory according to the robot arm joint angle and the robot arm end coordinates is as follows: Four interpolation points are inserted between the starting point and the end point of each trajectory. The lifting process between the starting point and the first interpolation point is a quintic polynomial trajectory, the carrying process through the four middle interpolation points is a quartic polynomial trajectory, and the descending process through the last interpolation point and the end point is a quintic polynomial trajectory. The formula for the robot arm motion trajectory is a 54445 polynomial function, as follows: S j1 (t)=a j1 t1 5 +a j2 t1 4 +a j3 t1 3 +a j4 t1 2 +a j5 t1+a j6 #(2-1) S j2 (t)=a j7 t2 4 +a j8 t2 3 +a j9 t2 2 +a j10 t2+a j11 #(2-2) S j3 (t)=a j12 t3 4 +a j13 t3 3 +a j14 t3 2 +a j14 t3+a j16 #(2-3) S j4 (t)=a j17 t4 4 +a j18 t4 3 +a j19 t4 2 +a j20 t4+a j21 #(2-4) S j5 (t)=a j22 t5 5 +a j23 t5 4 +a j24 t5 3 +a j25 t5 2 +a j26 t5+a j27 #(2-5) Where: S j1 (t), S j2 (t), S j3 (t),S j4 (t),S j5 (t) are the quintic, quartic, quartic, quartic, and quintic polynomials of joint j, respectively. jn is the coefficient of each segment function, n is the coefficient number of each segment function, and n=1, 2, 3, ..., 27, t1 to t5 are the interpolation times of each segment trajectory planning respectively; Use the improved algorithm to optimize the trajectory of the robot arm; The process of optimizing the trajectory of the robot arm using the improved algorithm is carried out by combining the non-dominated sorting with the elite strategy and the coati algorithm, wherein the motion time of each polynomial is used as the decision variable, and time and energy are used as the optimization objectives. The mathematical model of this multi-objective optimization objective is established under the premise of satisfying the parameter constraints as shown in formula (3-1): F(x)=[minf1(x),minf2(x)]#(3-1) s.t.g ji (x)≥0,i=1,…,p#(3-2) In formula (3-1), F(x) is the mathematical model of the optimization objective, x is the decision variable, f1(x) and f2(x) are sub-objective functions, g ji (x) is the constraint condition, p = 6; f1(x) is the total running time; f2(x) is the average acceleration of the joint; g j1 (x) is the speed constraint, g j2 (x) is the acceleration constraint.

2. The method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module according to claim 1, characterized in that: The pre-established robot arm member coordinate system is established by adopting the modfiedD_H parameter modeling method.

3. The method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module according to claim 1, characterized in that: The process of inputting the robot arm related parameters into the pre-established robot arm member coordinate system for kinematic analysis is as follows: Where: s i is sinθ i , c i is cosθ i , sα i-1 is sinα i-1 , cα i-1 is cosα i-1 ; a i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i distance; α i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i The angle of d i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The distance; θ i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The angle of is the transformation matrix from coordinate system i-1 to coordinate system i; The position coordinates and posture matrix of the end of the robot arm can be obtained by continuous right multiplication of the above formula, as shown in the following formula:

4. The method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module according to claim 1, characterized in that: The motion trajectory of the robotic arm includes a loading trajectory, an unloading trajectory and a reset trajectory, and the motion trajectory of the robotic arm needs to be divided into three processes: lifting, transporting and lowering.

5. A robot arm loading and unloading motion trajectory optimization system for a lens module, characterized in that: include: The data calculation module is used to receive the relevant parameters of the robot arm, obtain the robot arm rod coordinate system according to the relevant parameters of the robot arm, perform kinematic analysis on the robot arm rod coordinate system, and obtain the end coordinates of the robot arm, wherein the relevant parameters of the robot arm are D_H parameters, including: the length of the connecting rod a i-1 , connecting rod torsion angle α i-1 , connecting rod offset d i and the joint angle θ i ; The trajectory acquisition module is used to obtain the target path point and the robot arm joint angle, and generate the robot arm motion trajectory according to the robot arm joint angle and the robot arm end coordinates combined with the target path point; The target path point is obtained in the trajectory acquisition module. The process of outputting the motion trajectory of the robot arm is as follows: Four interpolation points are inserted between the starting point and the end point of each trajectory. The lifting process between the starting point and the first interpolation point is a quintic polynomial trajectory, the carrying process through the four middle interpolation points is a quartic polynomial trajectory, and the descending process through the last interpolation point and the end point is a quintic polynomial trajectory. The formula for the robot arm motion trajectory is a 54445 polynomial function, as follows: S j1 (t)=a j1 t1 5 +a j2 t1 4 +a j3 t1 3 +a j4 t1 2 +a j5 t1+a j6 #(2-1) S j2 (t)=a j7 t2 4 +a j8 t2 3 +a j9 t2 2 +a j10 t2+a j11 #(2-2) S j3 (t)=a j12 t3 4 +a j13 t3 3 +a j14 t3 2 +a j14 t3+a j16 #(2-3) S j4 (t)=a j17 t4 4 +a j18 t4 3 +a j19 t4 2 +a j20 t4+a j21 #(2-4) S j5 (t)=a j22 t5 5 +a j23 t5 4 +a j24 t5 3 +a j25 t5 2 +a j26 t5+a j27 #(2-5) Where: S j1 (t), S j2 (t), S j3 (t),S j4 (t),S j5 (t) are the quintic, quartic, quartic, quartic, and quintic polynomials of joint j, respectively. jn is the coefficient of each segment function, n is the coefficient number of each segment function, and n=1, 2, 3, ..., 27, t1 to t5 are the interpolation times of each segment trajectory planning respectively; Trajectory optimization module, used to optimize the trajectory of the robot arm using an improved algorithm; The process of trajectory optimization of the robot arm motion trajectory using the improved algorithm in the trajectory optimization module is carried out by combining the non-dominated sorting with the elite strategy and the coati algorithm. The motion time of each polynomial is used as the decision variable, and time and energy are the optimization targets. The mathematical model of this multi-objective optimization target is established under the premise of satisfying the parameter constraints as shown in formula (3-1): F(x)=[minf1(x),minf2(x)]#(3-1) s.t.g ji (x)≥0,i=1,…,p#(3-2) In formula (3-1), F(x) is the mathematical model of the optimization objective, x is the decision variable, f1(x) and f2(x) are sub-objective functions, g ji (x) is the constraint condition, p = 6; f1(x) is the total running time; f2(x) is the average acceleration of the joint; g j1 (x) is the speed constraint, g j2 (x) is the acceleration constraint.

6. The robot arm loading and unloading motion trajectory optimization system of a lens module according to claim 5, characterized in that: The mechanical arm rod coordinate system pre-established in the data calculation module is established by adopting the modfiedD_H parameter modeling method; Or the process of inputting the robot arm related parameters into the pre-established robot arm rod coordinate system for kinematic analysis in the data calculation module is as follows: Where: s i is sinθ i , c i is cosθ i , sα i-1 is sinα i-1 , cα i-1 is cosα i-1 ; a i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i distance; α i-1 : is the coordinate axis x i-1 Direction, coordinate axis z i-1 With coordinate axis z i The angle of d i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The distance; θ i : is the coordinate axis z i Direction, coordinate axis x i-1 With coordinate axis z i The angle of is the transformation matrix from coordinate system i-1 to coordinate system i; The position coordinates and posture matrix of the end of the robot arm can be obtained by continuous right multiplication of the above formula, as shown in the following formula: The motion trajectory of the robot arm in the trajectory acquisition module includes a loading trajectory, an unloading trajectory and a reset trajectory. The motion trajectory of the robot arm needs to be divided into three processes: lifting, transporting and descending.

7. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that: The memory stores a computer program that can be run on the processor. When the processor loads and executes the computer program, the method for optimizing the loading and unloading motion trajectory of a robot arm of a lens module according to any one of claims 1 to 4 is adopted.

8. A computer-readable storage medium having a computer program stored therein, characterized in that: When the computer program is loaded and executed by the processor, the method for optimizing the motion trajectory of a robot arm loading and unloading of a lens module according to any one of claims 1 to 4 is adopted.

Images