Mechanical arm motion path iterative optimization method based on batch collision detection

Abstract

The invention discloses a batch collision detection-based mechanical arm motion path iterative optimization method, which comprises the following steps of: firstly, carrying out equidistant sampling on an initial joint space motion path of a mechanical arm, forming a local candidate shortcut between any two sampling nodes, then carrying out equidistant interpolation on each local candidate shortcut, and carrying out iterative optimization on each local candidate shortcut; performing batch collision detection by using a gap neural network model so as to identify a # imgabs0 # collision-free local shortcut; aiming at a # imgabs 1 # collision-free local shortcut, a shortest global optimization path is found; and the steps are iterated, the initial sampling number is updated in each iteration process, when the length of the optimized path or the optimization time meets a preset condition, the iteration process is ended, and finally a better mechanical arm global path is obtained. And the joint space path length of the mechanical arm is adaptively optimized in an iterative optimization mode, the optimization effect is prevented from being influenced by the change of the path length, and efficient and rapid optimization of the motion path of the mechanical arm is achieved.

Description

Technical Field

[0001] The invention belongs to the technical field of robot motion optimization, and in particular relates to an iterative optimization method for a robot arm motion path based on batch collision detection. Background Art

[0002] The optimization of the robot's motion path is an important part of the robot's motion planning process. Although the sampling-based motion planning algorithm (RRT, PRM, etc.) can find a collision-free motion path for the robot relatively quickly, since such algorithms use random sampling to plan paths, the length of the planned robot's motion path is not optimal, and there is a large optimization space. In addition, in the real-time motion planning scenario of the robot, it is not only required that the robot quickly plan a feasible path, but also that the motion path optimization time is as short as possible. Existing path optimization methods, such as cubic spline interpolation, can find a smoother path, but the motion path optimization efficiency of such methods is usually difficult to meet the needs of real-time motion planning. The most important reason is that there are a large number of collision detections in the process of optimizing the robot's motion path, which takes a lot of motion time. Therefore, how to quickly optimize the motion path after finding a collision-free path for the robot has become a technical problem that needs to be solved in the real-time motion planning scenario of the robot.

[0003] At present, some scholars have proposed some motion path optimization methods based on learning to accelerate the robot motion path optimization process. However, such methods are greatly affected by the change of the robot path length and are difficult to adapt to complex robot motion path optimization tasks; and the optimization performance in terms of optimization time, path length, etc. needs to be further improved. In response to the above problems and challenges, the present invention proposes an iterative optimization method for robot motion paths based on batch collision detection. Summary of the invention

[0004] In response to the above technical problems, the present invention provides an iterative optimization method for a robot motion path based on batch collision detection, aiming to solve the problem that existing robot motion path optimization methods are inefficient and difficult to adapt to complex robot motion path optimization tasks.

[0005] The technical solution adopted by the present invention to solve the technical problem is: A method for iterative optimization of a robot arm motion path based on batch collision detection, the method comprising the following steps: S100: Perform equidistant sampling on the initial joint space motion path of the robot arm. The number of sampling points is , forming a local candidate shortcut between any two sampling nodes, forming a total of local candidate shortcuts; S200: For each local candidate shortcut, Perform equidistant interpolation to obtain interpolation points, interpolation points as input, and use the gap neural network model to perform batch collision detection to identify collision-free local shortcuts; S300: For A collision-free local shortcut is created, each shortcut connects two different robot joint space configuration points, the collision-free local shortcut is regarded as an edge of a graph, each configuration point is regarded as a node of the graph, an initial distance value is assigned to each node, and a priority queue is created for path exploration, the shortest path is found and recorded, backtracked, and the path sequence obtained by backtracking and its total path length are output to obtain a shortest global optimization path found in the collision-free local shortcut; S400: Iterate S100-S300, and the number of initial sampling points will be updated during each iteration , , It is the incremental value of the number of sampling points in the iterative process. When the preset iteration termination condition is met, the iterative process ends, and finally a better global path of the robot arm is obtained.

