Mobile robot path planning method in complex environment

Abstract

The invention discloses a mobile robot path planning method in a complex environment, and relates to the field of robot navigation, the method comprises the following steps: firstly, dividing the whole path planning process into different stages, and adopting different heuristic functions in different stages; a potential field is introduced to guide the traversal direction of the algorithm, so that the algorithm does not traverse a large number of useless nodes; then, a random escape strategy is utilized to solve the problem that the algorithm falls into a local minimum value, so that the problems that the algorithm traverses many nodes and is low in speed in a complex map environment are solved; finally, a plurality of methods are adopted to optimize the planned path, so that the path is smoother and more accords with practical application; meanwhile, simulation and experimental verification are carried out on the method provided by the invention, the result is surface, the efficiency and success rate of path planning are improved, and the path planning requirement in a complex map environment is met.

Description

Technical Field

[0001] The invention belongs to the technical field of mobile robot navigation, and in particular relates to a method for path planning in a complex environment. Background Art

[0002] Path planning for mobile robots in complex environments is a challenge. Although traditional methods such as the A* algorithm can find paths, they may perform poorly in terms of search efficiency, convergence speed, and path optimization. Especially in complex map environments, traditional algorithms often traverse a large number of useless nodes, resulting in path redundancy and waste of computing resources. In addition, there is a situation where the algorithm falls into a local minimum, which makes it easy for the algorithm to fail to find the optimal path or perform poorly in practical applications. With the improvement of computing power and the development of algorithm optimization technology, researchers have begun to explore how to combine multiple strategies to improve the performance of path planning. Summary of the invention

[0003] The present invention aims to provide an improved mobile robot path planning method to solve the problems of low efficiency and insufficient path optimization of traditional path planning algorithms in complex environments. The present invention significantly improves the speed, accuracy and path quality of path planning by combining the phased application of heuristic functions, potential field guidance and random escape strategy.

[0004] The specific technical solution includes the following steps:

[0005] Phased heuristic function:

[0006] In the early stage of path planning, Manhattan distance is used as a heuristic function to quickly expand the search range and reduce the risk of falling into a local minimum in the early stage.

[0007] In the middle and late stages of path planning, Euclidean distance is used as a heuristic function to provide more accurate path evaluation and help the algorithm find a better path.

[0008] Potential field guidance:

[0009] An artificial potential field is introduced to guide the traversal direction of the algorithm. The potential field consists of an attractive potential field (to guide the robot to move toward the target) and a repulsive potential field (to avoid obstacles). The formula is as follows:

[0010] Attractive potential field:

[0011]

[0012] Where λ is the gravitational scale factor and ρ(q,qgoal) is the relative distance between the robot and the target point.

[0013] Repulsive potential field:

[0014]

[0015] Where μ is the repulsive force scale factor; ρ0 is the distance of influence of the obstacle repulsive field on the robot. When d<ρ0, it means that the robot is subject to repulsion within the influence distance. When d>ρ0, the repulsive force is 0. d=ρ(q,q obs ) is the relative distance between the obstacle and the robot.

[0016] Random escape strategy:

[0017] When the algorithm does not make progress within a certain number of times, it jumps out of the local minimum by randomly selecting neighboring nodes to ensure that path planning can continue.

[0018] Path optimization processing:

[0019] Path pruning: By detecting whether there is a straight line of sight between two points, the path is simplified and unnecessary nodes are removed. It can be expressed as:

[0020]

[0021] The intersection detection between a line segment and a rectangle is:

[0022] intersect=(A,B,C,D)=ccw(A,C,D)≠ccw(B,C,D)andccw(A,B,C)≠ccw(A,B,D)

[0023] Among them, p1 and p2 represent two independent points, obstacles represent obstacles, and ccw(A,B,C) is a function that determines whether the three points are arranged counterclockwise.

[0024] Obstacle Clearance Distance: Add an extra safety distance to prevent the path from being too close to obstacles. The potential field expression formula after adding the clearance distance is:

[0025] Attractive potential field:

[0026]

[0027] Repulsive potential field:

[0028]

[0029] Total potential field:

[0030] U(q)=U att (q)+U rep (q)

[0031] Among them, q is the current position, q goal is the target position K attr is the attractive potential field coefficient. obs is the obstacle position, krep is the repulsive potential field coefficient, d 0 is the impact range, and clearance is the clearing distance.

