Robot online path planning method based on improved fast expansion tree algorithm

Abstract

The invention provides a robot online path planning method based on an improved fast expansion tree algorithm, and the method comprises the steps: obtaining a point cloud in real time through a depth camera, carrying out the cutting filtering and voxel filtering of the point cloud, and obtaining a feasible path through an improved RRT-connect algorithm. On the basis of a traditional RRT-connect algorithm, an artificial potential field strategy is added to avoid collision with an obstacle; an obstacle avoidance optimization strategy is added to guide the extension tree to modify the path of the robot before collision, and the purpose of bypassing the obstacle is achieved; a direct connection strategy is added, so that the time for connecting the extension tree with another tree after walking out of an obstacle area is greatly shortened; and the continuous displacement of the mechanical arm model is expressed as an envelope body, and rapid continuous collision detection is completed by using improved Gilbert-Johnson-Keerthi (GJK). According to the method, the RRT-connect is improved by combining multiple optimization strategies, and the time required by planning is greatly shortened on the basis of ensuring that the path is smooth and free of collision.

Description

Technical Field

[0001] The invention belongs to the field of path planning based on multi-degree-of-freedom industrial robot arms, and in particular relates to a robot online path planning method based on an improved fast expanding tree algorithm. Background Art

[0002] As the most widely used equipment in the field of industrial robots, robotic arms have the ability to replace humans in performing a large number of repetitive and dangerous tasks, thereby improving production efficiency and workplace safety. At present, the main methods used for robotic arm path planning include sampling-based search, artificial potential field method, intelligent algorithm and graph-based search method. Among the sampling-based methods, rapidly exploring random trees (RRT) are widely used. The RRT algorithm effectively finds feasible paths in complex environments by adopting random sampling and tree growth. RRT shows good applicability in dealing with high-dimensional state spaces and complex obstacles, but it also faces problems such as long planning time and the tendency to generate redundant nodes. Therefore, improving and optimizing the existing shortcomings of RRT is of great significance to improving the work efficiency and safety of industrial robots. Summary of the invention

[0003] The purpose of the present invention is to provide a robot online path planning method based on an improved rapidly expanding tree algorithm, using a depth camera to obtain obstacle point clouds in real time, and proposing an RRT-connect algorithm that integrates GJK's continuous collision detection and multiple improvement strategies to achieve real-time path planning. This method is suitable for various industrial scenarios and aims to solve the problems of the large number of planning nodes and inefficient search in complex scenes in the existing RRT. On the basis of the original RRT-connect algorithm, additional optimization strategies such as obstacle avoidance optimization, the addition of artificial potential fields, direct connection and GJK continuous collision detection are used to improve the algorithm search efficiency and reduce the time required for planning. The technical solutions adopted are as follows:

[0004] A robot online path planning method based on an improved fast expanding tree algorithm comprises the following steps:

[0005] Use the depth camera to obtain the initial point cloud, and use the filtered point cloud to represent the obstacle point cloud;

[0006] A path planning algorithm including random sampling, artificial potential field strategy, obstacle avoidance optimization, direct connection strategy and variable step size principle is used to generate a collision-free path for the robot.

[0007] Preferably, random sampling specifically includes the following steps:

[0008] Generate random points in high-dimensional space, find the node closest to the target point by calculating its position in the workspace, and determine the position of the new node.

[0009] The artificial potential field strategy specifically includes the following steps: based on the generated random nodes, exert influence, combine gravity and repulsion, and calculate the final point position.

[0010] Preferably, the obstacle avoidance optimization specifically includes the following steps:

[0011] The obstacle avoidance efficiency of the algorithm is improved by processing the collision nodes, and collision detection is performed when generating new random points to optimize the expansion of the tree.

[0012] Preferably, the direct connection strategy specifically includes the following steps:

[0013] After a new node is generated in the expansion tree, collision detection is performed and the nodes are directly connected if there is no collision.

[0014] Preferably, the collision detection specifically includes the following steps:

[0015] The robot arm's moving body at different times is simplified into an envelope, and a collision detection algorithm is used to perform collision detection between the envelope and obstacles in the environment;

[0016] If the detection result shows that the envelope intersects with the obstacle, it means that there is a collision risk on the moving path of the robot arm.

[0017] Preferably, the step size variation specifically includes the following steps:

[0018] The tree expansion efficiency is improved by adjusting the step size. When a collision occurs, the step size is reduced to avoid inefficient searches in obstacle areas.