[0006] Preferably, S100 includes: S110: After preliminary motion planning, an initial path in the joint space of the robot arm is obtained, which is expressed as , The starting configuration node is , the target configuration node is , the robot arm joint space is represented as ,in represents the degrees of freedom of the robot arm, then the starting configuration node of the robot arm and the target configuration node In the joint space, they are expressed as and , The initial path length is , The calculation method is as follows: ; in Represents the initial path of the robot joint space The number of path nodes included; , express Any two adjacent joint space path nodes in ; S120: Initial joint space motion path of the robot arm Perform equidistant sampling, and set the number of sampling points to , that is, the sampling step length is , configure the path starting point and path target configuration points Including, a total of sampling nodes; S130: construct a local candidate shortcut between any two sampling nodes, and construct a total of H local candidate shortcuts, where .

[0007] Preferably, S200 includes: S210: For each local candidate shortcut, Perform equidistant interpolation. A total of local candidate shortcuts are obtained interpolation points; S220: Assume that the number of obstacles in the robot arm operation space is The trained gap neural network model is used to interpolation points for batch collision estimation, and obtain The gap prediction value matrix corresponding to the interpolation points , according to the gap prediction value matrix Determine whether the robot arm joint configuration is When the interpolation point is reached, whether the robot arm collides with an obstacle; S230: For any local candidate shortcut, if there is an interpolation point predicted as a "collision" in the local candidate shortcut, the local candidate shortcut is discarded, otherwise it is retained, so as to finally identify Collision-free local shortcuts.

[0008] Preferably, S220 includes: Will The interpolation point data is input into the gap neural network model at one time for batch data processing, and the The gap prediction value matrix corresponding to the interpolation points ,in, for dimensional matrix, k=1,2,…, ; For the matrix For each element in , if its value is less than the safety threshold , then the element is set to 1, otherwise it is set to 0; For any gap prediction value matrix , if the matrix If all the elements are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with an obstacle; After completing the above operations, you can obtain Collision estimation results for interpolation points.

[0009] Preferably, the gap reasoning network model includes four parts that are forward connected in sequence. The first part uses the robot joint configuration node as the initial input and uses the kernel function , encode the joint angles and increase the frequency of the input values, where Represents the input robot joint configuration node, is an overclocking parameter. The output of the first part will be used as the input of the second part. The second part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the second part will be used as the input of the third part. The third part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the third part will be used as the input of the fourth part. The fourth part consists of a fully connected layer, and the output feature number is , the purpose is to achieve The mapping of the dimensional gap distance is used to predict the robot arm’s position in the operating space under the current joint configuration. The clearance value of each obstacle.

[0010] Preferably, S300 includes: S310: Acquired collision-free local shortcuts, each shortcut connects two different robot joint space configuration points, these collision-free local shortcuts are regarded as edges of a graph, and each configuration point is regarded as a node of the graph; S320: Initialization distance: Assign an initial distance value to each node, which is used to record the distance from the robot arm to the starting configuration node. The shortest path distance to the node, set the starting configuration node The distance of is 0, and the initial distance values ​​of all other nodes are set to infinity, indicating that they are not yet reachable; S330: Create a priority queue: Create a priority queue and start the configuration node and its distance as the initial element to be added to the queue. The priority queue will be arranged from small to large according to the node distance, so that the node with the smallest current distance can be processed each time; S340: Path exploration until the target configuration point is found The shortest path or priority queue is empty; S350: Record the shortest path: while updating the distance, record the predecessor node of each node; S360: Backtracking path: When the priority queue is processed or the target configuration node After the shortest path is found, the predecessor node record is used to Going back to , generate a complete shortest path; S370: Output path: Output the path sequence and total path length obtained by backtracking, and obtain the shortest global optimization path found in the collision-free local shortcut .

