An improved jps path planning method for unmanned vehicles suitable for complex environments

Abstract

The application discloses an improved JPS Unmanned vehicle path planning method of algorithm suitable for complex environment belongs to the field of unmanned driving. Based on the angle constraint of the current expansion node and the target node and the direction constraint of the current expansion node and its forced neighbor node as the judgment basis of the search neighborhood, the neighborhood search direction of the unmanned vehicle is updated in real time. JPS When the algorithm is used for path planning, the evaluation function is dynamically weighted according to the number of searched jump points and the distance from the current node to the target node. JPS The path trajectory is processed based on the Bezier curve interpolation method, so that the path trajectory is smoother. Based on the simulation of the two-dimensional complex grid map with different sizes but the same obstacle proportion, the results show that the application can improve the path planning efficiency under the premise that the path trajectory length is basically unchanged, and the path trajectory is smooth, and the robustness in the complex environment is better.

Description

Technical Field

[0001] This invention pertains to autonomous driving technology, specifically to an improved JPS-based autonomous vehicle path planning method suitable for complex environments. Background Technology

[0002] In recent years, with the continuous advancement of science and technology, the concept of autonomous driving has come into people's view. Autonomous driving is an important application of mobile robotic intelligent agents in the transportation field. Autonomous navigation path planning, as a key component of autonomous driving technology, faces the challenge of quickly planning safe, reliable, and smooth driving routes in complex environments, thus ensuring the sensitivity and safety of autonomous driving. This has become an urgent problem to be solved.

[0003] In terms of rapid path planning, a well-known method is the Jump Point Search (JPS) algorithm, proposed by Dr. Daniel Harabor of Monash University, Australia, in a paper. The core idea of ​​the JPS algorithm is to find jump points. When an autonomous vehicle moves within a grid map, such as... Figure 1 The current expanding node is `cur_node`, and the resulting jump point is `Jump_node`, whose forced neighbor is `force_neighbor.node`. Due to the presence of obstacle nodes, the path from `cur_node` through `Jump_node` to `force_neighbor.node` becomes the shortest path, with `Jump_node` being the current expanded jump point from `cur_node` to `force_neighbor.node`. If the obstacle nodes are removed, a symmetrical shortest path from `cur_node` to `force_neighbor.node` exists, and the shortest path is no longer unique. The JPS algorithm significantly improves search efficiency by finding jump points and calculating cost values. The cost function of the JPS algorithm consists of two parts: the total cost value g(n) from the starting node to the current node and the Euclidean distance h(n) from the current node to the target node.

[0004] As an improvement on the A* algorithm, the JPS algorithm has advantages such as fast planning speed and fewer search nodes. However, in complex environments, the number of hops obtained is large and the obstacle density is high. The traditional JPS path planning algorithm has the following shortcomings:

[0005] (1) The search for redundant neighborhood jump points during the path search process reduces the planning efficiency of the JPS path planning algorithm.

[0006] (2) The heuristic function is set in a relatively simple way. It does not dynamically update the heuristic function according to the current position of the autonomous vehicle and the number of jump points found, and its adaptability to complex environments is not strong.

[0007] (3) The planned driving trajectory is not smooth enough, and there is a possibility of collision with obstacles in complex environments, which makes it easy to collide in actual operation.

[0008] The paper "AGV Path Planning Based on Improved A* Algorithm," published in the journal *Applications of Computer Systems*, improves the search neighborhood of the A* path planning algorithm by proposing a five-neighborhood search method based on the angle constraint from the starting node to the target node. This method eliminates the need for searches in three directions, thus improving search efficiency. However, this method only considers the angle from the starting node to the target node as a criterion for neighborhood optimization and does not take into account the angular relationship between the current extended jump point and the target node during robot movement. Therefore, it cannot adapt to more complex environments. Summary of the Invention

[0009] To address the aforementioned issues, this invention proposes an improved JPS algorithm-based unmanned vehicle path planning method suitable for complex environments. This method improves path planning efficiency, produces smoother planned routes, and enhances the obstacle avoidance performance of unmanned vehicles in complex environments.

[0010] The technical solution for implementing this invention is: an improved JPS-based unmanned vehicle path planning method suitable for complex environments, comprising the following steps:

[0011] S1: Obtain a two-dimensional raster map of the complex environment area to be tested, initialize the two-dimensional raster map to obtain the initialized two-dimensional raster map, and then proceed to S2.

[0012] S2: Initialize the two-dimensional grid map as the search space for autonomous vehicle path planning. By introducing the angle constraints between the current node and the target node of the planning subject and the directional relationship constraints from the current expansion point to its forced neighbor node, the JPS path search neighborhood is improved, resulting in the improved path search neighborhood. Proceed to S3.

[0013] S3: Based on the cost function of the JPS algorithm, combined with the improved path search neighborhood, the path cost is calculated using a heuristic function with dual constraints of distance and number of jump points. Path planning is performed on the initialized two-dimensional grid map to obtain a drivable path in a global complex obstacle environment, and then proceed to S4.

[0014] S4: For the planned drivable path trajectory in a globally complex obstacle environment, smoothing is performed based on the Bezier interpolation function to obtain a smooth drivable path, and the robot travels based on this smooth path.

[0015] Compared with the prior art, the beneficial effects achieved by the present invention are:

[0016] (1) Instead of simply using the angle between the starting node and the target node, the angle between the current extended jump point and the target node is calculated to determine the five-neighbor search direction of the jump point. Based on the traditional jump point search algorithm, the search neighborhood of three directions is reduced, the number of redundant jump points that need to be processed is reduced, and the direction relationship between the current extended jump point and its forced neighbor nodes is introduced to further improve the neighborhood search direction. Under the premise that the total planned path length remains basically unchanged, the planning efficiency is greatly improved. Compared with the method of using the angle between the starting node and the target node to determine the five-neighbor search direction, the algorithm described in this patent has stronger real-time performance and robustness. It can be applied not only to simple environments, but also has strong adaptability in complex environments.

[0017] (2) By using a dynamic weighted evaluation function based on dual constraints of distance ratio factor and hop count factor, instead of simply using the traditional A* heuristic cost function to calculate the cost F(n) of the current hop, the autonomous vehicle can dynamically adjust the weight coefficient of h(n) according to the distance ratio factor between the current node and the target node and the number of hops already searched. In the early stage of path planning, the distance ratio between the current node and the target node is large, the number of hops is small, and the weight coefficient of h(n) is large, resulting in a significant improvement in the efficiency of path planning. In the later stage of path planning, the distance ratio between the current node and the target node is small, the number of hops is large, and the weight coefficient of h(n) is small, at which point the path planning is closer to the optimal path. The improved dynamic weight heuristic function dynamically adjusts the weights according to the hop count factor and distance ratio factor, making it more adaptable to complex environments and improving the overall computational efficiency of the JPS algorithm.

[0018] (3) By using the Bessel interpolation function to process the planned drivable path to obtain a smooth drivable trajectory, instead of simply using the planned drivable path for autonomous navigation, the drivable vehicle's trajectory is further away from obstacles, enhancing the safety of drivable vehicle navigation in complex environments. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of existing jump points and forced neighbor nodes.

[0020] Figure 2 This is a flowchart of the unmanned vehicle path planning method based on the improved JPS algorithm for complex environments, as described in this invention.

[0021] Figure 3(a) is a schematic diagram of the neighborhood search of the JPS algorithm.

[0022] Figure 3(b) is a schematic diagram of the five-neighbor search and judgment conditions of the improved JPS algorithm described in this invention.

[0023] Figure 4 This is an example graph illustrating neighborhood expansion after introducing forced neighbor direction relations based on the improved five-neighbor search.

[0024] Figure 5(a) is a schematic diagram of the JPS algorithm's path planning in a 25*25 complex environment grid map.

[0025] Figure 5(b) is a schematic diagram of the planned path of the improved JPS algorithm described in this invention in a 25*25 complex environment grid map.

[0026] Figure 6(a) is a schematic diagram of the JPS algorithm's path planning in a 35*35 complex environment grid map.

[0027] Figure 6(b) is a schematic diagram of the planned path of the improved JPS algorithm described in this invention in a 35*35 complex environment grid map.

[0028] Figure 7(a) is a schematic diagram of the JPS algorithm's path planning in a 50*50 complex environment grid map.

