A multi-target trajectory obstacle avoidance planning method for a robotic arm based on an improved MOABC algorithm

Abstract

The present invention discloses a multi-target trajectory obstacle avoidance planning method for a manipulator based on an improved MOABC algorithm, the method comprising: 1) constructing a manipulator and obstacle model; 2) defining a collision detection method; 3) constructing a segmented joint trajectory; 4) constructing and constraining a multi-target function; 5) constructing an external archive search strategy; 6) introducing an improved simulated annealing algorithm; 7) combining a penalty function constraint method with a multi-target optimization idea; 8) optimizing multi-target variables with an improved multi-target artificial bee colony algorithm. The present invention can plan a Pareto solution set that is collision-free, meets constraints, has both elitism and diversity, and achieves multiple objectives at the same time, has high efficiency, and can also be extended to a serial manipulator with any degree of freedom to use multiple high-order polynomials to perform multi-target trajectory obstacle avoidance planning for multiple obstacles.

Description

Technical Field

[0001] The invention relates to a robot arm trajectory planning technology, in particular to a robot arm multi-objective trajectory obstacle avoidance planning method based on an improved multi-objective artificial bee colony MOABC (Multi-objective artificial bee colony, MOABC for short) algorithm. Background Art

[0002] In recent years, with the continuous advancement of my country's industrialization and modernization, the robot market and scale have expanded unprecedentedly. my country has now become the world's largest robot market. The robotic arm is an important part of the robot system. The robotic arm plays an increasingly important role in various fields such as machinery manufacturing, transportation, agricultural production, aerospace, and medical health. However, the robotic arm system is a multi-input and multi-output strongly coupled nonlinear system. In-depth research on various aspects of robotic arm technology has certain practical significance.

[0003] Planning and optimizing the trajectory of the robot arm can make the robot arm operate more efficiently as expected. Robot arm obstacle avoidance planning refers to planning a path from the starting position to the target position without colliding with obstacles in a working environment with obstacles. There are four main directions of obstacle avoidance planning: one is to use heuristic algorithms; the second is to use field thinking methods; the third is to use reinforcement learning methods; and the fourth is to use sampling algorithms. Common evaluation indicators for the effect of robot arm path planning include obstacle avoidance, time, energy consumption, impact, path, smoothness, etc. The optimization target objects can usually be divided into single target, combined target, and multi-target optimization.

[0004] Many tasks of the robot arm are mainly suitable for fast, precise and repetitive work. Therefore, the requirements for various indicators of the robot arm are getting higher and higher. It is expected that the robot arm can realize automatic control in a complex environment and plan a motion trajectory that meets the requirements of safety, efficiency, energy saving and stability. This is the first step in the work of the robot arm. It can be seen that exploring a multi-target trajectory obstacle avoidance planning method for a robot arm with arbitrary degrees of freedom and arbitrary planning space has certain research value for the safety and application promotion of the robot arm. Summary of the invention

[0005] The purpose of the present invention is to provide a multi-target trajectory obstacle avoidance planning method for a manipulator based on an improved MOABC algorithm to address the deficiencies of the prior art. This method can plan a Pareto solution set that is collision-free, satisfies constraints, and achieves a relatively optimal non-dominated solution frontier for multiple objectives at the same time, thereby obtaining an ideal trajectory that is both elite and diverse and has high efficiency.

[0006] The technical solution for achieving the purpose of the present invention is:

[0007] A multi-objective trajectory obstacle avoidance planning method for a manipulator based on an improved MOABC algorithm, comprising the following steps:

[0008] 1) Construct a manipulator and obstacle model: Establish the joint coordinate system of the manipulator model according to the D-H convention. Derive from the base to the adjacent joint and then to the end effector through the D-H parameter method, establish the link transmission connection, obtain the D-H parameters, and establish the manipulator model. Use the sphere envelope method to model the obstacles with uncertain shapes, that is, take the center of the two points with the farthest distance on the obstacle as the center of the sphere, and use the length of the line segment connecting the two points as the diameter for envelope;