[0011] Preferably, S340 includes: S341: Take the node with the smallest distance value from the priority queue and assume it is the current node ; S342: Check from All adjacent nodes of the starting point; S343: For each adjacent node , calculated from arrive Path distance; S344: If the path distance is longer than the currently recorded If the distance is small, update distance and reinsert the node with the updated distance value into the priority queue.

[0012] Preferably, S400 includes: S410: For the found optimized path , iterate S100-S300, and update the number of sampling points in each iteration ,in , The initial value is , that is, the number of sampling points increases during each iteration ; S420: When the length of the optimized path and the optimization time meet one of the following preset conditions, the iterative process ends and a better global path of the robot arm is finally obtained, otherwise the aforementioned iterative optimization process continues to be executed; the preset conditions include: ,in represents the path length of the robot arm before optimization, represents the path length of the robot arm before optimization, , set by the user; or the cumulative optimization time exceeds the preset time .

[0013] The present invention proposes an iterative optimization method for the motion path of a robot arm based on batch collision detection, in which the possibility of finding a shorter path is increased by constructing multiple local candidate shortcuts, and a gap neural network model is used for batch collision detection to accelerate the path optimization process. In addition, the present invention uses an iterative optimization method to adaptively optimize the spatial path length of the robot arm joint, avoiding the optimization effect being affected by the change of the path length, and realizing efficient and rapid optimization of the motion path of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is a flow chart of a method for iterative optimization of a robot arm motion path based on batch collision detection in one embodiment of the present invention; Figure 2 Schematic diagram of candidate local shortcuts in the joint space of a robot arm in one embodiment of the present invention; Figure 3 This is a schematic diagram of a scene of a robotic arm and environmental obstacles in one embodiment of the present invention; Figure 4 A set of example diagrams of optimized robot arm motion paths in one embodiment of the present invention. DETAILED DESCRIPTION

[0015] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0016] In one embodiment, Figure 1 As shown, a method for iterative optimization of a robot arm motion path based on batch collision detection comprises the following steps: S100: Perform equidistant sampling on the initial joint space motion path of the robot arm. The number of sampling points is , forming a local candidate shortcut between any two sampling nodes, forming a total of local candidate shortcuts; S200: For each local candidate shortcut, Perform equidistant interpolation to obtain interpolation points, interpolation points as input, and use the gap neural network model to perform batch collision detection to identify collision-free local shortcuts; S300: For A collision-free local shortcut is created, each shortcut connects two different robot joint space configuration points, the collision-free local shortcut is regarded as an edge of a graph, each configuration point is regarded as a node of the graph, an initial distance value is assigned to each node, and a priority queue is created for path exploration, the shortest path is found and recorded, backtracked, and the path sequence obtained by backtracking and its total path length are output to obtain a shortest global optimization path found in the collision-free local shortcut; S400: Iterate S100-S300, and the number of initial sampling points will be updated during each iteration , , It is the incremental value of the number of sampling points in the iterative process, which is preset by the user. When the preset iteration termination condition is met, the iterative process ends, and finally a better global path of the robot arm is obtained.

[0017] In one embodiment, Figure 2 As shown, S100 includes: S110: After preliminary motion planning, an initial path in the joint space of the robot arm is obtained, which is expressed as , The starting configuration node is , the target configuration node is , the robot arm joint space is represented as ,in represents the degrees of freedom of the robot arm, then the starting configuration node of the robot arm and the target configuration node In the joint space, they are expressed as and , The initial path length is , The calculation method is as follows: ; in Indicates the initial path of the UR5 robot joint space The number of path nodes included; , express Any two adjacent joint space path nodes in ; S120: Initial joint space motion path of the robot arm Perform equidistant sampling, and set the number of sampling points to , that is, the sampling step length is , configure the path starting point and path target configuration points Including, a total of sampling nodes; S130: construct a local candidate shortcut between any two sampling nodes, and construct a total of H local candidate shortcuts, where .

