CN115755908B - A mobile robot path planning method based on JPS guided Hybrid A* - Google Patents

Abstract

The invention discloses a mobile robot path planning method based on JPS guided HybridA*. On the basis of the classic HybridA* algorithm, the extended characteristics of the JPS algorithm are used for optimization. When considering the h _{collision_avoidance1} heuristic value under the two-dimensional grid, the JPS algorithm is used Replace the A* algorithm for length calculation; add the p _bias penalty term in the calculation of the g _extend function value, and use the JPS algorithm to traverse to obtain the guide path point set. Compared with the classic Hybrid A* algorithm, the method of the present invention has a significant improvement in traversal speed, number of iterations and path length.

Description

A mobile robot path planning method based on JPS guided Hybrid A*

Technical field

The invention relates to the field of mobile robot path planning, specifically a mobile robot path planning method based on JPS guided Hybrid A*.

Background technique

The study of path planning is an important part of the field of mobile robot research. Its purpose is to enable the mobile robot to find a collision-free path from the starting state to the target state in a known or unknown environment. Most traditional path planning algorithms only consider the pose space of the robot. However, in fact, the robot is not only constrained by the pose space, but also constrained by various external forces. As the navigation technology of mobile robots and even an important core technology of the entire mobile robot technology, path planning technology has attracted more attention from researchers.

Most of the traditional mobile robot path planning algorithms rasterize the map and then use graph search algorithms such as the A* algorithm or Dijkstra algorithm for path planning. These algorithms do not consider the kinematic constraints of the robot when generating paths, so the planned Most paths are discrete and do not satisfy the non-integrity constraints of the robot; as an extension of the A* algorithm, the Hybrid A* algorithm adds robot kinematic constraints to the algorithm and changes the discrete expansion points to continuous expansion points, so that This allows the robot to obtain a continuous executable path.

However, when the Hybrid A* algorithm expands in a spacious free space area and the end point is located in a narrow area, a large number of candidate nodes will be added to the list, and only a small number of nodes can guide the path into the narrow area, resulting in an increase in the number of iterations, or even Unable to find feasible path.

Contents of the invention

Purpose of the invention: In view of the above existing technology, a mobile robot path planning method based on JPS guided Hybrid A* is proposed to increase the node traversal speed, reduce the number of iterations and increase the output path length under large-scale maps.

Technical solution: A mobile robot path planning method based on JPS guided Hybrid A*, including the following steps:

Step 1: Initialize the global map; the method is: model the environment and establish a two-dimensional coordinate system, discretize all obstacles in the environment and represent them with a uniformly distributed point set, and set the number of obstacles to k to form obstacles. The object set is {OBS _i }, that is, OBS ₁ , i＝1,…,k;

Step 2: Parameter initialization; the method is: set the initial pose Start (x _init , y _init , θ _init ), the end pose Goal (x _end , y _end , θ _end ) in the map, and construct the node sampling function Expansion_pattern, The list to be explored Openlist, the explored list Closedlist, and the mobile robot kinematics model; among them, (x _* , y _* ) is the coordinate of the center of mass of the mobile robot kinematics model in the absolute coordinate system under * conditions, and θ _* represents * The heading angle of the mobile robot under the conditions;

Step 3: Select the node with the smallest value from the list to be explored Openlist as the current node, find the next expansion node through forward simulation, and determine whether the distance between the expansion node and the end point is less than the threshold. If so, continue to step 6, otherwise continue Step 4;

Step 4: Calculate the cost value g _extend from the starting point to the extended node and the heuristic value h _extend from the extended node pose to the end pose while considering the kinematic constraints and obstacle avoidance conditions of the mobile robot, and calculate the total generation of the extended node value f _extend ;

Step 5: Put the extended node and its total cost value f _extend into the list to be explored Openlist, and return to step 3 to continue;

Step 6: Use the Reeds-Sheep curve to connect the extended node to the end point, and determine whether there are obstacles on the curve. If so, return to step 3 to continue; otherwise, return to the path point set;

Step 7: Resample the path point set and output the optimized path.

Further, in step 2, the specific process of constructing the node sampling function Expansion_pattern is:

The algorithm first performs uniform sampling within the range of φ∈{-φ _max ,0,φ _max }, v∈{-v _max ,v _max } to obtain the sampled value, and then in unit time according to the sample value [v,φ] Simulate the moving process of the mobile robot to determine whether the sampled value meets the collision condition; where v is the forward speed, φ is the deflection angle of the front wheel of the mobile robot, φ _max is the maximum deflection angle of the front wheel of the mobile robot, and v _max is the maximum deflection angle of the front wheel of the mobile robot. travel speed.

