Robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy

Abstract

This invention relates to a method for calculating the optimal robot trajectory in time, incorporating obstacle avoidance strategies. This addresses the technical problems of insufficient accuracy in calculating the optimal robot trajectory in existing technologies and low robot efficiency due to the lack of integration with collision avoidance strategies. The method provides a time-optimized particle swarm optimization algorithm to optimize the motion time of each drive axis, while simultaneously employing a genetic algorithm to optimize the start-up timing and speed ratio of each drive axis. This determines the optimal matching strategy for the motion timing between each drive axis, ensuring that the robot selects the optimal trajectory during operation.

Description

Technical Field

[0001] This invention relates to robot motion trajectory planning methods, specifically to a method for calculating the optimal time-based motion trajectory of a robot that incorporates obstacle avoidance strategies. Background Technology

[0002] Material handling and loading / unloading robots, as key equipment in industrial automation, are used to automatically move, grasp, place, and unload materials or products during the manufacturing process. They are typically equipped with robotic arms, with actuators (such as grippers, suction cups, electromagnets, etc.) suitable for different materials installed at the end effector, and are integrated into production lines, machine tools, stamping equipment, or assembly lines. Their main functions include: precisely transporting raw materials or semi-finished products from warehouses, conveyors, or silos to processing stations, and removing finished products or waste materials after processing. These robots can efficiently and tirelessly perform repetitive, labor-intensive material handling tasks and are widely used in industries such as automotive manufacturing, electronic assembly, metal processing, plastic molding, and food packaging. Material handling and loading / unloading robots face the dual challenges of efficiency and safety when performing complex filling tasks in confined spaces; therefore, optimizing the motion trajectories of these robots in a reasonable manner is crucial.

[0003] Existing motion trajectory optimization techniques primarily focus on optimizing the end effector trajectory of robotic devices, failing to treat each drive axis as an independent entity. Furthermore, end effector trajectory optimization requires each drive axis to simultaneously reach a specific position, thus limiting the scope of robot motion optimization and resulting in a low dimensionality and insufficient accuracy in calculating the optimal robot trajectory. In addition, existing trajectory optimization techniques are not integrated with collision avoidance strategies. If the robot is moving in a complex environment and working condition, it must first plan a complex spatial trajectory to avoid interference before optimization, further increasing the robot's movement time and reducing its efficiency. Summary of the Invention

[0004] The purpose of this invention is to solve the technical problems of insufficient accuracy in calculating the optimal motion trajectory of a robot and low robot efficiency due to the lack of integration with collision avoidance strategies, and to provide a method for calculating the optimal time motion trajectory of a robot that combines obstacle avoidance strategies.

[0005] To achieve the above objectives, the technical solution provided by this invention is as follows:

[0006] A method for calculating the optimal time-limited motion trajectory of a robot that incorporates obstacle avoidance strategies is characterized by the following steps:

[0007] Step 1: Determine the starting position, ending position, and motion trajectory feature points of each drive axis on the robot based on experience;

[0008] Step 2: Based on the starting position, ending position and motion trajectory feature points of each drive shaft determined in Step 1, calculate the first optimal motion time for each drive shaft using a time-optimized particle swarm optimization algorithm.

[0009] Step 3: Based on the first optimal motion time of each drive shaft obtained in Step 2, establish a first polynomial about time, and calculate the first optimal position-time function of each drive shaft based on the first polynomial.

[0010] Step 4: Based on the first optimal motion time of each drive axis obtained in Step 2 and the preset waiting time before each drive axis performs its action, determine whether there is interference between the combined motion of each drive axis and the external environment. If yes, proceed to Step 5. If no, calculate the robot's optimal motion trajectory based on the first optimal position-time function of each drive axis obtained in Step 3, and complete the calculation of the robot's time-optimal motion trajectory.

[0011] Step 5: Using a genetic algorithm, adjust the preset waiting time before each drive axis performs an action and the preset optimal speed percentage of each drive axis to obtain the optimal matching strategy for the motion timing between each drive axis after avoiding interference.

[0012] Step 6: Based on the optimal matching strategy obtained in Step 5 and the first optimal motion time of each drive axis obtained in Step 2, calculate the optimal motion trajectory of the robot and complete the calculation of the robot's time-optimal motion trajectory.

[0013] Furthermore, the specific process of step 2 is as follows:

[0014] Step 2.1: Based on the starting position, ending position, and motion trajectory feature points of each drive shaft determined in Step 1, divide the motion trajectory of each drive shaft into continuous trajectory segments.

[0015] Step 2.2: With the goal of optimizing the motion time of each drive shaft, determine the time objective function and constraints for each drive shaft based on the trajectory segments obtained in Step 2.1.

[0016] Step 2.3: Based on the time objective function and constraints of each driving axis obtained in Step 2.2, determine the M sets of time series corresponding to each driving axis, M≥1, and use the M sets of time series as the population of the particle swarm algorithm. Initialize the velocity parameters, inertia factor and acceleration constant of each particle in the population.

[0017] Step 2.4: Based on the M sets of time series corresponding to each drive shaft obtained in Step 2.3 and the time objective function of each drive shaft obtained in Step 2.2, calculate the local optimal solution p of the M sets of time series corresponding to each drive shaft.mbest According to the local optimal solution p mbest Calculate the global optimal solution g for the M time series corresponding to each drive shaft. best The global optimal solution g best This is the first optimal motion time for each drive shaft;

[0018] Step 2.5: Determine the global optimal solution g obtained in Step 2.4. best If the predetermined threshold is met, the calculation ends, and the global optimal solution g is obtained. best The first optimal motion time for each drive shaft is determined; otherwise, proceed to step 2.6.

[0019] Step 2.6: Based on the local optimal solution p obtained in Step 2.4 mbest Global optimal solution g best Using the M time series, velocity parameters, inertia factors, and acceleration constants determined in step 2.3, calculate the new velocity parameters:

[0020] Step 2.7: Based on the M time series obtained in Step 2.3 and the new velocity parameters obtained in Step 2.6, determine the new M time series;