[0029] Figure 7(b) is a schematic diagram of the planned path of the improved JPS algorithm described in this invention in a 50*50 complex environment grid map.

[0030] Figure 8 This is a comparison chart of the effects of the JPS algorithm and the improved JPS algorithm described in this invention in the above complex raster map.

[0031] Figure 9(a) shows the smoothed driving trajectory of the unmanned vehicle without the processing of the Bessel interpolation function.

[0032] Figure 9(b) shows the smoothed driving trajectory of the autonomous vehicle after processing with the Bessel interpolation function. Detailed Implementation

[0033] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0034] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present invention are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indication will also change accordingly.

[0035] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are feasible to those skilled in the art. If the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0036] The following section will further introduce the specific implementation method, as well as the technical difficulties and inventive points of this invention, using examples from this design.

[0037] In complex environments with large size and messy obstacle distribution, autonomous vehicles cannot dynamically weight the evaluation function according to the situation in the complex environment. The planning efficiency of the path planning is low, and the planned driving trajectory is not smooth enough. In environments with many obstacles, the driving path is too close to the obstacles, which poses a risk of collision with the obstacles.

[0038] To address the above issues, this invention proposes an improved JPS algorithm-based unmanned vehicle path planning method suitable for complex environments. Based on the JPS algorithm's evaluation function, a distance scaling factor and a jump point number factor are introduced. This allows for dynamic weighting of the evaluation function according to the unmanned vehicle's current node position and the distribution of obstacles in the surrounding environment. During the expansion of jump points, the search for redundant jump points in multiple directions is eliminated, significantly improving computational efficiency and path rationality. Furthermore, the planned drivable path is smoothed using a Bessel interpolation function, enhancing the robustness of unmanned vehicle navigation in complex environments.

[0039] Combination Figures 2-4 The present invention provides an improved JPS algorithm for unmanned vehicle path planning suitable for complex environments, comprising the following steps:

[0040] S1: For the complex environment area to be tested, a 16-line LiDAR is used to scan and generate a two-dimensional grid map. The grid map has the lower left corner as the origin o, the horizontal axis as the x-axis, and the vertical axis as the y-axis. The starting node start_node and the target node goal_node of the path planning are determined and initialized to obtain the initialized two-dimensional grid map. Proceed to S2.

[0041] S2: Initialize the two-dimensional grid map as the search domain for autonomous vehicle path planning. By introducing the angle constraints between the current node and the target node of the planning subject and the directional relationship constraints from the current expansion point to its forced neighbor node, the JPS path search neighborhood is improved, resulting in the improved path search neighborhood. Proceed to S3.

[0042] In S2, the introduction of angular constraints between the current node and the target node of the planning subject, as well as directional constraints from the current expansion point to its forced neighbor nodes, improves the JPS path planning search neighborhood by:

[0043] First, calculate the angle between the line connecting the current node cur_node and the target node goal_node and the x-axis. Initially, take the starting node start_node as the current node, and calculate the angle theta between the line connecting start_node and goal_node and the x-axis. This angle is used as a criterion for the five-neighbor search. Theta is calculated as follows:

[0044]

[0045] Where, x goal and y goal These correspond to the x and y coordinates of the target node raster, respectively. cur_onde and y cur_node These correspond to the x-coordinates and y-coordinates of the current jump point node grid, respectively.

[0046] Based on the calculated value of theta, the angle is divided into eight intervals, and the direction of the five-neighbor search is determined. The specific determination method is as follows:

[0047] Using the currently found jump point as a reference, define the upper right, directly above, upper left, directly left, lower left, directly below, lower right, and directly right as dir1, dir2, dir3, dir4, dir5, dir6, dir7, and dir8 respectively. Calculate the angle between the line connecting the current jump point and the target node and the x-axis as theta. If theta ∈ (30°, 60°], then the prior search neighborhood Dir obtained from the constraint angle theta is... search_priorThe prior search neighborhood is (dir1, dir2, dir3, dir7, dir8); if theta ∈ (60°, 120°], then the prior search neighborhood obtained by the constraint angle theta is (dir1, dir2, dir3, dir4, dir8); if theta ∈ (120°, 150°], then the prior search neighborhood obtained by the constraint angle theta is (dir1, dir2, dir3, dir4, dir5); if theta ∈ (150°, 180°] ∪ [-150°, -180°), then the prior search neighborhood obtained by the constraint angle theta is (dir2, dir3, dir4, dir5, dir6); if theta ∈ [-120°, -150°] ∪ [-150°, -180°], then the prior search neighborhood obtained by the constraint angle theta is (dir2, dir3, dir4, dir5, dir6); if theta ∈ [-120°, -150°] ∪ [-150°, -180°] ∪ [-150°, -180°] ∪ [-120°, -150°] ∪ [-120°, -180°] ∪ [-150°, -180°] ∪ [-12 ... If the constrained angle theta is (dir3, dir4, dir5, dir6, dir7), then the prior search neighborhood obtained by the constrained angle theta is (dir4, dir5, dir6, dir7, dir8); if theta ∈ [-60°, -120°), then the prior search neighborhood obtained by the constrained angle theta is (dir4, dir5, dir6, dir7, dir8); if theta ∈ [-30°, -60°), then the prior search neighborhood obtained by the constrained angle theta is (dir1, dir5, dir6, dir7, dir8); if theta ∈ (0°, 30°] ∪ [0°, -30°), then the prior search neighborhood obtained by the constrained angle theta is (dir1, dir2, dir6, dir7, dir8).

[0048] Based on the current extended jump point and the directional relationship between its forced neighbor nodes, the search direction set is further improved, and the following alternative search neighborhoods are obtained:

[0049]

[0050] In the above formula, (x1, y1) are the coordinates of the forced neighbor nodes, (x0, y0) are the coordinates of the initial expansion jump point, and Dir search_sub It is the set of alternative neighborhood search directions obtained from the directional relationship between the current extended jump point and its forced neighbor nodes.

[0051] Dir, the set of alternative neighborhood search directions search_sub With prior search neighborhood Dir search_proir Taking the union of the sets, we obtain the jump point search neighborhood set Dir. searchThe inclusion of the directional relationship between the current expanding jump point and its forcing neighbor nodes is to improve the adaptability of the five-neighbor search in complex environments. As shown in Figure 3, the triangle node is the current expanding node, the diamond node is its forcing neighbor node, and the circle node is the target point. If only the original five-neighbor search is used, the search directions are (dir1, dir2, dir3, dir7, dir8). Due to obstacles in these search directions, it is difficult to obtain new jump points, and the path planning is likely to fail. Therefore, an alternative neighborhood search direction set is introduced to reasonably expand the search range. At this point, the jump point search neighborhood set Dir... search Given (dir1, dir2, dir3, dir6, dir7, dir8), the expanded nodes are shown in the shaded area in the figure, and new jump points can be found by searching.

[0052] S3: Search for jump points that meet the conditions in the improved path search domain. Use a dynamic weight heuristic function based on the distance factor between the current node and the target node and the number of jump points to calculate the cost of the jump point. Select the jump point with the smaller cost as the next expansion point. Repeat this process until the target node is reached. The drivable trajectory planning is completed, and then proceed to S4.

[0053] Furthermore, S3, based on the improved path search neighborhood, finds jump points that meet the conditions on the initialized two-dimensional raster map. The specific method is as follows:

[0054] Step S31: Create the jump point search matrix OPEN and the jump point closure matrix CLOESD, which are used to store the jump points to be detected and the visited jump points and obstacles, respectively. At the start time, put the starting point coordinates into the OPEN matrix, and also put the parent node coordinates of the starting point coordinates, the total value F(n), and the jump point search neighborhood set Dir. search Store it in the OPEN matrix, initialize the jump node count variable Jump_node_count = 0, and increment the Jump_node_count variable by one for each jump node obtained after expansion.

[0055] Step S32: Select the node with the smallest total value F(n) in the OPEN matrix as the current expansion jump point, and move it towards Dir. search If no suitable jump point is found in the neighborhood direction and the OPEN matrix does not contain any nodes, the path planning fails and there is no drivable path from the current node to the target node, thus ending the path planning process. If the OPEN matrix contains a suitable jump point, proceed to step S33.

[0056] Step S33: Add the current extended jump point to the CLOESD matrix and record it as a visited node. In subsequent loops, the nodes already recorded in the CLOSED matrix will not be searched.

[0057] Determine whether the target node is in the OPEN matrix. If the target node is recorded in the OPEN matrix, proceed to step S4; otherwise, jump to step S2 and update the searched path cost F(n).

[0058] The dynamic weight function for updating the path cost F(n) is shown below:

[0059]

[0060] In the formula, F(n) is the total cost from the starting point to the target node in path planning, g(n) is the actual cost path length from the starting point to the current jump point, dist(goal_node, Jump_node) is the Euclidean distance from the target node to the current jump point, Jump_node_count is the number of jump points found so far, dist(start_node, Jump_node) is the Euclidean distance from the starting point to the current jump point, h(n) is the Euclidean distance from the current node to the target jump point, the first term of the coefficient before h(n) is defined as the distance scaling factor, the second term is defined as the jump point count factor, and k is the jump point count suppression factor, which is adjusted according to the actual map situation. The reference value can be obtained by the following function:

[0061]

[0062] S4: Use the Bezier interpolation function to optimize the drivable trajectory, making the path smooth, and the autonomous vehicle navigates based on this smooth trajectory.

[0063] The specific formula for smoothing based on the Bessel interpolation function is as follows:

[0064]

[0065] In the formula, t is the scaling factor. Let i be the i-th control point on the drivable trajectory in a global obstacle environment, where i = 0, 1, 2, 3. To optimize the obtained smooth curve.

[0066] Example 1

[0067] Combination Figures 2-4 This invention describes an improved JPS (Journal of Power Grid) path planning method for autonomous vehicles suitable for complex environments. Using a 25*25 grid map as an example, the method includes the following steps:

[0068] S1. For the complex environment area to be tested, a 32-line LiDAR is used to scan and generate a two-dimensional grid map. The grid map has the lower left corner as the origin o, the horizontal axis as the x-axis, and the vertical axis as the y-axis. The two-dimensional grid map is initialized to obtain an initialized two-dimensional grid map.

[0069] S2 initializes a two-dimensional grid map as the search space for autonomous vehicle path planning. To avoid searching for redundant jump points in the JPS algorithm and improve the computational efficiency of path planning, the angle constraints between the current node and the target node of the autonomous vehicle and the directional relationship constraints between the current extended jump point and its forced neighbor nodes are introduced as judgment conditions to improve the JPS path search neighborhood and obtain the improved path search neighborhood.

[0070] S2.1. Taking the starting node as the search starting point, calculate the angle theta between the line connecting the starting node and the target node and the positive right direction. This angle is used as the criterion for determining the five-neighbor search direction. The specific determination method is as follows:

[0071]

[0072] S2.2. Using the currently found jump point as the reference, define the upper right, directly above, upper left, directly left, lower left, directly below, lower right, and directly right as dir1, dir2, dir3, dir4, dir5, dir6, dir7, and dir8 respectively. Using the angle theta between the line connecting the current jump point and the target node (obtained in S2.1) and the x-axis as the reference, if theta ∈ (30°, 60°], then the prior search neighborhood Dir obtained from the constraint angle theta is... search_priorThe prior search neighborhood is (dir1, dir2, dir3, dir7, dir8); if theta ∈ (60°, 120°], then the prior search neighborhood obtained by the constraint angle theta is (dir1, dir2, dir3, dir4, dir8); if theta ∈ (120°, 150°], then the prior search neighborhood obtained by the constraint angle theta is (dir1, dir2, dir3, dir4, dir5); if theta ∈ (150°, 180°] ∪ [-150°, -180°), then the prior search neighborhood obtained by the constraint angle theta is (dir2, dir3, dir4, dir5, dir6); if theta ∈ [-120°, -150°] ∪ [-150°, -180°], then the prior search neighborhood obtained by the constraint angle theta is (dir2, dir3, dir4, dir5, dir6); if theta ∈ [-120°, -150°] ∪ [-150°, -180°] ∪ [-150°, -180°] ∪ [-120°, -150°] ∪ [-120°, -180°] ∪ [-150°, -180°] ∪ [-12 ... If the constrained angle theta is (dir3, dir4, dir5, dir6, dir7), then the prior search neighborhood obtained by the constrained angle theta is (dir4, dir5, dir6, dir7, dir8); if theta ∈ [-60°, -120°), then the prior search neighborhood obtained by the constrained angle theta is (dir4, dir5, dir6, dir7, dir8); if theta ∈ [-30°, -60°), then the prior search neighborhood obtained by the constrained angle theta is (dir1, dir5, dir6, dir7, dir8); if theta ∈ (0°, 30°] ∪ [0°, -30°), then the prior search neighborhood obtained by the constrained angle theta is (dir1, dir2, dir6, dir7, dir8).

[0073] S2.3 After obtaining the prior search neighborhood, it is necessary to calculate the candidate search neighborhood Dir based on the directional relationship from the current expansion point to its forced neighbor nodes. search_sub The specific calculation method is as follows:

[0074]

[0075] Where x1 and y1 correspond to the x-coordinates and y-coordinates of the forced neighboring node grid, respectively, and x0 and y0 correspond to the x-coordinates and y-coordinates of the current extended node grid, respectively.

[0076] S2.4, Dir based on prior search neighborhood search_prior and alternative search neighborhood Dir search_sub Determine the improved path search domain Dir search This will be used as the search neighborhood for the next path planning step, as follows:

[0077] Dir search =Dir search_prior ∪Dir search_sub

[0078] Autonomous vehicles rely on path search neighborhood Dirsearch The jump points in the initial 2D raster map are searched and used to calculate the path cost in S3. The specific steps for searching and storing jump point information are as follows, taking the dir8 direction as an example:

[0079] Expand from the current node towards dir8. If a jump point exists in the dir8 expansion direction, that is, during the expansion process, an obstacle is found in the next node in the (0, y+) direction and no obstacle exists in the next node in the (x+, y+) direction; or an obstacle exists in the next node in the (0, y-) direction and no obstacle exists in the next node in the (x+, y-) direction, record the coordinates of the jump point obtained in this expansion and the coordinates of the corresponding forced neighbor node. If there is no jump point in the dir8 expansion direction, an obstacle is found, or a target node is found, stop expanding in the dir8 direction. After the expansion is completed, add all the jump point coordinates obtained in this round of expansion to the OPEN matrix, and add the coordinates of the current node as the parent node coordinates of the current expansion jump point to the OPEN matrix. The jump point search information processing is complete.

[0080] S3. For the jump points in the OPEN matrix, use the dynamic weight heuristic function based on the dual constraints of distance scale factor and jump point number factor described in this invention to calculate the cost of the jump point. Select the jump point with the smaller cost as the next expansion point and perform path planning on the initialized two-dimensional grid map. If the target node is already in the OPEN matrix, it means that a drivable path has been obtained in the global complex obstacle environment, and proceed to S4; otherwise, proceed to S2 and continue to expand the jump points.

[0081] S3.1 Calculate the Euclidean distance dist(goal_node,cur_node) between the current jump point and the target node and the Euclidean distance dist(start_node,cur_node) between the current jump point and the starting node, where dist represents the Euclidean distance between the two nodes;

[0082] S3.2 Calculate the number of hops Num obtained during this neighborhood expansion process, and update the currently obtained hop count variable Jump_node_count, as follows:

[0083] Jump_node_count=Jump_node_count+Num

[0084] S3.3. Calculate the path cost F(n) based on dist(start_node,cur_node) and the jump point count variable Jump_Node_Count. Perform path planning on the initialized 2D grid map to obtain a drivable path in a globally complex obstacle environment. The calculation formula is as follows:

[0085]

[0086] Where k is the jump point suppression coefficient, which is adjusted according to the actual map conditions. A reference value can be obtained from the following function:

[0087]

[0088] In large and complex environments, due to the large number and uneven distribution of obstacles, the number of hops can easily increase too quickly during the planning process, causing the weight coefficient w(n) to converge to 0 prematurely. In this case, the evaluation function of the improved JPS is closer to the evaluation function of Dijkstra's algorithm, and the planned path is optimal but the efficiency is low. To avoid this situation caused by the number of hops increasing too quickly, a hop number suppression coefficient k is introduced to reduce the convergence speed of the weight factor w(n) and improve the planning efficiency of the algorithm.

[0089] The coordinates of the current expanded node are added to the CLOSED matrix and recorded as a visited node. Subsequent loops will not search for nodes already recorded in the CLOSED matrix.

[0090] S4. For the planned drivable path trajectory in a globally complex obstacle environment, smoothing is performed based on the Bezier interpolation function to obtain a smooth drivable path, and the robot travels based on this smooth path.

[0091] As can be seen from the comparison of the simulation diagrams in Figure 5(a) and Figure 5(b), in a small-sized 25*25 complex environment 2D grid map, the improved JPS algorithm described in this patent is slightly different from the JPS algorithm in terms of path planning trajectory, but the total trajectory length remains basically unchanged, indicating that the improved JPS algorithm described in this patent performs well in small-sized complex environments.

[0092] As can be seen from the comparison of the simulation diagrams in Figure 6(a) and Figure 6(b), in a medium-sized 35*35 complex environment 2D grid map, the improved JPS algorithm described in this patent is slightly different from the JPS algorithm in terms of path planning trajectory, but the total trajectory length remains basically unchanged, indicating that the improved JPS algorithm described in this patent performs well in medium-sized complex environments.

[0093] As can be seen from the comparison of the simulation diagrams in Figures 7(a) and 7(b), in a large-size 50*50 complex environment 2D grid map, the total trajectory length of the improved JPS algorithm described in this patent is slightly longer than that of the JPS algorithm, but the overall performance is good, indicating that the improved JPS algorithm described in this patent performs well in large-size complex environments.

[0094] Depend on Figure 8As can be seen, the improved JPS algorithm described in this patent, compared with the standard JPS algorithm, significantly improves the computational efficiency and real-time performance of the JPS planning algorithm while slightly increasing the total path length. Specifically, in complex environments of 25*25 and 35*35, the path length does not show significant degradation, but the computational efficiency is doubled. In medium-to-large complex environments of 50*50, the path length deteriorates slightly; however, the performance of the improved JPS algorithm in medium-to-large complex environments can be further improved by adjusting the value of the jump point suppression factor k.

[0095] As can be seen from Figures 9(a) and 9(b), the trajectory optimized using the Bessel interpolation function is smoother than the trajectory before optimization, and the trajectory is farther away from obstacles. The autonomous vehicle has stronger obstacle avoidance ability, higher robustness, and stronger adaptability in complex environments when driving along this smooth path.

[0096] The above embodiments are the best embodiments of the present invention and the technical principles applied. However, the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the principles of the present invention should be considered equivalent substitutions and are all within the protection scope of the present invention.

Claims

1. An improved JPS path planning method for unmanned vehicles suitable for complex environments, characterized in that, The specific steps are as follows: S1: Obtain a two-dimensional grid map of a complex environment region to be measured, and initialize the two-dimensional grid map to obtain an initialized two-dimensional grid map, and turn to S2; S2: Take the initialized two-dimensional grid map as a search space for path planning of the unmanned vehicle, improve the JPS path search neighborhood by introducing angle constraints of the current node and the target node of the planning agent and directional relationship constraints of the current expansion point to its forced neighbor nodes, obtain an improved path search neighborhood, and turn to S3; S3: On the basis of the JPS algorithm cost function, in combination with the improved path search neighborhood, a heuristic function is used to calculate the path cost value by adopting double constraints of distance and jump point number, and path planning is performed on the initialized two-dimensional grid map to obtain a drivable path in a global complex obstacle environment, and turn to S4; S4: For the drivable path trajectory planned in the global complex obstacle environment, smoothing processing is performed based on a Bezier interpolation function to obtain a smoothed drivable path, and the robot travels based on the smoothed path; The JPS algorithm is as follows: First, set the starting node and the target node of the unmanned vehicle, start from the grid where the starting node is located, search the 8-neighborhood direction around the starting node, add the jump point to the OPEN matrix, calculate the cost value of the jump point according to the cost function, select the jump point with the minimum total cost as the next moving point, and repeat the cycle until the target node is added to the OPEN matrix, and the navigation ends; In S1, the initialization of the two-dimensional grid map is as follows: Determine the positions of the starting point, the target point and the obstacles, create an obstacle position matrix obs and store the obstacle coordinates, create a jump point search matrix OPEN matrix and a jump point closed matrix to store the obstacle coordinates and the visited jump point coordinates in the path planning process, create a jump point number variable Jump_Node_Count to record the number of jump points recorded in the path planning, and the Jump_Node_Count variable is incremented by one each time a new jump point coordinate is recorded in the OPEN matrix; In S2, the initialized two-dimensional grid map is taken as the search space for path planning of the unmanned vehicle, and the JPS path search neighborhood is improved by introducing angle constraints of the current node and the target node of the planning agent and directional relationship constraints of the current expansion point to its forced neighbor nodes, and the improved path search neighborhood is obtained, which is as follows: S2.1, calculate the angle theta between the line connecting the current minimum jump point coordinate in the OPEN matrix of the jump point search matrix and the target node and the positive right direction, and the angle of the angle theta is determined based on the following formula: Wherein, x goal and y goal respectively correspond to the coordinates in the x direction and the y direction of the target node grid, x cur_node and y cur_node respectively correspond to the coordinates in the x direction and the y direction of the current hop point node grid; S2.

2. Determine the prior search region Dir according to the angle interval where theta is located search_prior In particular as follows: Wherein, based on the two-dimensional grid map, dir1 represents the positive right direction, dir2 represents the right upper direction, dir3 represents the positive upper direction, dir4 represents the left upper direction, dir5 represents the positive left direction, dir6 represents the left lower direction, dir7 represents the positive lower direction, and dir8 represents the right lower direction, S2.3, after getting the prior search region, the candidate search region Dir needs to be calculated according to the direction relation of the current expansion point to its forced neighbor nodes search_sub The specific calculation method is as follows: Wherein, x1 and y1 respectively correspond to the x direction and y direction coordinates of the forced neighbor node grid, x0 and y0 respectively correspond to the x direction and y direction coordinates of the current extended node grid, S2.4, determine the improved path search neighborhood Dir based on the prior search neighborhood search_prior and the alternative search neighborhood Dir search_sub determine the improved path search neighborhood Dir search and use it as the search neighborhood for the next step of path planning, as follows: Dir search = Dir search_prior ∪ Dir search_sub .

2. The improved JPS path planning method for unmanned vehicles applicable to complex environments according to claim 1, wherein, In S3, the heuristic function with double constraints of distance and jump point number is used to calculate the path cost value, which refers to: S3.1, calculate the Euclidean distance dist(goal_node, cur_node) between the current jump point and the target node and the Euclidean distance dist(start_node, cur_node) between the current jump point and the starting node, wherein dist represents the Euclidean distance between two nodes; S3.2, calculate the jump point number Num obtained in this neighborhood expansion process, and update the current jump point number variable Jump_node_count, as follows: Jump_node_count = Junp_node_count + Num S3.3, according to dist(goal_node, cur_node), dist(start_node, cur_node) and the updated jump point number variable Jump_Node_Count, calculate the path cost value, and perform path planning on the initialized two-dimensional grid map to obtain a drivable path in a global complex obstacle environment, and the calculation formula is as follows: In the formula, k is a jump point number suppression factor, which is adjusted according to the actual situation of the map, and the reference value of k is obtained from the following function: In the formula, F(n) is the total cost from the starting point to the target node in path planning, g(n) is the actual path length from the starting point to the current jump point, dist(goal_node, Jump_node) is the Euclidean distance from the target node to the current jump point, Jump_node_count is the current number of jump points searched, dist(start_node, Jump_node) is the Euclidean distance from the starting point to the current jump point, h(n) is the Euclidean distance from the current jump point to the target jump point, and the first term of the coefficient before h(n) is defined as the distance proportion factor, and the second term is defined as the jump point number factor.

3. The improved JPS path planning method for unmanned vehicles applicable to complex environments according to claim 2, wherein, In S4, for the drivable path trajectory planned in the global complex obstacle environment, the Bezier interpolation function is used for smoothing processing to obtain a smooth drivable path, and the robot travels based on the smooth path, as follows: The Bezier interpolation function is used to optimize the drivable trajectory obtained in the global complex obstacle environment, so that the drivable trajectory is smoother and the risk of collision with obstacles is reduced. The specific formula for smoothing processing based on the Bezier interpolation function is as follows: where t is a scale factor, is the ith control point on the feasible trajectory under the global obstacle environment, i = 0, 1, 2, 3, is the smooth curve obtained by optimization.

Images