[0018] In one embodiment, Figure 2 As shown, S200 includes: S210: For each local candidate shortcut, Perform equidistant interpolation. A total of local candidate shortcuts are obtained interpolation points; S220: Assume that the number of obstacles in the robot arm operation space is The trained gap neural network model is used to interpolation points for batch collision estimation, and obtain The gap prediction value matrix corresponding to the interpolation points , according to the gap prediction value matrix Determine whether the robot arm joint configuration is When the interpolation point is reached, whether the robot arm collides with an obstacle; S230: For any local candidate shortcut, if there is an interpolation point predicted as a "collision" in the local candidate shortcut, the local candidate shortcut is discarded, otherwise it is retained, so as to finally identify Collision-free local shortcuts.

[0019] In one embodiment, S220 includes: Will The interpolation point data is input into the gap neural network model at one time for batch data processing, and the The gap prediction value matrix corresponding to the interpolation points ,in, for dimensional matrix, k=1,2,…, ; For the matrix For each element in , if its value is less than the safety threshold ( , which can be set by the user), then the element is set to 1, otherwise it is set to 0; For any gap prediction value matrix , if the matrix If all the elements are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with an obstacle; After completing the above operations, you can obtain Collision estimation results for interpolation points.

[0020] In one embodiment, the gap reasoning network model includes four parts that are forward connected in sequence. The first part uses the robot joint configuration node as the initial input and uses the kernel function , encode the joint angles and increase the frequency of the input values, where Represents the input robot joint configuration node, is an overclocking parameter. The output of the first part will be used as the input of the second part. The second part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the second part will be used as the input of the third part. The third part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the third part will be used as the input of the fourth part. The fourth part consists of a fully connected layer, and the output feature number is , the purpose is to achieve The mapping of the dimensional gap distance is used to predict the robot arm’s position in the operating space under the current joint configuration. The clearance value of each obstacle.

[0021] In one embodiment, S300 includes: S310: Acquired collision-free local shortcuts, each shortcut connects two different robot joint space configuration points, these collision-free local shortcuts are regarded as edges of a graph, and each configuration point is regarded as a node of the graph; S320: Initialization distance: Assign an initial distance value to each node, which is used to record the distance from the robot arm to the starting configuration node. The shortest path distance to the node, set the starting configuration node The distance of is 0, and the initial distance values ​​of all other nodes are set to infinity, indicating that they are not yet reachable; S330: Create a priority queue: Create a priority queue and start the configuration node and its distance (0) as the initial element to be added to the queue. The priority queue will be arranged from small to large according to the node distance, so that the node with the smallest current distance can be processed each time; S340: Path exploration until the target configuration point is found The shortest path or priority queue is empty; S350: Record the shortest path: while updating the distance, record the predecessor node of each node; S360: Backtracking path: When the priority queue is processed or the target configuration node After the shortest path is found, the predecessor node record is used to Going back to , generate a complete shortest path; S370: Output path: Output the path sequence and total path length obtained by backtracking, and obtain the shortest global optimization path found in the collision-free local shortcut .

[0022] In one embodiment, S340 includes: S341: Take the node with the smallest distance value from the priority queue and assume it is the current node ; S342: Check from All adjacent nodes of the starting point (i.e. directly connected to other configuration nodes); S343: For each adjacent node , calculated from arrive Path distance; S344: If the path distance is longer than the currently recorded If the distance is small, update distance and reinsert the node with the updated distance value into the priority queue.

[0023] In one embodiment, S400 includes: S410: For the found optimized path , iterate S100-S300, and update the number of sampling points in each iteration ,in , The initial value is , that is, the number of sampling points increases during each iteration ; S420: When the length of the optimized path and the optimization time meet one of the following preset conditions, the iterative process ends and a better global path of the robot arm is finally obtained, otherwise the aforementioned iterative optimization process continues to be executed; the preset conditions include: ,in represents the path length of the robot arm before optimization, represents the path length of the robot arm before optimization, , set by the user; or the cumulative optimization time exceeds the preset time .