[0021] Step 2.8: Return to Step 2.3, and use the new M sets of time series as the M sets of time series corresponding to each drive axis, until the global optimal solution g in Step 2.5 is obtained. best The predetermined threshold is met.

[0022] Furthermore, the specific process of step 5 is as follows:

[0023] Step 5.1: Based on the preset waiting time before each drive shaft performs an action and the preset optimal speed percentage of each drive shaft, randomly generate a population;

[0024] Step 5.2: Determine the optimization objective function based on the preset waiting time before each drive axis performs its action and the preset percentage of the optimal motion speed for each drive axis;

[0025] Step 5.3: Based on the optimization objective function determined in Step 5.2, calculate the fitness value of each individual in the population obtained in Step 5.1, select the individual with the highest fitness value as the optimal parent individual, and select individuals that meet the preset fitness threshold to establish a population of individuals to be crossed.

[0026] Step 5.4: Determine whether the highest fitness value has reached the preset highest fitness threshold. If yes, determine the optimal matching strategy for the motion timing between each drive axis after avoiding interference based on the best parent individual. If no, proceed to step 5.5.

[0027] Step 5.5: Perform crossover operations on the group of individuals to be crossovered established in Step 5.3 to generate the next generation of individuals;

[0028] Step 5.6: Perform a low-probability mutation on the next generation individuals obtained in Step 5.5 to generate mutated individuals;

[0029] Step 5.7: Combine the optimal parent individuals obtained in Step 5.3, the next generation individuals obtained in Step 5.5, and the mutated individuals obtained in Step 5.6 into a new population;

[0030] Step 5.8: Return to step 5.3 and replace the population obtained in step 5.1 with the new population obtained in step 5.7 until the highest fitness value in step 5.4 reaches the preset highest fitness threshold.

[0031] Furthermore, step 6 specifically includes:

[0032] Step 6.1: Determine the optimal speed percentage of each drive shaft based on the optimal matching strategy obtained in Step 5;

[0033] Step 6.2: Based on the optimal speed percentage of each drive shaft determined in Step 6.1 and the first optimal motion time of each drive shaft obtained in Step 2, calculate the second optimal motion time tj of each drive shaft. T The calculation formula is as follows:

[0034] tj T =tj min / η j

[0035] In the formula, tj min For the first optimal motion time of the j-th drive shaft, η j The percentage of the optimal motion speed of the j-th drive shaft;

[0036] Step 6.3: Based on the second optimal motion time tj of each drive shaft T Establish a second polynomial with respect to time, and calculate the second optimal position-time function for each drive shaft based on the second polynomial;

[0037] Step 6.4: Based on the second optimal position-time function of each drive axis obtained in Step 6.3, calculate the optimal motion trajectory of the robot, and complete the calculation of the robot's time-optimal motion trajectory.

[0038] Furthermore, the new velocity parameter calculation formula in step 2.6 is as follows:

[0039]

[0040] In the formula, v m(n) represents the new velocity parameter, ω is the inertia factor, c1 and c2 are the first and second acceleration constants respectively, r1 and r2 are random numbers in the range of 0 to 1, m is any one of the M time series, m = 1, 2, 3, ..., M, n is the current iteration number, and t m (n) is the time parameter for the current iteration, v m (n-1) represents the particle velocity from the previous iteration. For the local optimum of the current iteration, g best (n) represents the global optimal solution in the current iteration;

[0041] The new formula for calculating the M group of time series in step 2.7 is as follows:

[0042] t m (n+1)=t m (n)+v m (n)