[0032] Path smoothing: Use B-spline interpolation to generate a smooth path. B-spline interpolation can be expressed as:

[0033]

[0034] The B-spline cardinality is:

[0035]

[0036] Among them, S(u) is the B-spline curve, Pi is the control point, Ni,k(u) is the B-spline basis function, and u is the parameter.

[0037] In some embodiments, the path planning method further comprises:

[0038] Path generation process:

[0039] The strategy of multi-stage heuristic function is adopted, so that the algorithm can adjust the search strategy according to different needs at different stages.

[0040] After introducing the potential field, the algorithm can reduce the traversal of invalid nodes according to the guidance of the potential field.

[0041] Adjustments in actual application:

[0042] According to the changes in the environment, the potential field coefficients are dynamically adjusted to adapt to different complexities and obstacle distributions.

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

[0044] Improved efficiency: Through phased heuristic functions and potential field guidance, the number of traversed nodes and search time are significantly reduced.

[0045] Path quality enhancement: Path optimization processing ensures the practicality and smoothness of the path, reducing risks in actual operation.

[0046] Avoid local optimality: The random escape strategy ensures that the algorithm can escape from the local optimal solution and find a more global optimal solution. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The present invention will be further described below in conjunction with the accompanying drawings.

[0048] Figure 1 It is a flow chart of the path planning strategy of the method of the present invention;

[0049] Figure 2is a comparison diagram of the path planning experimented by the path planning method of the present invention in the first obstacle environment;

[0050] Figure 3 is a comparison diagram of the path planning experimented by the path planning method of the present invention in the second obstacle environment;

[0051] Figure 4 is a comparison diagram of the path planning experimented by the path planning method of the present invention in the third obstacle environment;

[0052] Figure 5 It is a comparison diagram of the original path before path optimization and the optimized path under three obstacle environments of the present invention. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0054] The embodiments of the present invention are further described in detail below in conjunction with the accompanying drawings. The present invention provides a method for mobile robot path planning in a complex environment. Figure 1 For the overall flow chart of the path planning of the present invention, please refer to Figure 1 , the method comprises the following steps:

[0055] Step 1: Divide the path planning process into different stages, where the early stage uses Manhattan distance as the heuristic function and the later stage uses Euclidean distance as the heuristic function.

[0056] Step 2: Use artificial potential field to guide the traversal direction of the algorithm, introduce attractive potential field and repulsive potential field to avoid exploring invalid nodes, such as Figure 2 shown.

[0057] Step 3: During the algorithm search process, if it is found to be trapped in a local minimum, a new neighbor node is selected through a random escape strategy to continue the search.

[0058] Step 4: After the path is generated, the path is optimized, including path pruning, increasing obstacle removal distance and smoothing. Figure 5 shown.

[0059] The detailed process is as follows:

[0060] Create a starting node and set initial parameters, including Openlist (priority queue) and Closelist (visited node set).

[0061] If Openlist is empty, the algorithm ends; otherwise, the node with the smallest cost is popped out from Openlist as current_node.

[0062] Check if current_node is already in Closelist, if so, skip it; otherwise, add it to Closelist and update stuck_counter (the same number as the previous node) based on the same situation between the current node and the previous node.

[0063] If stuck_counter exceeds the threshold, the random escape strategy is called to randomly select a node from the neighboring nodes to add to the Openlist.

[0064] Check whether current_node is the target node. If so, output the path and end the algorithm; otherwise, continue exploring neighbor nodes.

[0065] Call the get_neighbors function to obtain neighbor nodes, calculate the cost and heuristic function value of each neighbor node, and add the neighbor nodes that meet the conditions to the Openlist.

[0066] Record current_node as the previous node and continue the next loop.

[0067] The target point is found, the final path is generated, and the path planning is completed.

[0068] In some embodiments, in step 1:

[0069] The calculation of Manhattan distance is used in the early stage, and the formula is:

[0070] h(n)=D*(|nx-goal.x|+|ny-goal.|)

[0071] Where D represents the distance between two points, nx and ny represent the x and y coordinates of the current node, and goal.x and goal.y represent the x and y coordinates of the target point.

[0072] The calculation of Euclidean distance is used in the later stage and the formula is:

[0073] h(n)=D*(|nx-goal.x|+|ny-goal.|)

[0074] Where D represents the distance between two points, nx and ny represent the x and y coordinates of the current node, and goal.x and goal.y represent the x and y coordinates of the target point.

[0075] In some embodiments, in step 2:

[0076] Attractive potential field:

[0077]

[0078] Where λ is the gravitational scale factor and ρ(q,qgoal) is the relative distance between the robot and the target point.

[0079] Repulsive potential field:

[0080]

[0081] Where μ is the repulsive force scale factor; ρ0 is the distance of influence of the obstacle repulsive force field on the robot. When d<ρ0, it means that the robot is subject to repulsion within the influence distance. When d>ρ0, the repulsive force is 0. d=ρ(q,q obs ) is the relative distance between the obstacle and the robot.

[0082] In some embodiments, in step 3:

[0083] Random escape strategies include:

[0084] When the algorithm does not make progress within a certain number of times (such as 5 times), a node is randomly selected from the neighboring nodes of the current node to continue searching.

[0085] In some embodiments, in step 4:

[0086] Path pruning: By detecting whether there is a straight line of sight between two points, the path is simplified and unnecessary nodes are removed. It can be expressed as:

[0087]

[0088] The intersection detection between a line segment and a rectangle is:

[0089] intersect=(A,B,C,D)=ccw(A,C,D)≠ccw(B,C,D)andccw(A,B,C)≠ccw(A,B,D)

[0090] Among them, p1 and p2 represent two independent points, obstacles represent obstacles, and ccw(A,B,C) is a function that determines whether the three points are arranged counterclockwise.

[0091] Obstacle Clearance Distance: Add an extra safety distance to prevent the path from being too close to obstacles. The potential field expression formula after adding the clearance distance is:

[0092] Attractive potential field:

[0093]

[0094] Repulsive potential field:

[0095]

[0096] Total potential field:

[0097] U(q)=U att (q)+U rep (q)

[0098] Among them, q is the current position, q goal is the target position K attr is the attractive potential field coefficient. obs is the obstacle position, k rep is the repulsive potential field coefficient, d 0 is the impact range, and clearance is the clearing distance.

[0099] Path smoothing: Use B-spline interpolation to generate a smooth path. B-spline interpolation can be expressed as:

[0100]

[0101] The B-spline cardinality is:

[0102]

[0103] Among them, S(u) is the B-spline curve, Pi is the control point, Ni,k(u) is the B-spline basis function, and u is the parameter.

[0104] In order to verify the effectiveness of the present invention, a simulation environment was used. The experimental platform was based on ROS (Robot Operating System) and Gazebo was used for simulation. The environment was set to a 50m×50m grid map, containing different obstacle layouts:

[0105] Early stage: Manhattan distance is used.

[0106] Later stage: Euclidean distance is used to make the path more accurate. Figure 2 It can be seen that in the first obstacle environment, the number of explored nodes is significantly reduced compared with traditional A*, D*, and weighted A* methods.

[0107] In the experiment, a single Manhattan distance and Euclidean distance were used as heuristic functions to compare with the heuristic functions used in the improved algorithm. The experiment found that the final path results of running in different obstacle environments are almost the same, and only the exploration time and the number of times they fall into the local minimum problem are different. Table 1 shows the running results of different heuristic functions in different obstacle environments:

[0108] Table 1. Algorithm running time and number of times falling into local minima under different heuristic functions

[0109]

[0110] Potential field guidance: through Figure 3 It can be seen that path planning can effectively avoid obstacles in the second obstacle environment.

[0111] Random escape strategy: In certain obstacle layouts, it prevents the algorithm from falling into local minima and ensures the success of path planning.

[0112] Table 2 and Figure 4 The comparison of the path planning effects of the method of the present invention and other methods (such as heuristic A* and D* algorithms) in the third map environment is demonstrated, and it is obvious that the method of the present invention has advantages in reducing the number of traversed nodes, increasing planning speed and success rate, and improving path quality.

[0113] Table 2 Average experimental data of five algorithms

[0114]

[0115] Figure 5 The comparison between the original path before path optimization and the optimized path is shown in detail, verifying the effectiveness of the present invention in path pruning, obstacle removal distance and smoothing.