[0009] 2) Define the collision detection method: The collision detection between the manipulator and the obstacle can be simplified to judge the position relationship between the straight line segment i and the center point O of the sphere by the D-H parameter relationship, that is, superimpose the maximum radial radius of the manipulator link on the radius of the sphere envelope. Define the maximum radial radius of the manipulator as R1, the radius of the obstacle as R0, and the distance from the straight line segment of the link to the center of the obstacle as d it , the obstacle center coordinate O is (x0, y0, z0), and the start and end point coordinates of each link are (x i , y i , z i ) and (x i+1 , y i+1 , z i+1 ), establish the mathematical model of each link segment by the homogeneous coordinate transformation method according to the D-H parameters, and the coordinate equation of the straight line segment i is shown in formula (1):

[0010]

[0011] Among them, λ is the proportional parameter, and the value range is 0 to 1. x, y, z represent the coordinates of the straight line segment i. Then, detect the distance d iλ between the straight line segment i and the center of the sphere: If d iλ < R0 + R1, then the manipulator collides with the obstacle. If d iλ > R0 + R1, then the manipulator does not collide with the obstacle. If during the movement of the manipulator, it is detected that the obstacle does not collide with any of the i links, it means that the manipulator has completed obstacle avoidance;

[0012] 3) Construct a segmented joint trajectory: Each joint of the manipulator sets intermediate points, and uses a segmented fifth-order polynomial to fit the key points of each joint variable to construct the operating trajectory of the manipulator to bypass the obstacle. The function expression of the fifth-order polynomial is shown in formula (2):

[0013] θ (t) = a0 + a1t + a2t 2 + a3t 3 + a4t 4+a5t 5 (2)

[0014] Where t represents the motion time, a0, ..., a5 represent the unknown coefficients, and the coefficients of the quintic polynomial are written in vector form, then formula (2) can be simplified as shown in formula (3):

[0015] θ (t) =[1,t,t 2 ,t 3 ,t 4 ,t 5 ]·a (3),

[0016] Assume that the position, velocity and acceleration of each joint of the robot arm at the initial time t0 are θ0, v0, a0 respectively, and at the end time t f The position, velocity and acceleration are θ f ,v f ,a f , where t0 = 0, let v0 = a0 = v f =a f =0, let the middle point be at t m The position at time θ m , construct the equation as shown in formula (4):

[0017]

[0018] By analogy with formula (3) and formula (4), if the motion trajectory of the robot contains n intermediate points, there are n+1 segments of running time and n+2 moments, which are represented by n+1 segments of quintic polynomials. Then 6(n+1) constraint equations are required to determine the coefficients. The simultaneous formula (3) and formula (4) are written in the form of matrix multiplication, and the coefficient matrix A is obtained as shown in formula (5):

[0019] A=M -1 *Q (5),

[0020] Among them, M is the matrix of the trajectory with respect to time t, A contains the unknown coefficients of the trajectory, and Q is the known constraint matrix;

[0021] 4) Multi-objective function construction and constraints: The optimization objective is defined as shown in formula (6):

[0022]

[0023]

[0024]

[0025] Where m is the number of path points, M is the number of joints of the robot, n is the number of interpolation points obtained from H / s, s is the interpolation interval, f1 is the running time, f2 is the sum of joint acceleration increments, and f3 is the sum of joint jerk increments.

[0026] At the same time, the movement of the robot arm is also subject to the following constraints:

[0027] Position constraint: θ ijmin ≤θ ij ≤θ ijmax ;

[0028] Speed ​​Constraints:

[0029] Acceleration constraints:

[0030] Jerk constraint:

[0031] Among them, θ ijmax ,v ijmax ,a ijmax ,j ijmax are the maximum values ​​of the corresponding joint displacement, velocity, acceleration and jerk respectively; θ ijmin , is the minimum value of the corresponding joint displacement; θ ij ,a ij ,v ij ,j ij are the displacement, velocity, acceleration and jerk of the i-th interpolation point of the corresponding joint respectively;