[0024] The following is a specific example to perform simulation verification analysis of the proposed method by setting the following experimental parameters: 1) Robot arm simulation environment such as Figure 3 As shown, the UR5 robot was used for verification, including 3 environmental obstacles ( ); 2) Initial value of the number of sampling points Z is 5, s, =1.1, mm; 3) The initial configuration node of the robot arm is =[-2.1808, -1.0708, 1.1, -1.5708, -1.4,0], target configuration node = [-1.2608, -1.0708, 1.1, -1.5708, -1.4, 0]; Initial configuration node in the robot and the target configuration node 50 collision-free motion paths are randomly generated, and the initial average path length is 9.8917 rad. After the path optimization method proposed by the present invention is used, the average path length after optimization is 4.904, and the average optimization time is 0.4619s. The optimization efficiency is far higher than the traditional robot motion optimization method. The optimization time is reduced by 89.16%, Figure 4 A set of optimized robot arm motion paths are shown.

[0025] The above-mentioned iterative optimization method of the robot arm motion path based on batch collision detection has the following beneficial effects: (1) Compared with the traditional robot path optimization method, the iterative optimization method proposed in the present invention increases the possibility of finding a shorter path by simultaneously constructing multiple local candidate shortcuts; at the same time, the present invention uses a gap neural network model for batch collision detection, which greatly shortens the time required for path optimization; (2) Compared with the existing learning-based robot motion path optimization method, the iterative optimization method proposed in the present invention can adaptively adjust the number of optimization times and the number of local candidate shortcuts according to the path length after each optimization, and can be applied to robot motion path optimization tasks with large changes in path length.

[0026] The above is a detailed introduction to the iterative optimization method of the robot arm motion path based on batch collision detection provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of ​​the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A method for iterative optimization of robot arm motion paths based on batch collision detection, characterized in that: The method comprises the following steps: S100: Perform equidistant sampling on the initial joint space motion path of the robot arm. The number of sampling points is , forming a local candidate shortcut between any two sampling nodes, forming a total of local candidate shortcuts; S200: For each local candidate shortcut, Perform equidistant interpolation to obtain interpolation points, interpolation points as input, and use the gap neural network model to perform batch collision detection to identify collision-free local shortcuts; S300: For A collision-free local shortcut is created, each shortcut connects two different robot joint space configuration points, the collision-free local shortcut is regarded as an edge of a graph, each configuration point is regarded as a node of the graph, an initial distance value is assigned to each node, and a priority queue is created for path exploration, the shortest path is found and recorded, backtracked, and the path sequence obtained by backtracking and its total path length are output to obtain a shortest global optimization path found in the collision-free local shortcut; S400: Iterate S100-S300, and the number of initial sampling points will be updated during each iteration , , It is the incremental value of the number of sampling points in the iterative process. When the preset iteration termination condition is met, the iterative process ends, and finally a better global path of the robot arm is obtained.

2. The method according to claim 1, characterized in that S100 includes: S110: After preliminary motion planning, an initial path in the joint space of the robot arm is obtained, which is expressed as , The starting configuration node is , the target configuration node is , the robot arm joint space is represented as ,in represents the degrees of freedom of the robot arm, then the starting configuration node of the robot arm and the target configuration node In the joint space, they are expressed as and , The initial path length is , The calculation method is as follows: ； in Represents the initial path of the robot joint space The number of path nodes included; , express Any two adjacent joint space path nodes in ; S120: Initial joint space motion path of the robot arm Perform equidistant sampling, and set the number of sampling points to , that is, the sampling step length is , configure the path starting point and path target configuration points Including, a total of sampling nodes; S130: construct a local candidate shortcut between any two sampling nodes, and construct a total of H local candidate shortcuts, where .