[0019] Preferably, in the filtering technique, irrelevant data is removed and relevant features of the obstacle, including position and shape, are highlighted.

[0020] Compared with the prior art, the advantages of the present invention are:

[0021] Through the depth camera and improved RRT, online path planning of the robotic arm is achieved.

[0022] The specific method is to accelerate planning by combining obstacle avoidance optimization, adding artificial potential field, direct connection and GJK collision detection optimization strategies based on the traditional two-way RRT algorithm, and judging whether the current path needs to be replanned according to the position of the obstacle point cloud. A set of configurations in the robot's position space is used as a tree node, and fast collision detection is performed in combination with the solved joint point position and adjacent points.

[0023] Specifically: Based on the traditional RRT-connect algorithm, an artificial potential field strategy is added to avoid collisions with obstacles; an obstacle avoidance optimization strategy is added to guide the expansion tree to modify the robot path before a collision occurs, so as to achieve the purpose of bypassing obstacles; a direct connection strategy is added to greatly reduce the time it takes for the expansion tree to connect with another tree after it walks out of the obstacle area.

[0024] The improved algorithm enables the robot to find a collision-avoiding path within its workspace without sacrificing its performance while maintaining search efficiency and path quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 This is a flow chart of the robot online path planning method based on the improved fast expanding tree algorithm;

[0026] Figure 2 Flowchart for improving the direct connection strategy in the RRT-connect algorithm.

[0027] Figure 3 Flowchart for improving the obstacle avoidance optimization strategy in the RRT-connect algorithm.

[0028] Figure 4 Schematic diagram of planning results for improving the RRT-connect algorithm. DETAILED DESCRIPTION

[0029] The following will be described in more detail with reference to the schematic diagram of the robot online path planning method based on the improved fast expanding tree algorithm of the present invention, wherein the preferred embodiment of the present invention is shown, and it should be understood that those skilled in the art can modify the present invention described herein, while still achieving the advantageous effects of the present invention. Therefore, the following description should be understood as being widely known to those skilled in the art, and not as a limitation of the present invention.

[0030] like Figures 1 to 3 , a robot online path planning method based on an improved fast expanding tree algorithm, comprising the following steps:

[0031] Step 1: Determine the initial position X of the robot arm start and X goal , obstacle information and two random trees start and Tree goal .

[0032] Where X = {θ1,θ2,...,θ n}, n represents the degree of freedom.

[0033] Step 2: Set the initial search step size k p , range threshold d represents the final connection range, attraction coefficient katt and the rejection coefficient k rep , maximum number of iterations n max , random search probability r1, r2(0 <r 1< r2<1), obstacle avoidance optimization distance d r .

[0034] Step 3: Randomly generate a number from 0 to 1 (search probability) and determine the interval in which it is located;

[0035] If the search probability is between 0 and r1, random sampling is used to determine the new node X new ;

[0036] If the search probability is between r1 and r2, a force F pointing to the target point is added based on random sampling. att and the force F away from the obstacle rep , add them together to get the new node X new ,like Figure 2 In the random probability potential field sampling in Figure (a), execute step 4;

[0037] If the search probability is between r2 and 1, then the vector pointing to the end point of another tree and the current step length k p Directly determine the new node X new ,like Figure 2 In the target sampling in (b), execute step 5;

[0038] Step 4: Generate a new node X by randomly rand , and determine its neighboring node X near , further calculate the robot in X near The point is subjected to a force F directed toward the target point att and the force F away from the obstacle rep The resultant force F of the new node X is calculated by the following formula new ;

[0039]

[0040] Among them, w1, w2 are weight coefficients;

[0041] F is the robot in X near The net force at k is F = k att F att +k rep F rep ;

[0042] k p The step size for the robot to search;

[0043] In order to ensure that the robot can search for the target point and improve the completeness of the search probability, w1 should be slightly larger than w2.

[0044] Step 5: Calculate Node X near1 and X new2 The vector direction between And according to the search step length k p Determine the new node X new :

[0045]

[0046] Among them, in step 5:

[0047] X new2 -Current random tree goal The terminal node in

[0048] X near1 -Current random tree start Neighborhood nodes of

[0049] X new -Random Tree start The new node of

[0050] Step 6: Use the GJK algorithm to near and X new Perform collision detection between them.

[0051] If there is no collision, go to step 3 and set X new Add to Tree start If there is a collision, go to step 7.

[0052] Similarly, random tree Tree goal The steps for generating new nodes are the same as steps 5 to 6.