[0032] 5) Construct an external archive search strategy: Adopt the multi-objective artificial bee colony MOABC algorithm. In the bee collection stage, replace the random limited search method originally used by bees. Set the size R of the external archive set E, guide the population to optimize according to the elite information of the external archive, add the non-dominated solutions generated during the search process to the archive, and randomly select the jth dimension e of any solution from the external archive set E. j a Introduced into the search method, each new solution is generated by the neighbor solution x j k And elite solution j a Working together, the search method is shown in formula (7):

[0033]

[0034] 6) Introducing the improved simulated annealing algorithm: The MOABC algorithm for building the external archive search strategy in step 5) is further optimized, and the Suppapitnarm-improved multi-objective simulated annealing SMOSA (Multi-Objective Simulated Annealing, SMOSA for short) criterion is introduced. Assuming that the current nectar source is x i j ,,The new honey source after iteration is x new j , then the corresponding fitness change value is Each objective function is assigned a temperature for annealing iterative update. The changes in each objective are compared with each other before archiving. The acceptance probability p is shown in formula (8), where N is the number of optimized objective functions:

[0035]

[0036] If Δ≤0, the new nectar source is accepted to replace the current nectar source. If p>rand(0,1), the inferior nectar source is also accepted to replace the current nectar source. Otherwise, it remains unchanged and T is used. n =a n T cooling method, iterate N times at each temperature T, by constantly changing x i j To find the optimal value at the current temperature, then lower the temperature and continue searching until the termination condition is met;

[0037] 7) Combining the penalty function constraint method with the multi-objective optimization idea: The penalty function constraint method and the multi-objective optimization idea are combined to transform it into an unconstrained optimization problem. The constructed formula is merged with the original function, and the penalty function is defined as shown in formula (9):

[0038]

[0039] Among them, the death penalty factors σ1 and σ2 are set to infinity Inf respectively;

[0040] They are penalty functions respectively;

[0041] d k (x)≥0(k=1,2,...,K);

[0042] g j (x)≤0(j=1,2,...,p),

[0043] When an individual meets the constraints, the penalty value of the individual is 0. Otherwise, the death penalty factor is set to infinity Inf to eliminate the individual that does not meet the constraints. In trajectory interpolation, if the robot collides with an obstacle, the death penalty factor is set to infinity Inf. If the angle, speed, acceleration, and jerk of the robot do not meet the constraints, the death penalty factor is also set to infinity Inf.

[0044] 8) Improve the multi-objective artificial bee colony algorithm to optimize multi-objective variables: For the improved MOABC algorithm obtained by step 5), step 6), and step 7), the intermediate points of the piecewise polynomial of the robot arm's motion process and the time parameters of the piecewise polynomial are used as the nectar source initialization, the fitness is calculated, a fast non-dominated sort is performed to generate a non-dominated solution set and the external archive set is updated. In the bee collecting stage, the nectar source is searched and updated. In the bee spectator stage, a greedy selection search strategy is performed and the nectar source is updated. The fitness is calculated, a fast non-dominated sort is performed to generate a non-dominated solution set and the crowding calculation is performed, and the external archive set is updated. If the mining threshold limitNum of the nectar source is exceeded and it has not been updated, the scout bee stage is entered to generate a new nectar source, the fitness is calculated and updated by greedy selection, and the current optimal solution set is recorded. If the loop reaches the maximum number of iterations, the optimal solution set is output, otherwise the temperature is updated and the loop continues.

[0045] This technical solution consists of three parts: collision detection, trajectory planning and multi-objective optimization. Firstly, a piecewise quintic polynomial trajectory equation is used to extract the intermediate point parameters that affect the trajectory characteristics as variables to be optimized. Secondly, an improved multi-objective artificial bee colony algorithm (Multi-objective artificial bee colony algorithm) is generated by combining the artificial bee colony algorithm with an external archive search strategy, an improved simulated annealing algorithm, a penalty function constraint method and a multi-objective optimization idea. With the robot arm time-energy-pulsation as the optimization target, this technical solution is used to optimize the variables to solve a Pareto solution set that is collision-free, satisfies the constraints and optimizes multiple objectives simultaneously.