[0116] In summary, the present invention provides an efficient and accurate mobile robot path planning method in complex environments by combining phased heuristic functions, potential field guidance and random escape strategies. This method not only reduces the search time and the number of nodes, but also improves the practicality and safety of the path.

[0117] It should be noted that, in this application, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0118] The above description is only a specific implementation of the present application, so that those skilled in the art can understand or implement the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest range consistent with the principles and novel features applied for herein.

Claims

1. A method for mobile robot path planning in a complex environment, characterized in that: The steps include: Step 1: Divide the path planning process into different stages, where Manhattan distance is used as the heuristic function in the early stage and Euclidean distance is used as the heuristic function in the later stage; Step 2: Use artificial potential fields to guide the traversal direction of the algorithm, introduce attractive potential fields and repulsive potential fields to avoid exploring invalid nodes; Step 3: During the algorithm search process, if it is found to be trapped in a local minimum, a new neighbor node is selected through a random escape strategy to continue searching; Step 4: After the path is generated, the path is optimized, including path pruning, increasing obstacle removal distance, and smoothing.

2. The mobile robot path planning method according to claim 1, characterized in that: The early stage refers to the search process from the starting point to a certain intermediate distance. The Manhattan distance is calculated as follows: D=(|nx-goal.x|+|ny-goal.y|) Where D represents the distance between two points, nx and ny represent the x and y coordinates of the current node, and goal.x and goal.y represent the x and y coordinates of the target point.

3. The mobile robot path planning method according to claim 1 or 2, characterized in that: The latter stage refers to the search process from the intermediate distance to the target point. The Euclidean distance is calculated as: Where D represents the distance between two points, nx and ny represent the x and y coordinates of the current node, and goal.x and goal.y represent the x and y coordinates of the target point.

4. The mobile robot path planning method according to any one of claims 1 to 3, characterized in that: The attractive potential field and the repulsive potential field are calculated by the following formula: U(q)=U att (q)+U rep (q) Among them U att (q) is the gravitational potential field generated by the target, U rep (q) is the sum of the repulsive potential fields generated by each obstacle. The commonly used gravitational field functions are as follows: where λ is the gravitational scale factor, ρ(q,q goal ) is the relative distance between the robot and the target point. According to the negative gradient of the gravitational field, the gravitational force F att for: The repulsion function generates a certain repulsive field around the robot through obstacles, which produces repulsive force on the robot. The repulsive force is inversely proportional to the distance from the robot to the obstacle. The repulsive potential field function is expressed as: Where μ is the repulsive force scale factor; ρ0 is the distance of influence of the obstacle repulsive field on the robot. When d<ρ0, it means that the robot is subject to repulsion within the influence distance. When d>ρ0, the repulsive force is 0. d=ρ(q,q obs ) is the relative distance between the obstacle and the robot.

5. The mobile robot path planning method according to any one of claims 1 to 4, characterized in that: Random escape strategies include: When the algorithm does not make progress within a certain number of times (for example, 5 times), a node is randomly selected from the neighboring nodes of the current node to continue searching.

6. The mobile robot path planning method according to any one of claims 1 to 5, characterized in that: The path optimization process steps include: Path pruning: Simplify the path by detecting whether there is a straight line of sight between two points. Monitoring whether there is a straight line of sight between two points can be expressed as: The intersection detection between a line segment and a rectangle is: intersect=(A,B,C,D)=ccw(A,C,D)≠ccw(B,C,D)andccw(A,B,C)≠ccw(A,B,D) Among them, p1 and p2 represent two independent points, obstacles represent obstacles, and ccw(A,B,C) is a function to determine whether the three points are arranged counterclockwise; Obstacle Clearance Distance: Add an additional safety distance when generating a path to prevent the path from being too close to an obstacle. The potential field expression formula after adding the clearance distance is: Attractive potential field: Repulsive potential field: Total potential field: U(q)=U att (q)+U rep (q) Among them, q is the current position, q goal is the target position K attr is the attractive potential field coefficient, q obs is the obstacle position, k rep is the repulsive potential field coefficient, d0 is the influence range, and clearance is the clearance distance; Path smoothing: Use B-spline interpolation to generate a smooth path. B-spline interpolation can be expressed as: The B-spline cardinality is: Among them, S(u) is the B-spline curve, Pi is the control point, Ni,k(u) is the B-spline basis function, and u is the parameter.

Images