[0043] In the formula, t m (n)=[t m1 (n), t m2 (n), ..., t mi [n] represents the time parameter for the nth iteration. Further, the formula for calculating the first optimal position-time function for each drive shaft in step 3 is as follows:

[0044] q i (t i ) = a i0 +a i1 t i +a i2 t i 2 +…+a ik t i k 3≤k≤5

[0045] In the formula, q i (t i Let t be the optimal position function for each drive shaft, i be the number of segments of the drive shaft's motion trajectory, and t be the position function for each drive shaft. i Let k be the time taken by the robot to run on each segment of the trajectory, k be the order of the spline polynomial, and a be the time taken by the robot to run on each segment of the trajectory. i0 a i1 …a ik The robot trajectory parameters are obtained by solving the least squares method based on the established first polynomial.

[0046] Furthermore, the method for determining whether there is interference between the combined motion of each drive shaft and the external environment in step 4 is as follows:

[0047] The outer contour of the robot link is simplified to a circle and an ellipse. The line connecting the center of the drive shaft to the end point of the robot device is regarded as an envelope circle formed by a series of circular intervals. If the distance from the midpoint of the robot link to the interference indicator is less than the radius of the envelope circle, then interference is considered to exist; otherwise, interference is considered not to exist.

[0048] Furthermore, in step 2.2:

[0049] The time objective function formula is as follows:

[0050]

[0051] In the formula, t i Let N be the time required for the drive shaft to move between the i-th and i+1-th trajectory feature points, and N be the total number of all segmented curves of the drive shaft.

[0052] The constraints are defined as follows:

[0053]

[0054] In the formula, g i Let q be the position, velocity, and acceleration on the i-th segment of the drive shaft's trajectory. i Let be the position on the i-th segment of the trajectory. Let be the velocity on the i-th segment of the drive shaft's trajectory. Let g be the acceleration on the i-th segment of the trajectory of the drive shaft. min q represents the minimum limit position, velocity, and acceleration of the drive shaft. min For the joint negative limit of the drive shaft, g max q represents the maximum limit position, velocity, and acceleration of the drive shaft. max For the joint of the drive shaft, positive limit position. The rated speed of the drive shaft, For the rated acceleration.

[0055] Furthermore, in step 5.2:

[0056] The formula for optimizing the objective function is as follows:

[0057]

[0058] In the formula, t1 f …,tj f t1 is the waiting time before each drive axis performs an action. T …,tj T The optimal percentage of motion speed for each drive shaft; (x inf ,y inf For the position of each point, l link l is the distance from each point on the center line of the rotating arm to the center of the rotation axis.end r is the distance from the center point of the fixture to the center of the rotation axis. link r is the maximum distance from the centerline of the robot's rotating arm to the surface. end Let q1, q2, and q3 be the radius of the robot's end effector gripper, and q1, q2, and q3 be the displacement and angle corresponding to each drive axis, respectively.

[0059] The beneficial effects of this invention are:

[0060] 1. This invention combines a robot time-optimal motion trajectory calculation method with obstacle avoidance strategy. By using a polynomial with respect to time, it designs an optimal position-time function for the continuous variation of velocity and acceleration of each drive axis, aiming to reduce running jitter, ensure safe and stable operation, and maximize mechanical efficiency.

[0061] 2. The present invention combines an obstacle avoidance strategy with a robot time-optimal motion trajectory calculation method. By introducing a time-optimized particle swarm optimization algorithm and comprehensively considering speed and acceleration constraints, the invention achieves optimization of single-axis motion time for each drive axis.

[0062] 3. The present invention combines a robot time-optimal motion trajectory calculation method with obstacle avoidance strategy, and uses a genetic algorithm to optimize the waiting time before each drive axis executes the action and the optimal motion speed percentage of each drive axis, so as to ensure that the robot maintains a safe distance from the external environment during the movement.

[0063] 4. The robot time-optimal motion trajectory calculation method of this invention, which combines obstacle avoidance strategy, does not restrict the robot end effector to simultaneously reach certain trajectory feature points. Instead, it adopts a time-optimized particle swarm optimization algorithm to optimize the motion of a single drive axis. Through a genetic algorithm, it finds the optimal combination of motion timing and speed percentage for each drive axis, breaking through the limitations of traditional end effector trajectory optimization, significantly shortening the task execution time, improving the accuracy of determining the optimal motion trajectory, and enhancing the robot's working efficiency. Attached Figure Description

[0064] Figure 1 This is a flowchart of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy according to the present invention;

[0065] Figure 2a , 2b 2c, 2d, and 2e are the core technical principle diagrams of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0066] Figure 3 This is a schematic diagram of the robot grasping process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0067] Figure 4This is a schematic diagram of the robot retraction process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0068] Figure 5 This is a flowchart of the time-optimized particle swarm algorithm in step 2 of an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0069] Figure 6 This is a schematic diagram of the iterative principle of the time-optimized particle swarm algorithm in step 2 of an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0070] Figure 7 This is a schematic diagram showing the correspondence between acceleration, rotational speed, and position curves in step 3 of an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0071] Figure 8 This is a schematic diagram illustrating the principle of trajectory interference detection using model simplification and safety margin calculation methods in step 4 of an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0072] Figure 9 This is a timing diagram of each drive axis in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0073] Figure 10 This is a schematic diagram of the genetic algorithm in step 5 of an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0074] Figure 11 The figure shows the experimental results of the time-optimized particle swarm algorithm in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0075] Figure 12a 12b is a diagram showing the test results of the filling equipment in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0076] Figure 13 This is a comparison of the computation time of an embodiment of the robot time-optimal motion trajectory calculation method combining obstacle avoidance strategy of the present invention with the PTP trajectory planning method and the end-effector trajectory optimization method.

[0077] Figure 14 This is a schematic diagram of the material handling process of a loading robot in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0078] Figure 15This is a schematic diagram of the polynomial curves of the drive axis trajectories on the y-axis, swing axis and x-axis during the material picking process of the loading robot in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention. Among them, 15a is a schematic diagram of the position, velocity and acceleration curves of the y-axis and swing axis of the grasping process step 1, and 15b is a schematic diagram of the position, velocity and acceleration curves of the x-axis and y-axis of the grasping process step 2.

[0079] Figure 16 This is a schematic diagram of the loading robot retrieval process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0080] Figure 17a Images 17b and 17c are schematic diagrams illustrating the position, velocity, and acceleration curves on the x-axis, y-axis, and pendulum axis during the loading robot's recovery process, as shown in the embodiment of the robot's time-optimal motion trajectory calculation method combining obstacle avoidance strategy according to the present invention.

[0081] Figure 18 This is a sequence diagram of the action timing of the loading robot material handling process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0082] Figure 19 This is a sequence diagram of the action timing of the loading robot's retraction process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention.

[0083] Figure 20 This is a schematic diagram of the motion trajectory curve of the loading robot material picking process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention;

[0084] Figure 21 This is a schematic diagram of the motion trajectory curve of the loading robot recovery process in an embodiment of the robot time-optimal motion trajectory calculation method combined with obstacle avoidance strategy of the present invention. Detailed Implementation

[0085] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0086] This invention relates to a method for trajectory planning of handling and loading / unloading robots. The invention mainly comprises two technical aspects: collision avoidance technology in a known environment and motion time optimization technology. Obstacle avoidance technology in a known environment refers to a method for the robot to safely and effectively plan a path given the position and shape of obstacles in the environment. Motion optimization technology refers to optimizing the actions of a robot, intelligent agent, or system through mathematical and computational methods to achieve a certain goal, such as minimizing energy consumption, time, path length, or maximizing efficiency and safety. In optimization problems, the objective function must first be clearly defined, such as minimizing energy consumption, minimizing time, or maximizing efficiency. Subsequently, mathematical modeling of the robot's motion and the objective is required, inputting obstacle safety boundaries and constraints on velocity and acceleration. Then, evolutionary algorithms such as genetic algorithms or particle swarm optimization are used to find the optimal trajectory.

[0087] The technical route and core principle of this invention are shown in Figure 2. This invention provides a trajectory planning method for loading, unloading, and handling equipment suitable for operations in confined spaces. This method is based on the optimal position-time relationship of each drive axis and integrates a multi-drive axis motion matching strategy based on interference checking. Unlike traditional end-point trajectory optimization methods, this invention focuses on the speed optimization of each independent drive axis, aiming to find a motion timing configuration scheme that does not interfere with the external environment and has fewer trajectory constraint points. First, the starting, path, and ending positions of each drive axis during motion are defined. Then, based on the constraint state of each drive axis throughout the entire motion, the optimization strategy for each drive axis is subdivided into two paths:

[0088] Path 1 represents a fully constrained scenario: a typical scenario with fully constrained processes is the crawling process, such as... Figure 3 As shown, in the grasping process, the process of the loading robot's end end reaching into the inner wall of the cargo hold to pick up materials is constrained by multiple degrees of freedom. The robot's swing arm and translation angle cannot change significantly. Under these circumstances, the drive shaft is spatially constrained throughout the entire process from the starting point to the ending point of the movement. Each drive shaft needs to arrive at the key feature point of the trajectory synchronously. Therefore, there is no need to adjust the speed ratio and start-stop sequence. The particle swarm algorithm can be directly used to solve the optimal value of the coefficients of the trajectory polynomial.

[0089] Path 2 represents a non-full-process interference scenario: a typical case of non-full-process constraint is the process retraction, such as... Figure 4As shown, in this process, the robot's end effector is constrained only by the cargo hold's outer wall in one direction. As long as the robot maintains a certain distance from the cargo hold's outer wall for a certain period, the process is as follows: First, a particle swarm optimization algorithm is used to optimize the velocity curves of individual drive axes to obtain the optimal velocity curves for each drive axis that can accurately reach each preset position while meeting speed and acceleration constraints. Then, the start and end times of each drive axis and the percentage of velocity in each segment are used as genetic algorithm genes. By finding the optimal timing and velocity ratio configuration that satisfies safety margins, the most time-efficient action execution can be achieved within a confined space.

[0090] This embodiment presents a method for calculating the optimal time-based motion trajectory of a robot by combining obstacle avoidance strategies, such as... Figure 1 As shown, the specific implementation steps are as follows:

[0091] Step 1: Determine the starting position, ending position, and motion trajectory feature points of each drive axis on the robot based on experience.

[0092] Step 2: Based on the starting and ending positions of each drive shaft and the feature points of its trajectory determined in Step 1, calculate the first optimal motion time for each drive shaft using a time-optimized particle swarm optimization algorithm, such as... Figure 5 and Figure 6 As shown, this method is specifically designed to solve trajectory planning problems in complex dynamic environments. It aims to minimize the overall operation time by optimizing the speed curves of each drive shaft, while ensuring operational safety and mechanical efficiency. The specific process is as follows:

[0093] Step 2.1: Based on the starting position, ending position, and motion trajectory feature points of each drive shaft determined in Step 1, divide the motion trajectory of each drive shaft into continuous trajectory segments.

[0094] Step 2.2: With the goal of optimizing the motion time of each drive shaft, determine the time objective function and constraints for each drive shaft based on the optimization objective and the trajectory segments obtained in Step 2.1.

[0095] The time objective function formula is as follows:

[0096]

[0097] In the formula, t i The time required for the drive shaft to move between the i-th and i+1-th trajectory feature points. Let N be the unknown quantity to be optimized, and let N be the total number of segmented curves of the entire drive shaft.

[0098] The constraints are defined as follows:

[0099]

[0100] In the formula, g i Let q be the position, velocity, and acceleration on the i-th segment of the drive shaft's trajectory. i Let be the position on the i-th segment of the trajectory. Let be the velocity on the i-th segment of the drive shaft's trajectory. Let g be the acceleration on the i-th segment of the trajectory of the drive shaft. min q represents the minimum limit position, velocity, and acceleration of the drive shaft. min For the joint negative limit of the drive shaft, g max q represents the maximum limit position, velocity, and acceleration of the drive shaft. max For the joint of the drive shaft, positive limit position. The rated speed of the drive shaft, For the rated acceleration;

[0101] Step 2.3: Based on the time objective function and constraints of each drive axis obtained in Step 2.2, determine the M sets of time series t = [t1, t2, ..., t] corresponding to each drive axis. i ]∈R i m = 1, 2, 3, ..., M, where the m-th time series is represented as t m =[t m1 ,tm2,……,t mi ], m represents the particle swarm number, there are m swarms in total. The more particle swarms there are, the wider the search range and the easier it is to find the global optimum, but the computational requirements are also relatively large; M time series are used as the population of the particle swarm algorithm, and the velocity parameters, inertia factors and acceleration constants of each particle in the population are initialized.

[0102] Step 2.4: Based on the M sets of time series corresponding to each drive shaft obtained in Step 2.3 and the time objective function of each drive shaft obtained in Step 2.2, calculate the local optimal solution p of the M sets of time series corresponding to each drive shaft. mbest According to the local optimal solution p mbest Calculate the global optimal solution g for the M time series corresponding to each drive shaft. best The global optimal solution g best This is the first optimal motion time for each drive shaft;