[0046] This technical solution constructs an external archive search strategy, and uses the elite information of the external archives to guide the population to find the best solution. This not only makes the algorithm move in a better direction, but also ensures the randomness of the algorithm. It introduces an improved simulated annealing algorithm, and uses simulated annealing to have strong global search and probability mutation capabilities. In the process of accepting new solutions, it probabilistically accepts worse solutions instead of only accepting better solutions, thereby tending to the global optimal solution. The penalty function method is introduced to deal with the multi-objective trajectory planning and optimization problem of the serial robot with constraints, and converts it into an unconstrained optimization problem. This technical solution enables the robot to improve operating efficiency, reduce energy consumption and reduce jitter while avoiding obstacles, and obtain continuous and smooth trajectories of angle, angular velocity, angular acceleration and angular acceleration. This technical solution has a fast convergence speed and a wide search space, thereby obtaining a Pareto optimal solution with eliteness and diversity, and has high efficiency. By analogy, this technical solution can also be extended to serial robots with any degree of freedom, using multiple high-order polynomials to perform multi-objective trajectory obstacle avoidance planning for multiple obstacles.

[0047] This method can plan a non-dominated frontier Pareto solution set that is collision-free, satisfies constraints, and achieves multiple objectives simultaneously, thereby obtaining an ideal trajectory that is both elitist and diverse and has high efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 A schematic diagram of a method flow chart of an embodiment;

[0049] Figure 2 Schematic diagram of the joint coordinate system in the embodiment;

[0050] Figure 3 is a schematic diagram of an external archive search strategy in an embodiment;

[0051] Figure 4 Schematic diagram of the frontier of the four algorithms in the embodiment on the ZDT1 problem;

[0052] Figure 5 A schematic diagram of a Pareto solution set for optimizing the trajectory of a robotic arm in an embodiment;

[0053] Figure 6 Schematic diagram of the movement process of the robot arm in the embodiment;

[0054] Figure 7 Schematic diagram of the changes in the joint angles of the robotic arm in the embodiment. DETAILED DESCRIPTION

[0055] The content of the present invention is further described below in conjunction with the drawings and embodiments, but the present invention is not limited thereto.

[0056] Example:

[0057] Reference Figure 1 , a multi-objective trajectory obstacle avoidance planning method for a robotic arm based on an improved MOABC algorithm, comprising the following steps:

[0058] 1) Construct a robotic arm and obstacle model: Establish a joint coordinate system for the robotic arm model according to the D-H convention. Derive from the base to the adjacent joint and then to the end effector through the D-H parameter method, establish a link transmission connection, obtain the D-H parameters, and establish a robotic arm model as Figure 2 shown. Use the sphere envelope method to model obstacles with uncertain shapes, that is, take the center of the two points with the farthest distance on the obstacle as the center of the sphere, and use the length of the line segment connecting the two points as the diameter for envelope;

[0059] 2) Define a collision detection method: The collision detection between the robotic arm and the obstacle can be simplified to judge the position relationship between the straight line segment i and the center point O of the sphere by the D-H parameter relationship. That is, superimpose the maximum radial radius of the robotic arm link on the radius of the sphere envelope. Define the maximum radial radius of the robotic arm as R1, the radius of the obstacle as R0, and the distance from the straight line segment of the link to the center of the obstacle as d it , the obstacle center coordinate O is (x0, y0, z0). Establish a mathematical model of each link segment by the homogeneous coordinate transformation method according to the D-H parameters. The equation of the straight line segment i is shown in formula (1):

[0060]

[0061] where λ is a proportional parameter, and the value range is 0 to 1. x, y, z represent the coordinates of the straight line segment i. Then, detect the distance d between the straight line segment i and the center of the sphere iλ and the relationship between R0 + R1: If d iλ < R0 + R1, then the robotic arm collides with the obstacle. If d iλ > R0 + R1, then the robotic arm does not collide with the obstacle. If during the movement of the robotic arm, it is detected that the obstacle does not collide with any of the i links, it indicates that the robotic arm has completed obstacle avoidance;