Further, in step 3, the specific process of forward simulation to find expansion nodes is:

Expand the current node through the forward simulation calculation formula and connect the nodes with the Reeds-Sheep curve; the current node pose is N _current (x _current ,y _current ,θ _current ), and the extended node pose is N _expend (x _expend ,y _expend ,θ _expend ), the forward simulation calculation formula is as follows:

x _expend =x _current +cosθ _current vdt

y _expend =y _current +cosθ _current vdt

In the formula, L _w is the distance between the front and rear axes of the mobile robot kinematic model; N is a constant.

Further, in step 4, the specific process of calculating the total generation value f _extend of the extended node is:

f _extend =g _extend +h _extend

in:

g _extend ＝g _current +C _simu +p _bias +p _{v_change} +p _{v_reverse} +p _{phy_change}

In the formula, g _current is the cost value of the starting point extending to the current node, C _simu is the forward simulation cost value superimposed each time the child node is expanded, p _bias is the penalty for the expanded node to deviate from the guidance path, and p _{v_change} is the mobile robot The penalty introduced when the forward speed v changes, p _{v_reverse} is the penalty introduced when the forward speed v direction of the mobile robot changes, p _{phy_change} is the penalty introduced when the front wheel deflection angle φ of the mobile robot is changed;

h _extend =β*max(h _{nonholonomics} ,max(h _{collision_avoidance1} ,h _{collision_avoidance2} ))

In the formula, β is the weight coefficient of the heuristic value, h _{nonholonomics} is the heuristic value from the extended node pose to the end pose using the Reeds-Sheep curve without considering obstacle avoidance conditions, h _{collision_avoidance1} is the JPS algorithm in the two-dimensional network The heuristic value from the extended node to the end point under the grid, h _{collision_avoidance2} is the Manhattan distance from the extended node to the end point.

Furthermore, the collision detection method is used in the forward simulation in step 3 and in the judgment of whether there are obstacles on the curve in step 6. In the collision detection method, each path point is used as the axis center of the rear axis of the mobile robot. Calculate the coordinates of the four vertices of the mobile robot to construct a simple kinematics model for boundary collision detection.

Beneficial effects: (1) The mobile robot path planning method based on JPS guided Hybrid A* of the present invention, as an improved algorithm of Hybrid A*, combines the certain advantages of the JPS algorithm itself in node traversal speed and quantity, so that The path generated by the JPS algorithm will fit more closely with obstacles when turning corners than the path generated by the A* algorithm, and the output path will be more streamlined but will not cause collisions. Therefore, the improved algorithm improves the node traversal speed, number of iterations, and output paths in large-scale maps. The improvement effect of indicators such as length is significant.

(2) When the Hybrid A* algorithm expands into a spacious free space area and the end point is located in a narrow area, due to its traversal characteristics, the number of iterations will increase and even a feasible path cannot be found. Because of the existence of forced settlement, the JPS algorithm is more sensitive to narrow areas with many obstacles. Therefore, the number of iterations required to reach the end point is greatly reduced compared to the A* algorithm. The present invention uses the JPS algorithm instead of the A* algorithm to calculate avoidance. The heuristic value at the time of failure and the use of JPS algorithm to obtain the guidance path point set effectively avoid the expansion of irrelevant nodes and quickly generate effective paths.

Description of the drawings

Figure 1 is a flow chart of this method;

Figure 2 is a schematic diagram of the JPS algorithm principle;

Figure 3 is a schematic diagram of the kinematics model of the mobile robot;

Figure 4 is the initialization and parameter setting map;

Figures 5 and 6 are schematic diagrams of algorithm simulation results of the present invention.

Detailed ways

The present invention will be further explained below in conjunction with the accompanying drawings.

Referring to Figure 1, a mobile robot path planning method based on JPS guided Hybrid A* in this embodiment mainly includes the following steps:

Step 1: Global map initialization. The method is: model the environment and establish a two-dimensional coordinate system, discretize all obstacles in the environment and represent them with a uniformly distributed point set. Assume that the number of obstacles is k, and the obstacle set is {OBS _i } , that is, OBS ₁ , i=1,…,k.

Step 2: Parameter initialization. The method is: set the initial pose Start (x _init , y _init , θ _init ), the end pose Goal (x _end , y _end , θ _end ) in the map, build the node sampling function Expansion_pattern, and open the list to be explored. Explore the Closedlist list, and the mobile robot kinematic model; where (x _* , y _* ) is the coordinate of the center of mass of the mobile robot kinematic model in the absolute coordinate system under * conditions, and θ _* represents the heading angle of the mobile robot under * conditions , where the * condition is the initial pose init and the end pose end.