[0103] Step 2.5: Determine the global optimal solution g obtained in Step 2.4. best If a predetermined threshold is met, the calculation ends, and the first optimal motion time for each drive shaft is obtained, i.e., the shortest motion time combination g for a certain drive shaft. best =t min =[t 1min , t 2min , ..., tNmin Let t be the total time of the N segments of motion on the j-th axis. jmin If not, proceed to step 2.6;

[0104] Step 2.6: Based on the local optimal solution p obtained in Step 2.4 mbest Global optimal solution g best Based on the M time series, velocity parameters, inertia factor, and acceleration constant determined in step 2.3, calculate the new velocity parameters. The formula for calculating the new velocity parameters is as follows:

[0105]

[0106] In the formula, ω is the inertia factor, c1 and c2 are the first and second acceleration constants respectively, r1 and r2 are random numbers in the range of 0 to 1, m is any one of the M time series, m = 1, 2, 3, ..., M, n is the current iteration number, and t m v is a time parameter m (n-1) represents the particle velocity from the previous iteration. For the local optimum of the current iteration, g best (n) represents the global optimal solution in the current iteration; c2r2[g] represents the learning property of the particle itself. best (n)-t m [(n)] represents the social nature of particles, such as their ability to share information and their cooperative nature;

[0107] Step 2.7: Based on the M time series obtained in Step 2.3 and the new velocity parameters obtained in Step 2.6, determine the new M time series. The calculation formula for the new M time series is as follows:

[0108] t m (n+1)=t m (n)+v m (n)

[0109] In the formula, t m (n)=[t m1 (n), t m2 (n), ..., t mi [(n)] is the time parameter of the nth iteration, which is the time sequence of the time required for the driving shaft to move between adjacent trajectory feature points;

[0110] Step 2.8: Return to Step 2.3, and use the new M sets of time series as the M sets of time series corresponding to each drive axis, until the global optimal solution g in Step 2.5 is obtained. best The predetermined threshold is met.

[0111] Step 3: Based on the first optimal motion time of each drive shaft obtained in Step 2, establish a first polynomial about time, and calculate the first optimal position-time function of each drive shaft based on the first polynomial. This technique optimizes the motion of each individual drive shaft separately. The expression of the position, velocity, and acceleration of an individual drive shaft relies on polynomial interpolation technology. The process involves refining the motion trajectory of each individual drive shaft into a series of continuous and smooth segments. Each segment is described by an independent polynomial, thereby ensuring a seamless and smooth transition of velocity and acceleration at the segment junctions. Figure 7 The position curve example shown fully demonstrates the high smoothness and accuracy of this technology. The trajectory of each interval can be represented as a k-th order polynomial. The formula for calculating the first optimal position-time function of each drive axis is as follows:

[0112] q i (t i ) = a i0 +a i1 t i +a i2 t i 2 +…+a ik t i k 3≤k≤5

[0113] In the formula, q i (t i Let t be the optimal position function for each drive shaft, i be the number of segments of the drive shaft's motion trajectory, and t be the number of segments of the spline function. A complex trajectory can be composed of multiple spline functions. i Let k be the running time of the robot in each trajectory segment, and k be the order of the spline polynomial. Since the purpose of this invention is to simplify the computational complexity of trajectory optimization, the order of the spline function is limited to 3-5, provided that the rotational acceleration of the drive shaft of the loading and handling robot is continuous. The unknown coefficient a... i0 a i1 …a ik These are the robot trajectory parameters, used to describe the trajectory parameters of the loading and handling robot. i0 a i1 …a ik Solve using the least squares method, if a i0 a i1 …a ik Given this, the trajectory parameters of the loading and handling robot are also determined.

[0114] Solving for a using the least squares method i0 a i1 …a ik The process is as follows:

[0115] Given the initial position x0, the position curve dividing points x1, x2, ..., and the endpoint x0. i In addition to the acceleration and velocity at the initial and final points (generally taken as 0), the velocity and acceleration between the dividing points must also be continuous. Therefore, the trajectory parameter 'a' can be derived based on the above conditions. i0 a i1 …a ik Regarding the duration t of each trajectory segment i The solution;

[0116] After obtaining the optimal motion time for each drive shaft in step 2.2, substitute it into the following formula to obtain the corresponding velocity and acceleration curves. Then, determine whether the velocity and acceleration are greater than the constraints. If they are greater than the velocity constraints, reject the optimal motion time using the rejection method. Define the first polynomial:

[0117] a = A -1 b, a = [a 0k … a 02 a 01 a 00 a 1k … a i2 a i1 a i0 ] T ,

[0118]

[0119] b = [0 0 0…x i 0 0x0 0 0…x (i-1) ] T

[0120] In the formula, t1, t2, ..., t i To obtain the optimal motion time for each drive shaft obtained in step 2.2, the coefficients 'a' are solved using the polynomial above, and the trajectory is expressed as a spline polynomial. If the unknown coefficients 'a' are... ik Let t1, t2, ..., t be the independent variables. i When the variable to be optimized is taken as the dependent variable, its dimension is the product of the number of trajectory segments and the order of the polynomial, which is computationally intensive and complex. If the particle swarm optimization algorithm is used to directly search for the time intervals t1, t2, ..., t, the optimization time intervals will be much smaller. i The search dimension is reduced to the number of trajectory segments i. Then, the unknown parameters of the trajectory are solved, and the velocity of each interpolated trajectory segment is solved as a constraint. Trajectories that do not meet the constraints are eliminated by rejection method, and the better trajectory is retained. This avoids deriving complex mapping relationships and reduces the amount of computation.

[0121] Step 4: Based on the first optimal motion time of each drive axis obtained in Step 2.4 and the preset waiting time before each drive axis performs its action, determine whether there is interference between the combined motion of each drive axis and the external environment. If yes, proceed to Step 5; otherwise, determine the optimal motion trajectory of the robot based on the first optimal position-time function of each drive axis obtained in Step 3.

[0122] This invention employs model simplification and safety margin calculation methods for trajectory interference detection. The purpose of adjusting the motion timing is to find the shortest velocity combination within the safety margin, such as... Figure 8 As shown, the articulated arm is simplified to an ellipse, and the end effector is simplified to a circle with a diameter equal to the maximum arm width and the fixture width. The interference risk is assessed by calculating the distance from each point on the joint line to the external environment. During the iterative optimization of the speed combination, the interference situation is continuously monitored. Once interference is detected, the speed combination is discarded until the speed combination with no interference and the shortest time is found, which is the optimal motion trajectory. Finally, the optimization results are analyzed.

[0123] The method to determine whether there is interference between the combined motion of each drive axis and the external environment is to simplify the outer contour of the robot link into a circle and an ellipse. The line connecting the center of the drive axis to the end point of the robot device is regarded as an envelope circle formed by a number of circular intervals. If the distance from the middle point of the robot link to the interference indicator is less than the radius of the envelope circle, then interference is considered to exist; otherwise, interference is considered not to exist.

[0124] The shortest time tj required for each drive axis to travel from the starting point to the stopping point, calculated using a time-optimized particle swarm optimization algorithm. min Subsequently, this invention proposes an optimal scheduling algorithm for the start-stop timing and speed ratio of each drive axis under the constraint of obstacle avoidance. In confined space operation environments, the overall shortest working time of a loading and handling robot is not solely determined by the performance parameters and structural characteristics of the motors (such as speed and acceleration limitations). In fact, it is also constrained by external environmental factors, especially spatial interference problems. This requires that the movements of each drive axis must be highly coordinated to avoid collisions with the surrounding environment.

[0125] Timing of each drive axis as follows Figure 9 As shown, the waiting time tj before each drive axis performs an action is set. f The task execution time is tj T And the time after execution is tj b tj is the drive shaft number, where the task execution time is the ratio of the optimal motion time and speed percentage for a single drive shaft. T =tj min / η j By adjusting the waiting time tj before each drive axis performs an action.f and speed percentage η j This allows for the adjustment of the timing of actions.

[0126] Step 5, as follows Figure 10 As shown, a genetic algorithm is used to adjust the preset waiting time before each drive axis executes its action and the preset optimal speed percentage of each drive axis, to obtain the optimal matching strategy for the motion timing between each drive axis after avoiding interference. The specific steps are as follows:

[0127] Step 5.1: Based on the preset waiting time before each drive shaft performs an action and the preset optimal speed percentage of each drive shaft, randomly generate a population;

[0128] Step 5.2: Determine the optimization objective function based on the preset waiting time before each drive axis performs its action and the preset percentage of the optimal motion speed for each drive axis;

[0129] The formula for optimizing the objective function is as follows:

[0130]

[0131] In the formula, t1 f …,tj f t1 is the waiting time before each drive axis performs an action. T …,tj T The optimal percentage of motion speed for each drive shaft; (x inf ,y inf For the position of each point, l link Let l be the distance (variable) from each point on the center line of the rotating arm to the center of the rotation axis. end r is the distance (constant) from the center point of the fixture to the center of the rotation axis. link r is the maximum distance from the centerline of the robot's rotating arm to the surface. end Let q1, q2, and q3 be the radius of the robot's end effector gripper, and q1, q2, and q3 be the displacement and angle corresponding to each drive axis, respectively.

[0132] Step 5.3: Based on the optimization objective function determined in Step 5.2, calculate the fitness value of each individual in the population, select the individual with the highest fitness value as the optimal parent individual, and select individuals that meet the preset better fitness threshold to establish a population of individuals to be crossed.

[0133] Step 5.4: Determine whether the value of the highest fitness reaches the preset highest fitness threshold. If yes, determine the optimal matching strategy for the motion timing between each drive axis based on the best parent individual. If no, proceed to step 5.5.

[0134] Step 5.5: Perform a crossover operation on the group of individuals to be crossoverdone established in Step 5.3 to generate the next generation of individuals; for example, two individuals [t1] in the group of individuals to be crossoverdone. f η1, t2 f η2, ..., tj f η j ] and [t1 f ′,η1′,t2 f ′,η2′,…,tj f ′,η j The offspring generated by ′] are [t1] f η1, t2 f ′,η2′,…,tj f ′,η j ′];

[0135] Step 5.6: Perform a low-probability mutation on the next generation individuals obtained in Step 5.5 to generate mutated individuals. Mutation is usually achieved by randomly changing certain gene loci in the individual's chromosome.

[0136] Step 5.7: Combine the optimal parent individual obtained in Step 5.3, the next generation individual obtained in Step 5.5, and the mutated individual obtained in Step 5.6 into a new population, and return to Step 5.3;

[0137] Step 5.8: Return to step 5.3 and replace the population obtained in step 5.1 with the new population obtained in step 5.7 until the highest fitness value in step 5.4 reaches the preset highest fitness threshold.

[0138] Step 6: Based on the optimal matching strategy obtained in Step 5 and the first optimal motion time of each drive axis obtained in Step 2, calculate the optimal motion trajectory of the robot. The specific process is as follows:

[0139] Step 6.1: Determine the optimal speed percentage of each drive shaft based on the optimal matching strategy obtained in Step 5;

[0140] Step 6.2: Based on the optimal speed percentage of each drive shaft determined in Step 6.1 and the first optimal motion time of each drive shaft obtained in Step 2, calculate the second optimal motion time of each drive shaft using the following formula:

[0141] tj T =tj min / η j

[0142] In the formula, tj min For the first optimal motion time of the j-th drive shaft, η j The percentage of the optimal motion speed of the j-th drive shaft;

[0143] Step 6.3: Based on the second optimal motion time of each drive shaft, establish a second polynomial about time, and calculate the second optimal position-time function of each drive shaft based on the second polynomial. The difference between the calculation of the second polynomial and the second optimal position-time function and the first polynomial and the first optimal position-time function lies in the different time values ​​in the formula.

[0144] Step 6.4: Determine the optimal motion trajectory of the robot based on the second optimal position-time function of each drive axis obtained in Step 6.3.

[0145] The time-optimized particle swarm optimization algorithm in this embodiment exhibits consistent convergence. Taking the grabbing process as an example, after substituting the process actions into the optimization algorithm, the motion time of each iteration is calculated. As the number of iterations increases, the motion time gradually converges. The time convergence of each iteration is shown below. Figure 11 As shown, the optimal query read time can reach 0.001s;

[0146] The final time of the loading and retrieval process in this embodiment is shown in Figure 12. In this embodiment, the traditional PTP trajectory planning, end-effector trajectory optimization, and the speed optimization method combining obstacle avoidance proposed in this invention are used for verification and comparison. The comparison results are shown in Figure 12. Figure 13 To avoid interference, multiple trajectory feature points need to be set. Traditional PTP trajectory planning requires frequent starts and stops at trajectory feature points, which takes up a lot of time. Although the end trajectory optimization method achieves continuous speed at feature points, the feature points themselves are a constraint, affecting the search for the optimal result. Therefore, the method proposed in this invention can improve by 10% compared to end trajectory optimization.

[0147] The loading robot's grasping process in this embodiment is as follows: Figure 14 As shown, the material grabbing process can be divided into three steps: moving to the cargo hold opening, entering the cargo hold to grab the material, and clamping the material. In step 1, the x-axis moves in the positive direction, the y-axis moves in the negative direction, the swing shaft rotates, and when moving to the cargo hold opening, the swing shaft is adjusted to the opening direction. In step 2, both the x and y axes move in the positive direction to grab the material at the same angle as the cargo hold opening. In step 3, except for the clamping shaft, the other axes do not move.

[0148] The process of retrieving materials from the cargo hold is a "fully constrained" condition. In this condition, the x-axis and y-axis are linked, and the x-axis movement direction is the same as the previous step. Therefore, the x-axis motion trajectories of the first two steps and the y-axis motion of the second step are merged into a single unit. Treating the y-axis and pendulum axis motion of step 1 as separate entities, and substituting them into the particle swarm optimization algorithm, we can obtain the following... Figure 15 Position, velocity, and acceleration curves are shown. The maximum velocity along the x-axis is 850 mm / s, the maximum velocity along the y-axis is 420 mm / s, and the maximum velocity along the pendulum axis is 200 deg / s. All drive axes can reach their maximum speeds.

[0149] The loading robot retrieval process in this embodiment is as follows: Figure 16 As shown, the retraction condition from release to retraction to zero position only considers the interference with the outer wall of the cargo hold. The motion of each axis can be independently substituted into the particle swarm optimization algorithm. The optimized position, velocity, and acceleration curves of each drive axis are shown in Figure 17.

[0150] This embodiment allocates the timing of actions for the material picking and retrieval processes separately, such as... Figure 18 and 19 As shown. The material handling process includes 5 timing parameters: the waiting time t1 before the 3 drive shafts move. f t2 f t3 f In the movement to the hatch, the speed ratios η1 and η2 of the y-axis and swing axis are considered. However, in the material handling step, due to spatial linkage requirements between the x-axis and y-axis, no speed scaling is applied. The retraction process encompasses six timing parameters. These parameters serve as genetic factors for the legacy algorithm, and are substituted into the algorithm for optimization to obtain the optimal motion trajectory that meets safety margins, such as... Figure 20 and 21 As shown.

Claims

1. A method for calculating the optimal time-based motion trajectory of a robot combined with obstacle avoidance strategies, characterized in that, Includes the following steps: Step 1: Determine the starting position, ending position, and motion trajectory feature points of each drive axis on the robot based on experience; Step 2: Based on the starting position, ending position and motion trajectory feature points of each drive shaft determined in Step 1, calculate the first optimal motion time for each drive shaft using a time-optimized particle swarm optimization algorithm. Step 3: Based on the first optimal motion time of each drive shaft obtained in Step 2, establish a first polynomial about time, and calculate the first optimal position-time function of each drive shaft based on the first polynomial. Step 4: Based on the first optimal motion time of each drive axis obtained in Step 2 and the preset waiting time before each drive axis performs its action, determine whether there is interference between the combined motion of each drive axis and the external environment. If yes, proceed to Step 5. If no, calculate the robot's optimal motion trajectory based on the first optimal position-time function of each drive axis obtained in Step 3, and complete the calculation of the robot's time-optimal motion trajectory. Step 5: Using a genetic algorithm, adjust the preset waiting time before each drive axis performs an action and the preset optimal speed percentage of each drive axis to obtain the optimal matching strategy for the motion timing between each drive axis after avoiding interference. Step 6: Based on the optimal matching strategy obtained in Step 5 and the first optimal motion time of each drive axis obtained in Step 2, calculate the optimal motion trajectory of the robot and complete the calculation of the robot's time-optimal motion trajectory.

2. The method for calculating the optimal robot trajectory in time by combining obstacle avoidance strategy according to claim 1, characterized in that, Step 2 is as follows: Step 2.1: Based on the starting position, ending position, and motion trajectory feature points of each drive shaft determined in Step 1, divide the motion trajectory of each drive shaft into continuous trajectory segments. Step 2.2: With the goal of optimizing the motion time of each drive shaft, determine the time objective function and constraints for each drive shaft based on the trajectory segments obtained in Step 2.

1. Step 2.3: Based on the time objective function and constraints of each driving axis obtained in Step 2.2, determine the M sets of time series corresponding to each driving axis, M≥1, and use the M sets of time series as the population of the particle swarm algorithm. Initialize the velocity parameters, inertia factor and acceleration constant of each particle in the population. Step 2.4: Based on the M sets of time series corresponding to each drive shaft obtained in Step 2.3 and the time objective function of each drive shaft obtained in Step 2.2, calculate the local optimal solution p of the M sets of time series corresponding to each drive shaft. mbest According to the local optimal solution p mbest Calculate the global optimal solution g for the M time series corresponding to each drive shaft. best The global optimal solution g best This is the first optimal motion time for each drive shaft; Step 2.5: Determine the global optimal solution g obtained in Step 2.

4. best If the predetermined threshold is met, the calculation ends, and the global optimal solution g is obtained. best The first optimal motion time for each drive shaft is determined; otherwise, proceed to step 2.

6. Step 2.6: Based on the local optimal solution p obtained in Step 2.4 mbest Global optimal solution g best Using the M time series, velocity parameters, inertia factors, and acceleration constants determined in step 2.3, calculate the new velocity parameters: Step 2.7: Based on the M time series obtained in Step 2.3 and the new velocity parameters obtained in Step 2.6, determine the new M time series; Step 2.8: Return to Step 2.3, and use the new M sets of time series as the M sets of time series corresponding to each drive axis, until the global optimal solution g in Step 2.5 is obtained. best The predetermined threshold is met.

3. The method for calculating the optimal robot trajectory in time by combining obstacle avoidance strategy according to claim 2, characterized in that, Step 5 is as follows: Step 5.1: Based on the preset waiting time before each drive shaft performs an action and the preset optimal speed percentage of each drive shaft, randomly generate a population; Step 5.2: Determine the optimization objective function based on the preset waiting time before each drive axis performs its action and the preset percentage of the optimal motion speed of each drive axis; Step 5.3: Based on the optimization objective function determined in Step 5.2, calculate the fitness value of each individual in the population obtained in Step 5.1, select the individual with the highest fitness value as the optimal parent individual, and select individuals that meet the preset fitness threshold to establish a population of individuals to be crossed. Step 5.4: Determine whether the highest fitness value has reached the preset highest fitness threshold. If yes, determine the optimal matching strategy for the motion timing between each drive axis after avoiding interference based on the best parent individual. If no, proceed to step 5.

5. Step 5.5: Perform crossover operations on the group of individuals to be crossovered established in Step 5.3 to generate the next generation of individuals; Step 5.6: Perform a low-probability mutation on the next generation individuals obtained in Step 5.5 to generate mutated individuals; Step 5.7: Combine the optimal parent individuals obtained in Step 5.3, the next generation individuals obtained in Step 5.5, and the mutated individuals obtained in Step 5.6 into a new population; Step 5.8: Return to step 5.3 and replace the population obtained in step 5.1 with the new population obtained in step 5.7 until the highest fitness value in step 5.4 reaches the preset highest fitness threshold.

4. The method for calculating the optimal robot motion trajectory in time by combining obstacle avoidance strategy according to claim 3, characterized in that, Step 6 specifically involves: Step 6.1: Determine the optimal speed percentage of each drive shaft based on the optimal matching strategy obtained in Step 5; Step 6.2: Based on the optimal speed percentage of each drive shaft determined in Step 6.1 and the first optimal motion time of each drive shaft obtained in Step 2, calculate the second optimal motion time tj of each drive shaft. T The calculation formula is as follows: ie T =ie min / η j In the formula, tj min For the first optimal motion time of the j-th drive shaft, η j The percentage of the optimal motion speed of the j-th drive shaft; Step 6.3: Based on the second optimal motion time tj of each drive shaft T Establish a second polynomial with respect to time, and calculate the second optimal position-time function for each drive shaft based on the second polynomial; Step 6.4: Based on the second optimal position-time function of each drive axis obtained in Step 6.3, calculate the optimal motion trajectory of the robot, and complete the calculation of the robot's time-optimal motion trajectory.

5. The method for calculating the optimal robot trajectory in time by combining obstacle avoidance strategy according to claim 4, characterized in that: The new velocity parameter calculation formula in step 2.6 is as follows: In the formula, v m (n) represents the new velocity parameter, ω is the inertia factor, c1 and c2 are the first and second acceleration constants respectively, r1 and r2 are random numbers in the range of 0 to 1, m is any one of the M time series, m = 1, 2, 3, ..., M, n is the current iteration number, and t m (n) is the time parameter for the current iteration, v m (n-1) represents the particle velocity from the previous iteration. For the local optimum of the current iteration, g best (n) represents the global optimal solution in the current iteration; The new formula for calculating the M group of time series in step 2.7 is as follows: t m (n+1)=t m (n)+v m (n) In the formula, t m (n)=[t m1 (n), t m2 (n), ..., t mi [n] represents the time parameter for the nth iteration.

6. The method for calculating the optimal robot motion trajectory in time by combining obstacle avoidance strategy according to claim 5, characterized in that, The formulas for calculating the first optimal position-time function of each drive shaft in step 3 are as follows: q i (t i )=a i0 +a i1 t i +a i2 t i 2 +…+a ik t i k ,3≤k≤5 In the formula, q i (t i Let t be the optimal position function for each drive shaft, i be the number of segments of the drive shaft's motion trajectory, and t be the position function for each drive shaft. i Let k be the time taken by the robot to run on each segment of the trajectory, k be the order of the spline polynomial, and a be the time taken by the robot to run on each segment of the trajectory. i0 a i1 …a ik The robot trajectory parameters are obtained by solving the least squares method based on the established first polynomial.

7. The method for calculating the robot's time-optimal motion trajectory combined with obstacle avoidance strategy according to claim 6, the method for determining whether there is interference between the combined motion of each drive axis and the external environment in step 4 is as follows: The outer contour of the robot link is simplified to a circle and an ellipse. The line connecting the center of the drive shaft to the end point of the robot device is regarded as an envelope circle formed by a series of circular intervals. If the distance from the midpoint of the robot link to the interference indicator is less than the radius of the envelope circle, then interference is considered to exist; otherwise, interference is considered not to exist.

8. The method for calculating the optimal robot trajectory in time by combining obstacle avoidance strategy according to claim 7, characterized in that, In step 2.2: The time objective function formula is as follows: In the formula, t i Let N be the time required for the drive shaft to move between the i-th and i+1-th trajectory feature points, and N be the total number of all segmented curves of the drive shaft. The constraints are defined as follows: g min ≤g i ≤g max In the formula, g i Let q be the position, velocity, and acceleration on the i-th segment of the drive shaft's trajectory. i Let be the position on the i-th segment of the trajectory. Let be the velocity on the i-th segment of the drive shaft's trajectory. Let g be the acceleration on the i-th segment of the trajectory of the drive shaft. min q represents the minimum limit position, velocity, and acceleration of the drive shaft. min For the joint negative limit of the drive shaft, g max q represents the maximum limit position, velocity, and acceleration of the drive shaft. max For the joint of the drive shaft, positive limit position. The rated speed of the drive shaft, For the rated acceleration.

9. The method for calculating the optimal robot trajectory in time by combining obstacle avoidance strategy according to claim 8, characterized in that, In step 5.2: The formula for optimizing the objective function is as follows: target fuction=min[max(t1 f +t1 T ,…,tj f +tj T )] In the formula, t1 f …,tj f t1 is the waiting time before each drive axis performs an action. T …,tj T The optimal percentage of motion speed for each drive shaft; (x inf ,y inf For the position of each point, l link l is the distance from each point on the center line of the rotating arm to the center of the rotation axis. end r is the distance from the center point of the fixture to the center of the rotation axis. link r is the maximum distance from the centerline of the robot's rotating arm to the surface. end Let q1, q2, and q3 be the radius of the robot's end effector gripper, and q1, q2, and q3 be the displacement and angle corresponding to each drive axis, respectively.

Images