[0062] 3) Construct a segmented joint trajectory: Each joint of the robotic arm can set intermediate points and use a segmented fifth-order polynomial to fit the key points of each joint variable, thereby constructing the operating trajectory of the robotic arm to bypass the obstacle. The function expression of the fifth-order polynomial is shown in formula (2):

[0063] θ (t) = a0 + a1t + a2t 2 + a3t 3 + a4t 4 + a5t 5 (2),

[0064] Where t represents the motion time, a0, ..., a5 represent the unknown coefficients, and the coefficients of the quintic polynomial are written in vector form, then formula (2) can be simplified as shown in formula (3):

[0065] θ (t) =[1,t,t 2 ,t 3 ,t 4 ,t 5 ]·a (3),

[0066] According to actual needs, the position, velocity and acceleration of each joint of the robot arm at the initial time t0 are θ0, v0, a0 respectively, and at the end time t f The position, velocity and acceleration are θ f ,v f ,a f , where t0 = 0, in this example, v0 = a0 = v f =a f =0, let the middle point be at t m The position at time θ m , construct the equation as shown in formula (4):

[0067]

[0068] By analogy with formula (3) and formula (4), if the motion trajectory of the robot contains n intermediate points, there are n+1 segments of running time and n+2 moments, which are represented by n+1 segments of quintic polynomials. Then 6(n+1) constraint equations are required to determine the coefficients. The simultaneous formula (3) and formula (4) are written in the form of matrix multiplication, and the coefficient matrix A is obtained as shown in formula (5):

[0069] A=M -1 *Q (5),

[0070] Among them, M is the matrix of the trajectory with respect to time t, A contains the unknown coefficients of the trajectory, and Q is the known constraint matrix;

[0071] 4) Multi-objective function construction and constraints: In order to maximize the key performance of the robot arm in terms of running time, energy consumption and pulsation impact, the purpose of optimization is to plan an efficient, energy-saving and stable running trajectory while the robot arm performs global obstacle avoidance and satisfies the constraints. Therefore, the optimization objective is defined as shown in formula (6):

[0072]

[0073]

[0074]

[0075] Among them, m is the number of path points, M is the number of joints of the robot, n is the number of interpolation points obtained from H / s, s is the interpolation interval, f1 is the running time, which is used to measure the operating efficiency of the robot, f2 is the sum of joint acceleration increments, which is used to measure the energy consumption of the robot, and f3 is the sum of joint jerk increments, which is used to measure the pulsation impact of the robot.

[0076] At the same time, the movement of the robot arm is also subject to the following constraints:

[0077] Position constraints:

[0078] Speed ​​Constraints:

[0079] Acceleration constraints:

[0080] Jerk constraint:

[0081] Among them, θ ijmax ,v ijmax ,a ijmax ,j ijmax are the maximum values ​​of the corresponding joint displacement, velocity, acceleration and jerk respectively; θ ijmin , is the minimum value of the corresponding joint displacement; θ ij ,a ij ,v ij ,j ij are the displacement, velocity, acceleration and jerk of the i-th interpolation point of the corresponding joint respectively;

[0082] 5) Construct an external archive search strategy: Adopt the multi-objective artificial bee colony MOABC algorithm. In the bee collection stage, replace the random limited search method originally used by bees. Set the size R of the external archive set E, guide the population to optimize according to the elite information of the external archive, add the non-dominated solutions generated during the search process to the archive, and randomly select the jth dimension e of any solution from the external archive set E. j a Introduced into the search method, each new solution is generated by the neighbor solution x j k And elite solution j a Working together, the search method is shown in formula (7):

[0083]

[0084] 6) Introducing the improved simulated annealing algorithm: The MOABC algorithm for building the external archive search strategy in step 5) is further optimized, and the Suppapitnarm improved multi-objective simulated annealing SMOSA criterion is introduced. Assuming that the current nectar source is x i j ,,The new honey source after iteration is x new j , then the corresponding fitness change value is Each objective function is assigned a temperature for annealing iterative update. The changes in each objective are compared with each other before archiving. The acceptance probability p is shown in formula (8), where N is the number of optimized objective functions:

[0085]

[0086] If Δ≤0, the new nectar source is accepted to replace the current nectar source. If p>rand(0,1), the inferior nectar source is also accepted to replace the current nectar source. Otherwise, it remains unchanged and T is used. n =a n T cooling method, iterate N times at each temperature T, by constantly changing x i j To find the optimal value at the current temperature, then lower the temperature and continue searching until the termination condition is met;

[0087] 7) Combining the penalty function constraint method with the multi-objective optimization idea: In order to deal with the multi-objective trajectory planning and optimization problem of the serial robot with constraints, the penalty function constraint method and the multi-objective optimization idea are combined to transform it into an unconstrained optimization problem. The constructed formula is merged with the original function, and the penalty function can be defined as shown in formula (9):

[0088]

[0089] This example takes the minimization problem as an example, where the death penalty factors σ1 and σ2 are set to infinity Inf respectively;

[0090] They are penalty functions respectively;

[0091] d k (x)≥0(k=1,2,...,K);

[0092] g j (x)≤0(j=1,2,...,p),

[0093] When an individual meets the constraints, the penalty value of the individual is 0. Otherwise, the death penalty factor is set to infinity Inf to eliminate the individual that does not meet the constraints. In trajectory interpolation, if the robot collides with an obstacle, the death penalty factor is set to infinity Inf. If the angle, speed, acceleration, and jerk of the robot do not meet the constraints, the death penalty factor is also set to infinity Inf.

[0094] 8) Improve the multi-objective artificial bee colony algorithm to optimize multi-objective variables: For the improved MOABC algorithm obtained by step 5), step 6), and step 7), the intermediate points of the piecewise polynomial of the robot arm's motion process and the time parameters of the piecewise polynomial are used as the nectar source initialization, the fitness is calculated, a fast non-dominated sort is performed to generate a non-dominated solution set and the external archive set is updated. In the bee collecting stage, the nectar source is searched and updated. In the bee spectator stage, a greedy selection search strategy is performed and the nectar source is updated. The fitness is calculated, a fast non-dominated sort is performed to generate a non-dominated solution set and the crowding calculation is performed, and the external archive set is updated. If the mining threshold limitNum of the nectar source is exceeded and it has not been updated, the scout bee stage is entered to generate a new nectar source, the fitness is calculated and updated by greedy selection, and the current optimal solution set is recorded. If the loop reaches the maximum number of iterations, the optimal solution set is output, otherwise the temperature is updated and the loop continues.

[0095] The test function ZDT1 in the CEC09 test set is used to analyze the performance of this method, the standard MOABC algorithm, the LMOABC algorithm, and the NSGA-Ⅱ algorithm. The frontier surface diagrams of the four algorithms on the ZDT1 problem are shown in Figure 2. Figure 4 As shown in Table 1, the IGD index is a comprehensive performance index that reflects the convergence, uniformity and distribution of the algorithm. The IGD values ​​of the four algorithms on ZDT1 are shown in Table 1. Figure 4 As shown in Table 2, the Pareto solution set obtained by this method can fit the real Pareto frontier well, which further verifies the performance superiority of the proposed algorithm:

[0096] Table 1 IGD value statistics

[0097]

[0098] The Pareto frontier of this method plans and optimizes the running time, energy consumption and pulsation of the robot end trajectory is as follows Figure 5 As shown by Figure 5 It can be seen that energy consumption and pulsation impact are positively correlated, and negatively correlated with running time: that is, the closer to point A, the better the time optimal performance; the closer to point B, the better the energy optimal and pulsation optimal performance, such as Figure 6 As shown in , when the robot arm takes the optimization result of point C, the robot arm plans the obstacle avoidance trajectory and satisfies the optimized motion of the constraints, as shown in Figure 7As shown in the figure, the angle, angular velocity, angular acceleration and angular jerk of each joint change when the robot takes point C. The fifth-order polynomial interpolation theory can be used to solve the high-order continuous and smooth motion trajectory in which the start and stop speed and acceleration can be specified as 0, and it is ensured that the angular acceleration trajectory of each joint runs continuously and transitions smoothly. The improved artificial bee colony algorithm can be used to achieve multi-objective optimization that meets the constraints. While improving any one or several objective functions, other objective functions will not be weakened, thereby selecting the motion trajectory that meets the requirements of avoiding obstacles, short running time, low energy consumption, and small pulsation impact.