In this step, the specific process of constructing the node sampling function Expansion_pattern is as follows: the algorithm first conducts uniform sampling within the range of φ∈{-φ _max ,0,φ _max }, v∈{-v _max ,v _max } to obtain the sampling value, Then the forward process of the mobile robot is simulated forward according to the sampled value [v,φ] in unit time to determine whether the sampled value meets the collision condition; where v is the forward speed, φ is the deflection angle of the front wheel of the mobile robot, and φ _max is The maximum deflection angle of the front wheel of the mobile robot, v _max is the maximum forward speed.

Step 3: Select the node with the smallest cost value f _extend from the list to be explored Openlist as the current node, find the next extension node through forward simulation, and determine whether the distance between the extension node and the end point is less than the threshold. If so, continue to step 6. Otherwise continue to step 4.

In this step, the specific process of finding expanded nodes through forward simulation is: expand the current node through the forward simulation calculation formula, and connect the nodes with the Reeds-Sheep curve. The current node pose is N _current (x _current , y _current , θ _current ), and the extended node pose is N _expend (x _expend , y _expend , θ _expend ). The forward simulation calculation formula is as follows:

x _expend =x _current +cosθ _current vdt

y _expend =y _current +cosθ _current vdt

In the formula, L _w is the distance between the front and rear axes of the mobile robot kinematic model, N is a constant, and the unit time is divided equally.

Step 4: Calculate the cost value g _extend from the starting point to the extended node and the heuristic value h _extend from the extended node pose to the end pose while considering the kinematic constraints and obstacle avoidance conditions of the mobile robot, and calculate the total generation of the extended node value f _extend .

In this step, the specific process of calculating the total generation value f _extend of the extended node is:

f _extend =g _extend +h _extend

in:

g _extend ＝g _current +C _simu +p _bias +p _{v_change} +p _{v_reverse} +p _{phy_change}

In the formula, g _current is the cost value of the starting point extending to the current node, C _simu is the forward simulation cost value superimposed each time the child node is expanded, p _bias is the penalty for the expanded node to deviate from the guidance path, and p _{v_change} is the mobile robot The penalty introduced when the forward speed v changes, p _{v_reverse} is the penalty introduced when the forward speed v direction of the mobile robot changes, and p _{phy_change} is the penalty introduced when the front wheel deflection angle φ of the mobile robot is changed.

h _extend =β*max(h _{nonholonomics} ,max(h _{collision_avoidance1} ,h _{collision_avoidance2} ))

In the formula, β is the weight coefficient of the heuristic value, h _{nonholonomics} is the heuristic value from the extended node pose to the end pose using the Reeds-Sheep curve without considering obstacle avoidance conditions, h _{collision_avoidance1} is the JPS algorithm in the two-dimensional network The heuristic value from the extended node to the end point under the grid, h _{collision_avoidance2} is the Manhattan distance from the extended node to the end point. Taking the maximum value means that the larger of the two heuristic values will be used to reflect the difficulty of traveling from the expansion node to the end point.

Step 5: Put the extension node and its cost value f _extend into the Openlist list to be explored, and return to step 3 to continue.

Step 6: Use the Reeds-Sheep curve to connect the extended node and the end point, and determine whether there are obstacles on the curve. If so, return to step 3 to continue; otherwise, return to the path point set.

Step 7: Resample the path point set and output the optimized path.

In this method, the collision detection method is used in the forward simulation in step 3 and in determining whether there are obstacles on the curve in step 6. Considering that the algorithm itself is based on the kinematic model of the mobile robot for path planning, each path point is regarded as For the rear axis of the mobile robot, a simple kinematics model is constructed by calculating the coordinates of the four vertices of the mobile robot to perform boundary collision detection.

Figure 2 is a simple schematic diagram of the JPS algorithm. Figure 3 is the kinematic model of the mobile robot established in the absolute coordinate system. It is replaced by a simple rectangle in the algorithm. Only the center of mass of the mobile robot is used to replace the model during runtime. Figure 4 is the algorithm. The initial environment before planning, as well as the starting and ending positions, Figure 5 and Figure 6 are the simulation results of the algorithm of the present invention in two different maps respectively.

The above are only preferred embodiments of the present invention. It should be noted that those skilled in the art can make several improvements and modifications without departing from the principles of the present invention. These improvements and modifications can also be made. should be regarded as the protection scope of the present invention.

Claims (2)

1. The path planning method of the mobile robot based on JPS guiding hybrid A is characterized by comprising the following steps:

step 1: initializing a global map; the method comprises the following steps: performing environment modeling and establishing a two-dimensional coordinate system, discretizing all barriers in the environment and representing by uniformly distributed point sets, wherein the number of barriers is k, and the composition barrier set is { OBS } _i "i.e.)i＝1,…,k；

Step 2: initializing parameters; the method comprises the following steps: setting initial pose Start (x) in map _init ,y _init ,θ _init ) Final point pose gold (x _end ,y _end ,θ _end ) Constructing a node sampling function Expansion_pattern, a list to be explored Openlist, an explored list Closedlist and a mobile robot kinematic model; wherein, (x) _* ,y _* ) For the coordinates of the centroid of the kinematic model of the mobile robot in an absolute coordinate system under the condition of θ _* Expressed under the following conditionsHeading angle of lower mobile robot;

step 3: selecting a node with the minimum cost value from the Openlist to be explored as a current node, searching for the next expansion node through forward simulation, judging whether the distance between the expansion node and the terminal point is smaller than a threshold value, if so, continuing the step 6, otherwise, continuing the step 4;

step 4: calculating cost value g from starting point to expansion node _extend Expanding the node pose to the end pose and simultaneously considering the heuristic value h of the kinematic constraint and obstacle avoidance condition of the mobile robot _extend And calculates the total cost value f of the expansion node _extend ；

Step 5: the expansion node and the total cost value f _extend Putting the search result into an Openlist to be explored, and returning to the step 3 for continuing;

step 6: connecting the expansion node with the end point by using a Reeds-sheet curve, judging whether an obstacle exists on the curve or not, and if yes, returning to the step 3 to continue; otherwise, returning to the path point set;

step 7: resampling the path point set and outputting an optimized path;

in the step 2, the specific process of constructing the node sampling function expansion_pattern is as follows:

the algorithm is firstly that phi is epsilon { -phi _max ,0,φ _max }，v∈{-v _max ,v _max Uniformly sampling in the range of } to obtain sampling value, and then according to the sampling value [ v, phi ] in unit time]Forward simulating the moving robot to judge whether the sampling value accords with the collision condition; wherein v is the forward speed, phi is the deflection angle of the front wheel of the mobile robot, phi _max V is the maximum deflection angle of the front wheel of the mobile robot _max Is the maximum forward speed;

in the step 3, the specific process of searching the expansion node by forward simulation is as follows:

expanding a current node through a forward simulation calculation formula, and connecting the nodes by using a Reeds-shaep curve; the current node pose is N _current (x _current ,y _current ,θ _current ) Expanding the node pose to N _expend (x _expend ,y _expend ,θ _expend ) The forward simulation calculation formula is as follows:

x _expend ＝x _current +cosθ _current vdt

y _expend ＝y _current +cosθ _current vdt

wherein L is _w Distance between front and rear axes of the kinematic model of the mobile robot; n is a constant;

in the step 4, the total cost value f of the extension node is calculated _extend The specific process of (2) is as follows:

f _extend ＝g _extend +h _extend

wherein:

g _extend ＝g _current +C _simu +p _bias +p _{v_change} +p _{v_reverse} +p _{phy_change}

in the formula g _current C for the cost value of the starting point expansion to the current node _simu For forward simulation cost value, p, superimposed each time a child node is extended _bias To extend the penalty of a node for deviating from the bootstrap path, p _{v_change} Penalty introduced when the mobile robot forward speed v is changed, p _{v_reverse} Penalty introduced when the travelling speed v direction of the mobile robot is changed, p _{phy_change} Penalty introduced when changing the front wheel deflection angle phi of the mobile robot;

h _extend ＝β*max(h _{nonholonomics} ,max(h _{collision_avoidance1} ,h _{collision_avoidance2} ))

wherein, beta is a weight coefficient of heuristic value, h _{nonholonomics} Estimating the secondary expansion junction without considering obstacle avoidance for using Reeds-Sheep curveHeuristic value from point pose to end pose, h _{collision_avoidance1} Heuristic value h from expansion node to end point of JPS algorithm under two-dimensional grid _{collision_avoidance2} To extend the manhattan distance from the node to the endpoint.

2. The method for planning a path of a mobile robot based on JPS guided hybrid a of claim 1, wherein: in the step 3, a collision detection method is adopted in forward simulation and in the step 6, whether an obstacle exists on the curve or not is judged, each path point is used as the axis of a rear axle of the mobile robot in the collision detection method, and a simple kinematic model is built by calculating four vertex coordinates of the mobile robot to detect boundary collision.