Robot collision avoidance trajectory planning method for multiple waypoints
By combining the progressive solution of the progressive and global formula, the online trajectory generation and improved whale algorithm solve the problem of long -distance robot trajectory planning time and collision in existing technology, and realize the shortest exercise time and non -collision trajectory planning.
Patent Information
- Application Number
- CN202411474078.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-21
- Publication Date
- 2025-01-24
AI Technical Summary
The prior art is difficult to quickly generate multi-point robot trajectory while ensuring the kinematic constraints of the robot, so that the motion time is as short as possible and avoid collisions.
Use the progressive solution solution method to quickly generate the initial feasible trajectory, and use the global pursuit solution to perform secondary optimization in high -dimensional space, combine the online trajectory to generate and improve the whale algorithm, optimize the middle road point status to reduce the movement time, while adding a collision testing To ensure the physical feasibility of the trajectory.
In the case of ensuring exercise constraints, the multi -point robot trajectory that generates the shortest exercise time, and ensure that the trajectory has no collision, and make full use of the motion ability of the robotic arm to improve the efficiency of exercise.
Smart Images
Figure CN119356323A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the field of robots, and in particular to a robot collision avoidance trajectory planning method for multiple waypoints. Background Art
[0002] In industrial production, robots often need to perform some fixed-point movements, such as cargo handling or workpiece assembly. However, in some scenarios, the robot must traverse some newly added work points without stopping, such as painting or flying photography tasks. These newly added work points are called waypoints of the robot. When multiple waypoints are specified in a task, the robot usually needs to traverse them quickly while ensuring kinematic constraints. Therefore, this requires the robot planning algorithm to generate the optimal motion trajectory with the goal of minimizing motion time while ensuring kinematic constraints.
[0003] The multi-waypoint planning problem is currently widely used in the fields of mobile robots and drones. Compared with these fields, the multi-waypoint planning problem of typical industrial robots, such as 6-DOF manipulators, is more complicated. This complexity comes from its smaller workspace and higher dimensionality. A small change in the waypoints can make a significant difference in the entire trajectory. With each additional waypoint, the overall trajectory needs to be globally replanned, which also makes the multi-waypoint planning problem in robots more difficult.
[0004] At present, most of the existing robot trajectory planning methods for multiple waypoints focus on fitting with spline curves. This type of method often uses cubic B-splines or quintic B-splines to construct constraint equations for path points, thereby optimizing the corresponding spline curve parameters and finally forming the optimal solution. The motion curve formed in this way is often C1 or C2 continuous and has good motion performance. However, although this method can generate a smooth motion trajectory, it does not fully utilize the motion ability of the robot arm, resulting in a long planned motion time. Some methods using S-curves follow the principle of Bang-Bang control and can optimize faster robot motion. However, the model established by this method is difficult to solve, and often requires many additional constraints to solve it, and the number of waypoints in the application scenario is limited. In addition, the method based on the S-curve does not take into account the self-collision or environmental collision problems in the robot's motion process, which often makes the planned trajectory physically infeasible.
[0005] Based on the above problems, the present invention proposes a multi-waypoint robot collision avoidance trajectory planning method. The method consists of two parts: an incremental solution algorithm for quickly generating a feasible trajectory, and a global solution algorithm for secondary optimization in a high-dimensional space. The two parts of the algorithm can be used in scenarios with high requirements for algorithm calculation time or high requirements for trajectory running time. By combining the two algorithms, the solution to the robot's multi-waypoint problem can be achieved. In addition, compared with other multi-waypoint trajectory planning methods, this method itself can also add collision detection to ultimately form a collision-free planning trajectory. Summary of the invention
[0006] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is how to realize the robot collision avoidance trajectory planning of multiple waypoints.
[0007] To achieve the above-mentioned purpose, the present invention provides a robot collision avoidance trajectory planning method for multiple waypoints, characterized in that it comprises two parts, an incremental solution algorithm for quickly generating a feasible trajectory, and a global solution algorithm for performing secondary optimization in a high-dimensional space; the two parts of the algorithm can be used for scenarios with high requirements on algorithm calculation time or high requirements on trajectory running time, and by combining the two algorithms, the solution to the robot's multiple waypoint problem can be achieved; in addition, compared with other multi-waypoint trajectory planning methods, the present method itself can also add collision detection to ultimately form a collision-free planning trajectory.
[0008] Furthermore, the method preparation work comprises the following steps:
[0009] Step 1: Model the overall working scene of the robot;
[0010] Step 2: Train the corresponding discriminant neural network according to the robot's kinematic model;
[0011] Step 3: mathematically describe the multi-way point problem;
[0012] Step 4: Introduce the algorithm for online trajectory generation.
[0013] Furthermore, the information that needs to be obtained in step 1 includes the robot's kinematic model and collision model, and the position and direction of the waypoint relative to the robot's origin coordinate system; if obstacles are involved in the scene, their position information and shape information should also be obtained.
[0014] Furthermore, the input of step 2 is the test point in space and the corresponding joint configuration of the robotic arm, and the output is the distance from the test point to each link of the joint; whether a collision occurs is determined by the physical model and distance of the link, so as to design the corresponding self-collision and environment collision programs.
[0015] Furthermore, in step 3, x i (t)=(p i (t),v i (t),a i (t)) represents the state of the i-th axis (i∈{1,2,…N}) at time t, p i (t),v i (t),a i (t) represent joint position, velocity and acceleration respectively; in the multi-waypoint planning problem, the position p passing through the waypoint i is determined, but the speed and acceleration are unknown; only the speed and acceleration values at the starting point and the end point are 0; the optimization model expression is:
[0016]
[0017] Among them, t s and t e is the start and end time; j i (t) is the jerk rate, which is the derivative of acceleration; the optimization model is achieved by optimizing j i (t) to obtain the shortest running time t * ;t k represents the moment when the robot passes the kth waypoint, and P ik represents the position of the robot arm when the i-th degree of freedom passes through the k-th waypoint; X is =(P is ,0,0),X ie =(P ie ,0,0), indicating the starting and ending states.
[0018] Furthermore, the function of step 4 is to quickly generate a corresponding trajectory that satisfies the kinematic constraints for a problem with only one starting point and one end point, where the starting point and end point states x i (t) can be specified arbitrarily; with the help of the online trajectory generation algorithm, for the multiple waypoint problem, i The determination of (t) is transformed into the state x of the intermediate waypoint i v in (t) i (t) and a i Optimization of (t).
[0019] Furthermore, the method comprises the following steps:
[0020] Step S1, firstly, using an incremental solution algorithm to generate an initial solution;
[0021] Step S2, through the two-point method of online trajectory generation in step 4, an evaluation system function can be obtained, the input of the evaluation system function is the state of the middle point, and the output is the total trajectory running time of the three-point problem; the evaluation system function is not only used for three-point problems, but also applicable to multi-way points. The evaluation system function is nonlinear, so in the present invention, the whale algorithm is used to find the minimum value of the function;
[0022] Step S3, assuming that there are m intermediate waypoints in total, in each three-point problem, the terminal velocity and acceleration state are both set to 0, and the initial state is the optimized solution obtained from the previous three-point problem, and then the state of the intermediate point is optimized by using the above-mentioned online trajectory generation and whale algorithm; in this way, after solving the m three-point problems, the initial state solutions for all waypoints are generated, and the corresponding complete trajectory is obtained by the online trajectory generation method;
[0023] Step S4: Since the state is assumed in the process of incremental solution, the algorithm will eventually converge to the local optimal solution. In order to obtain a better trajectory, the result obtained by incremental solution is used as the initial value and put into the global solver for secondary optimization; the global solution is to solve the state of the intermediate waypoints as a whole;
[0024] Step S5, the robot collision detection is added in the evaluation system function; in step S2, an evaluation function system is established, and the state of the intermediate point is converted into the movement time of the whole robot through the method of online trajectory generation in step 4;
[0025] Furthermore, in step S1, the progressive solution algorithm converts the multi-waypoint problem into multiple three-point problems; wherein the three-point problem refers to a problem with only one intermediate waypoint, and after determining the state of the intermediate waypoint, the three-point problem is regarded as a combination of two online trajectory generation.
[0026] Furthermore, in step S3, in order to optimize in high-dimensional space, the whale algorithm is improved. First, an initial value is input into the whale algorithm to control the generation of the population; secondly, the range control parameter A0 in the whale algorithm is iteratively decreased. Through continuous decrease, the algorithm gradually transitions from the original exploration behavior to the development behavior, so that it can perform global optimization in the early stage and local refinement in the later stage, completing the secondary optimization of the robot arm trajectory.
[0027] Furthermore, in step S5, the evaluation system function is used to detect obstacles or the connecting rod itself using the method of step 2. If a collision occurs, the movement time output by the function is infinite, so that the fitness of the individual in the whale population is reduced and it is gradually eliminated during the evolution process; therefore, the method framework proposed in the present invention is compatible with collision detection.
[0028] This method can fully utilize the kinematic capabilities of the robot arm, making the overall movement more efficient.
[0029] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS
[0030] Figure 1 It is an overall module diagram of a preferred embodiment of the present invention;
[0031] Figure 2 is a schematic diagram of converting a multi-point problem into a three-point problem in a preferred embodiment of the present invention;
[0032] Figure 3 It is a flowchart of an algorithm for progressive solution using online trajectory generation and whale algorithm according to a preferred embodiment of the present invention.
[0033] Figure 4 It is an algorithm flow chart of a preferred embodiment of the present invention using an improved whale algorithm for global solution. DETAILED DESCRIPTION
[0034] The following describes several preferred embodiments of the present invention with reference to the drawings in the specification, so that the technical content is clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.
[0035] Figure 1 This is the overall module diagram of this system. Figure 2 In , each local black box represents a three-point problem, and the problem only contains one intermediate point. In this problem, we set the velocity and acceleration of the terminal state to 0, so as to optimize the motion state of the intermediate point. For example, in the first three-point problem, since the final state x(t2) is unknown, we set its velocity and acceleration states to 0 here. Then according to the velocity and acceleration of the initial point is 0. Using the method of online trajectory generation, as long as there is a v(t1) and a(t1), it can be regarded as two online trajectory generation problems, thereby obtaining the optimal solutions to the two problems. Adding the two time periods can be used as the running time of this three-point problem. Therefore, it can be used Figure 3 The algorithm flow solves this progressive idea. Figure 4 The improved whale algorithm is used to perform secondary optimization on the initial solution. During the optimization process, as the number of iterations of the population increases, the range of its search solution will decrease with the change of A0, so that the optimization process of the overall solution changes from exploration to mining, and finally converges to a better solution.
[0036] In view of the above-mentioned defects of the existing methods, the primary technical problem to be solved by the present invention is to make the movement time of the planned multi-waypoint robot trajectory as short as possible while ensuring the robot's kinematic constraints, and its movement will not cause collision. The steps are as follows:
[0037] Preparation:
[0038] P1: To model the robot's overall working scene, the information that needs to be obtained includes: the robot's kinematic model and collision model, the position and direction of the waypoint relative to the robot's origin coordinate system. If there are obstacles in the scene, their position information and shape information should also be obtained.
[0039] P2: According to the robot's kinematic model, the corresponding discriminant neural network is trained. Its input is the test point in space and the corresponding joint configuration of the robot arm, and the output is the distance from the test point to each link of the joint. The physical model and distance of the link are used to determine whether a collision occurs, so as to design the corresponding self-collision and environment collision programs.
[0040] P3: Make x i (t)=(p i (t),v i (t),a i (t)) represents the state of the i-th axis (i∈{1,2,…N}) at time t, p i (t),v i (t),a i (t) represent joint position, velocity and acceleration respectively; in the multi-waypoint planning problem, the position p passing through the waypoint i is determined, but the speed and acceleration are unknown; only the speed and acceleration values at the starting point and the end point are 0; the optimization model expression is:
[0041]
[0042] Among them, t s and t e is the start and end time; j i (t) is the jerk rate, which is the derivative of acceleration; the optimization model is achieved by optimizing j i (t) to obtain the shortest running time t * ;t k represents the moment when the robot passes the kth waypoint, and P ikrepresents the position of the robot arm when the i-th degree of freedom passes through the k-th waypoint; X is =(P is ,0,0),X ie =(P ie ,0,0), indicating the starting and ending states.
[0043] P4: For a problem with only one starting point and one end point, the corresponding trajectory that satisfies the kinematic constraints can be quickly generated, where the starting and end states x i (t) can be specified arbitrarily; with the help of the online trajectory generation algorithm, for the multiple waypoint problem, i The determination of (t) is transformed into the state x of the intermediate waypoint i v in (t) i (t) and a i Optimization of (t).
[0044] Multi-waypoint trajectory planning algorithm:
[0045] W1: Although the problem can be reduced in dimension and transformed by online trajectory generation, optimizing the states of multiple intermediate waypoints at the same time is also a high-dimensional space optimization problem, which is often difficult to solve. Therefore, we first use the incremental solution algorithm to generate the initial solution.
[0046] W2: The algorithm of progressive solution converts the multi-way point problem into multiple three-point problems. Among them, the three-point problem refers to a problem with only one middle way point. After determining the state of the middle way point, it can also be regarded as a combination of two online trajectory generation. Therefore, through the two-point method of online trajectory generation, we can get an evaluation system, the function input is the state of the middle point, and the function output is the total trajectory running time of the three-point problem. It is worth mentioning that this evaluation system is not only used for three-point problems, but also applicable to multi-way points. However, this function is nonlinear, so in the present invention, it is considered to use the whale algorithm to find the minimum value of the function.
[0047] W3: Assume that there are m intermediate waypoints in total. In each three-point problem, we set the terminal velocity and acceleration states to 0, and the initial state to the optimized solution obtained from the previous three-point problem. Then, we use the online trajectory generation and whale algorithm mentioned above to optimize the state of the intermediate points. In this way, after solving the m three-point problems, we can generate the initial state solutions for all waypoints, and obtain the corresponding complete trajectory through the online trajectory generation method.
[0048] W4: Since we make assumptions about the state during the incremental solution, the algorithm will eventually converge to a local optimal solution. In order to obtain a better trajectory, we use the result obtained by incremental solution as the initial value and put it into the global solver for secondary optimization.
[0049] W5: Global solution is to solve the state of the intermediate waypoints as a whole. In order to optimize in high-dimensional space, we have improved the whale algorithm. First, an initial value is input into the whale algorithm to control the generation of the population. Secondly, the range control parameter in the whale algorithm is iteratively reduced. Through continuous reduction, the algorithm gradually transitions from the original exploration behavior to the development behavior, so that it can perform global optimization in the early stage and local refinement in the later stage. Thus, the secondary optimization of the robot arm trajectory is completed.
[0050] W6: The robot's collision detection is added in the evaluation function. In W3, an evaluation function system is established, and the state of the intermediate point is converted into the movement time of the entire robot through the method of online trajectory generation. In this function, the generated trajectory can be further subjected to collision detection, and the obstacle or the connecting rod itself can be detected using the P2 method. If a collision occurs, the movement time output by the function is infinite. As a result, the fitness of the individual in the whale population is reduced, and it is gradually eliminated during the evolution process. Therefore, the method framework proposed in the present invention is compatible with collision detection.
[0051] The preferred specific embodiments of the present invention are described in detail above. It should be understood that ordinary technicians in the field can make many modifications and changes based on the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by technicians in the technical field based on the concept of the present invention through logical analysis, reasoning or limited experiments on the basis of the prior art should be within the scope of protection determined by the claims.
Claims
1. A robot collision avoidance trajectory planning method for multiple waypoints, characterized in that: It consists of two parts: an incremental solution algorithm for quickly generating a feasible trajectory, and a global solution algorithm for performing secondary optimization in a high-dimensional space. The two algorithms can be used in scenarios with high requirements for algorithm calculation time or high requirements for trajectory running time. By combining the two algorithms, the robot's multi-waypoint problem can be solved. In addition, compared with other multi-waypoint trajectory planning methods, this method itself can also add collision detection to ultimately form a collision-free planning trajectory.
2. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 1, characterized in that: The method preparation comprises the following steps: Step 1: Model the overall working scene of the robot; Step 2: Train the corresponding discriminant neural network according to the robot's kinematic model; Step 3: mathematically describe the multi-way point problem; Step 4: Introduce the algorithm for online trajectory generation.
3. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 2, characterized in that: The information that needs to be obtained in step 1 includes the robot's kinematic model and collision model, and the position and direction of the waypoint relative to the robot's origin coordinate system; if obstacles are involved in the scene, their position information and shape information should also be obtained.
4. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 3, characterized in that: The input of step 2 is the test point in space and the corresponding joint configuration of the robotic arm, and the output is the distance from the test point to each link of the joint; the physical model and distance of the link are used to determine whether a collision occurs, thereby designing the corresponding self-collision and environmental collision programs.
5. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 4, characterized in that: In step 3, x i (t)=(p i (t),v i (t),a i (t)) represents the state of the i-th axis (i∈{1,2,…N}) at time t, p i (t),v i (t),a i (t) represents joint position, velocity and acceleration respectively; In the multi-waypoint planning problem, the position p of the waypoint is i is determined, but the speed and acceleration are unknown; only the speed and acceleration values at the starting point and the end point are 0; the optimization model expression is: Among them, t s and t e is the start and end time; j i (t) is the jerk rate, which is the derivative of acceleration; the optimization model is achieved by optimizing j i (t) to obtain the shortest running time t * ;t k represents the moment when the robot passes the kth waypoint, and P ik represents the position of the robot arm when the i-th degree of freedom passes through the k-th waypoint; X is =(P is ,0,0),X ie =(P ie ,0,0), indicating the starting and ending states.
6. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 5, characterized in that: The function of step 4 is to quickly generate the corresponding trajectory that satisfies the kinematic constraints for a problem with only one starting point and one end point, where the starting point and end point states x i (t) can be specified arbitrarily; with the help of the online trajectory generation algorithm, for the multiple waypoint problem, i The determination of (t) is transformed into the state x of the intermediate waypoint i v in (t) i (t) and a i Optimization of (t).
7. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 6, characterized in that: The method comprises the following steps: Step S1, firstly, using an incremental solution algorithm to generate an initial solution; Step S2, through the two-point method of online trajectory generation in step 4, an evaluation system function can be obtained, the input of the evaluation system function is the state of the middle point, and the output is the total trajectory running time of the three-point problem; the evaluation system function is not only used for three-point problems, but also applicable to multi-way points. The evaluation system function is nonlinear, so in the present invention, the whale algorithm is used to find the minimum value of the function; Step S3, assuming that there are m intermediate waypoints in total, in each three-point problem, the terminal velocity and acceleration state are both set to 0, and the initial state is the optimized solution obtained from the previous three-point problem, and then the state of the intermediate point is optimized by using the above-mentioned online trajectory generation and whale algorithm; in this way, after solving the m three-point problems, the initial state solutions for all waypoints are generated, and the corresponding complete trajectory is obtained by the online trajectory generation method; Step S4: Since the state is assumed in the process of incremental solution, the algorithm will eventually converge to the local optimal solution. In order to obtain a better trajectory, the result obtained by incremental solution is used as the initial value and put into the global solver for secondary optimization; the global solution is to solve the state of the intermediate waypoints as a whole; In step S5, the collision detection of the robot is added in the evaluation system function; in step S2, an evaluation function system is established, and the state of the intermediate point is converted into the movement time of the entire robot through the online trajectory generation method in step 4.
8. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 7, characterized in that: In step S1, the progressive solution algorithm converts the multi-waypoint problem into multiple three-point problems; wherein the three-point problem refers to a problem with only one intermediate waypoint. After determining the state of the intermediate waypoint, the three-point problem is regarded as a combination of two online trajectory generation.
9. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 8, characterized in that: In step S3, in order to optimize in high-dimensional space, the whale algorithm is improved. First, an initial value is input into the whale algorithm to control the generation of the population; secondly, the range control parameter A0 in the whale algorithm is iteratively decreased. By continuously decreasing, the algorithm gradually transitions from the original exploration behavior to the development behavior, so that it can perform global optimization in the early stage and local refinement in the later stage, completing the secondary optimization of the robot arm trajectory.
10. The robot collision avoidance trajectory planning method for multiple waypoints as claimed in claim 9, characterized in that: In step S5, the evaluation system function is used to detect obstacles or the connecting rod itself using the method of step 2. If a collision occurs, the movement time output by the function is infinite, so that the fitness of the individual in the whale population is reduced and it is gradually eliminated during the evolution process; therefore, the method framework proposed in the present invention is compatible with collision detection.