Claims

1. A multi-target trajectory obstacle avoidance planning method for a manipulator based on an improved MOABC algorithm, characterized in that: The steps include: 1) Construct the robot and obstacle model: Establish the joint coordinate system of the robot model according to the DH convention, derive the linkage transmission connection from the base to the adjacent joints and then to the end effector through the DH parameter method, obtain the DH parameters, build the robot model, and use the sphere envelope method to model the obstacle with uncertain shape, that is, take the center of the two farthest points on the obstacle as the sphere center, and use the length of the line segment connecting the two points as the diameter for envelope; 2) Define the collision detection method: The collision detection between the robot arm and the obstacle is simplified from the DH parameter relationship to the position relationship between the straight line segment i and the sphere center point O. That is, the radial maximum radius of the robot arm connecting rod is superimposed on the radius of the sphere envelope. The maximum radial radius of the robot arm is defined as R1, the obstacle radius is R0, and the distance from the connecting rod straight line segment to the obstacle center is d it The coordinates of the obstacle center O are (x0, y0, z0), and the coordinates of the first and last points of each link are (x i ,y i ,z i ) and (x i+1 ,y i+1 ,z i+1 ), the mathematical model of each connecting rod segment is established by homogeneous coordinate transformation method according to DH parameters. The coordinate equation of straight line segment i is shown in formula (1): Among them, λ is a proportionality parameter with a value range from 0 to 1, and x, y, z represent the coordinates of the straight line segment i. Next, the distance d between the straight line segment i and the center of the sphere is detected. iλ The relationship with R0 + R1: If d iλ < R0 + R1, the robotic arm collides with the obstacle. If d iλ > R0 + R1, the robotic arm does not collide with the obstacle. If during the movement of the robotic arm, it is detected that the obstacle does not collide with any of the i links, it indicates that the obstacle avoidance of the robotic arm is completed. 3) Constructing segmented joint trajectory: By setting the intermediate points of each joint of the robot arm, a segmented quintic polynomial is used to fit the key points of each joint variable to construct the running trajectory of the robot arm and bypass obstacles. The function expression of the quintic polynomial is shown in formula (2): θ (t) =a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (2), Where t represents the motion time, a0, ..., a5 represent the unknown coefficients, and the coefficients of the quintic polynomial are written in vector form, then formula (2) can be simplified as shown in formula (3): θ (t) =[1,t,t 2 ,t 3 ,t 4 ,t 5 ]·a (3), Assume that the position, velocity and acceleration of each joint of the robot arm at the initial time t0 are θ0, v0, a0 respectively, and at the end time t f The position, velocity and acceleration are θ f ,v f ,a f , where t0 = 0, let v0 = a0 = v f =a f =0, let the middle point be at t m The position at time θ m , construct the equation as shown in formula (4): By analogy with formula (3) and formula (4), if the motion trajectory of the robot contains n intermediate points, there are n+1 segments of running time and n+2 moments, which are represented by n+1 segments of quintic polynomials. Then 6(n+1) constraint equations are required to determine the coefficients. The simultaneous formula (3) and formula (4) are written in the form of matrix multiplication, and the coefficient matrix A is obtained as shown in formula (5): A=M -1 *Q (5), Among them, M is the matrix of the trajectory with respect to time t, A contains the unknown coefficients of the trajectory, and Q is the known constraint matrix; 4) Multi-objective function construction and constraints: The optimization objective is defined as shown in formula (6): Where m is the number of path points, M is the number of joints of the robot, n is the number of interpolation points obtained from H / s, s is the interpolation interval, f1 is the running time, f2 is the sum of joint acceleration increments, and f3 is the sum of joint jerk increments. At the same time, the movement of the robot arm is also subject to the following constraints: Position constraint: θ ijmin ≤θ ij ≤θ ijmax ; Speed ​​Constraints: Acceleration constraints: Jerk constraint: Among them, θ ijmax ,v ijmax ,a ijmax ,j ijmax are the maximum values ​​of the corresponding joint displacement, velocity, acceleration and jerk respectively; θ ijmin , is the minimum value of the corresponding joint displacement; θ ij ,a ij ,v ij ,j ij are the displacement, velocity, acceleration and jerk of the i-th interpolation point of the corresponding joint respectively; 5) Construct an external archive search strategy: Adopt the multi-objective artificial bee colony MOABC algorithm. In the bee collection stage, replace the random limited search method originally used by bees. Set the size R of the external archive set E, guide the population to optimize according to the elite information of the external archive, add the non-dominated solutions generated during the search process to the archive, and randomly select the jth dimension e of any solution from the external archive set E. j a Introduced into the search method, each new solution is generated by the neighbor solution x j k And elite solution j a Working together, the search method is shown in formula (7): 6) Introducing the improved simulated annealing algorithm: The MOABC algorithm for building the external archive search strategy in step 5) is further optimized, and the Suppapitnarm improved multi-objective simulated annealing SMOSA criterion is introduced. Assuming that the current nectar source is x i j , the new honey source after iteration is x new j , then the corresponding fitness change value is Each objective function is assigned a temperature for annealing iterative update. The changes in each objective are compared with each other before archiving. The acceptance probability p is shown in formula (8), where N is the number of optimized objective functions: If Δ≤0, the new nectar source is accepted to replace the current nectar source. If p>rand(0,1), the inferior nectar source is also accepted to replace the current nectar source. Otherwise, it remains unchanged and T is used. n =a n T cooling method, iterate N times at each temperature T, by constantly changing x i j To find the optimal value at the current temperature, then lower the temperature and continue searching until the termination condition is met; 7) Combining the penalty function constraint method with the multi-objective optimization idea: The penalty function constraint method and the multi-objective optimization idea are combined to transform it into an unconstrained optimization problem. The constructed formula is merged with the original function, and the penalty function is defined as shown in formula (9): Among them, the death penalty factors σ1 and σ2 are set to infinity Inf respectively; They are penalty functions respectively; d k (x)≥0(k=1,2,...,K); g j (x)≤0(j=1,2,...,p), When an individual meets the constraints, the penalty value of the individual is 0. Otherwise, the death penalty factor is set to infinity Inf to eliminate the individual that does not meet the constraints. In trajectory interpolation, if the robot collides with an obstacle, the death penalty factor is set to infinity Inf. If the angle, speed, acceleration, and jerk of the robot do not meet the constraints, the death penalty factor is also set to infinity Inf. 8) Improve the multi-objective artificial bee colony algorithm to optimize multi-objective variables: For the improved MOABC algorithm obtained by step 5), step 6), and step 7), the intermediate points of the piecewise polynomial of the robot arm's motion process and the time parameters of the piecewise polynomial are used as the nectar source initialization, the fitness is calculated, a fast non-dominated sort is performed to generate a non-dominated solution set and the external archive set is updated. In the bee collecting stage, the nectar source is searched and updated. In the bee spectator stage, a greedy selection search strategy is performed and the nectar source is updated. The fitness is calculated, a fast non-dominated sort is performed to generate a non-dominated solution set and the crowding calculation is performed, and the external archive set is updated. If the mining threshold limitNum of the nectar source is exceeded and it has not been updated, the scout bee stage is entered to generate a new nectar source, the fitness is calculated and updated by greedy selection, and the current optimal solution set is recorded. If the loop reaches the maximum number of iterations, the optimal solution set is output, otherwise the temperature is updated and the loop continues.

Images