[0053] Step 7: Figure 3 As shown, X new It is the node that encounters obstacles during expansion. The dotted line represents the location where the obstacle avoidance optimization node is generated, and its radius is the obstacle avoidance optimization distance d. r .

[0054] Randomly generate sampling points X in space obj1 Get the vector as the end point Will take Connect the point X rn1 And perform continuous collision detection;

[0055] If there is a collision, pick a random point X again obj1 And perform continuous collision detection again; if there is no collision, such as X rn1 , then the generated Xrn1 Added as a new node to the expansion tree.

[0056]

[0057] Step 8: Change the step size k according to whether the current sample has passed step 6 (whether there has been a collision) p .

[0058] If it passes, reset the current step size to the initial step size k p0 ; If not, then the current step length k p Increase to k p *k.

[0059] Step 9: According to the range threshold d, determine the search tree 1 (random tree Tree start )’s new terminal node X new1 With search tree 2 (random tree Tree goal )’s new terminal node X new2 Will there be a collision between them?

[0060] If a collision occurs, repeat steps 3-8; if no collision occurs, start from the two new nodes and obtain the node set from the initial point to the target point according to the index of the number of nodes as the initial path. Repeat the above steps 1-9 until the target point is found, or the number of iterations exceeds n max .

[0061] Step 10: For each interpolation interval, a piecewise cubic polynomial is calculated and the continuity conditions of the first and second order derivatives are imposed to ensure the smoothness of the curve at the connection points.

[0062] Step 11: Output each path point of the smoothed path from the initial position as a collision-free path.

[0063] Step 12: Update the point cloud and determine whether the planned path conflicts with the current obstacle point cloud.

[0064] If a conflict occurs, steps 3-11 are used to plan the local path where the conflict occurs, without changing other non-conflicting paths, so as to achieve real-time planning.

[0065] like Figure 4 As shown in the figure, a three-dimensional simulation was carried out to verify the effectiveness of this method.

[0066] The above is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any technician in the relevant technical field, without departing from the scope of the technical solution of the present invention, makes any form of equivalent replacement or modification to the technical solution and technical content disclosed in the present invention, which does not depart from the content of the technical solution of the present invention and still falls within the protection scope of the present invention.

Claims

1. A robot online path planning method based on an improved fast expanding tree algorithm, characterized in that: The following steps are involved: Use the depth camera to obtain the initial point cloud, and use the filtered point cloud to represent the obstacle point cloud; A path planning algorithm including random sampling, artificial potential field strategy, obstacle avoidance optimization, direct connection strategy and variable step size principle is used to generate a collision-free path for the robot.

2. The robot online path planning method based on the improved rapid expansion tree algorithm according to claim 1 is characterized in that: Random sampling specifically includes the following steps: Generate random points in high-dimensional space, find the node closest to the target point by calculating its position in the workspace, and determine the position of the new node. The artificial potential field strategy specifically includes the following steps: based on the generated random nodes, exert influence, combine gravity and repulsion, and calculate the final point position.

3. The robot online path planning method based on the improved rapid expansion tree algorithm according to claim 1 is characterized in that: Obstacle avoidance optimization specifically includes the following steps: The obstacle avoidance efficiency of the algorithm is improved by processing the collision nodes, and collision detection is performed when generating new random points to optimize the expansion of the tree.

4. The robot online path planning method based on the improved rapid expansion tree algorithm according to claim 1 is characterized in that: The direct connection strategy specifically includes the following steps: After a new node is generated in the expansion tree, collision detection is performed and the nodes are directly connected if there is no collision.

5. The robot online path planning method based on the improved rapid expansion tree algorithm according to claim 1 is characterized in that: Collision detection specifically includes the following steps: The robot arm's moving body at different times is simplified into an envelope, and a collision detection algorithm is used to perform collision detection between the envelope and obstacles in the environment; If the detection result shows that the envelope intersects with the obstacle, it means that there is a collision risk on the moving path of the robot arm.

6. The robot online path planning method based on the improved rapid expansion tree algorithm according to claim 1 is characterized in that: The variable step size specifically includes the following steps: The tree expansion efficiency is improved by adjusting the step size. When a collision occurs, the step size is reduced to avoid inefficient searches in obstacle areas.

7. The robot online path planning method based on the improved rapid expansion tree algorithm according to claim 1 is characterized in that: In filtering technology, irrelevant data is removed and relevant features of obstacles, including location and shape, are highlighted.

Images