3. The method according to claim 2, characterized in that S200 includes: S210: For each local candidate shortcut, Perform equidistant interpolation. A total of local candidate shortcuts are obtained interpolation points; S220: Assume that the number of obstacles in the robot arm operation space is The trained gap neural network model is used to interpolation points for batch collision estimation, and obtain The gap prediction value matrix corresponding to the interpolation points , according to the gap prediction value matrix Determine whether the robot arm joint configuration is When the interpolation point is reached, whether the robot arm collides with an obstacle; S230: For any local candidate shortcut, if there is an interpolation point predicted as "collision" in the local candidate shortcut, the local candidate shortcut is discarded, otherwise it is retained, so as to finally identify Collision-free local shortcuts.

4. The method according to claim 3, characterized in that S220 includes: Will The interpolation point data is input into the gap neural network model at one time for batch data processing, and the The gap prediction value matrix corresponding to the interpolation points ,in, for dimensional matrix, k=1,2,…, ; For the matrix For each element in , if its value is less than the safety threshold , then the element is set to 1, otherwise it is set to 0; For any gap prediction value matrix , if the matrix If all the elements are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with an obstacle; After completing the above operations, you can obtain Collision estimation results for interpolation points.

5. The method according to claim 4, characterized in that The gap reasoning network model consists of four parts that are forward connected in sequence. The first part takes the robot joint configuration node as the initial input and uses the kernel function Encode the joint angles and increase the frequency of the input values, where Represents the input robot joint configuration node, is an overclocking parameter. The output of the first part will be used as the input of the second part. The second part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the second part will be used as the input of the third part. The third part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the third part will be used as the input of the fourth part. The fourth part consists of a fully connected layer, and the output feature number is , the purpose is to achieve The mapping of the dimensional gap distance is used to predict the robot arm’s position in the operating space under the current joint configuration. The clearance value of each obstacle.

6. The method according to claim 5, characterized in that S300 includes: S310: Acquired collision-free local shortcuts, each shortcut connects two different robot joint space configuration points, these collision-free local shortcuts are regarded as edges of a graph, and each configuration point is regarded as a node of the graph; S320: Initialization distance: Assign an initial distance value to each node, which is used to record the distance from the robot arm to the starting configuration node. The shortest path distance to the node, set the starting configuration node The distance of is 0, and the initial distance values ​​of all other nodes are set to infinity, indicating that they are not yet reachable; S330: Create a priority queue: Create a priority queue and start the configuration node and its distance as the initial element to be added to the queue. The priority queue will be arranged from small to large according to the node distance, so that the node with the smallest current distance can be processed each time; S340: Path exploration until the target configuration point is found The shortest path or priority queue is empty; S350: Record the shortest path: while updating the distance, record the predecessor node of each node; S360: Backtracking path: When the priority queue is processed or the target configuration node After the shortest path is found, the predecessor node record is used to Going back to , generate a complete shortest path; S370: Output path: Output the path sequence and total path length obtained by backtracking, and obtain the shortest global optimization path found in the collision-free local shortcut .

7. The method according to claim 6, characterized in that S340 includes: S341: Take the node with the smallest distance value from the priority queue and assume it is the current node ; S342: Check from All adjacent nodes of the starting point; S343: For each adjacent node , calculated from arrive Path distance; S344: If the path distance is longer than the currently recorded If the distance is small, update distance and reinsert the node with the updated distance value into the priority queue.

8. The method according to claim 7, characterized in that S400 includes: S410: For the found optimized path , iterate S100-S300, and update the number of sampling points in each iteration ,in , The initial value is , that is, the number of sampling points increases during each iteration ; S420: When the length of the optimized path and the optimization time meet one of the following preset conditions, the iterative process ends and a better global path of the robot arm is finally obtained, otherwise the aforementioned iterative optimization process continues to be executed; the preset conditions include: ,in represents the path length of the robot arm before optimization, represents the path length of the robot arm before optimization, , set by the user; or the cumulative optimization time exceeds the preset time .

Images