A Path Planning Method for Unmanned Vessels Based on an Improved A* Algorithm

Abstract

This invention discloses a path planning method for unmanned surface vessels (USVs) based on an improved A* algorithm, comprising the following steps: Step S1: Establishing a grid map using environmental information obtained from semantic segmentation results, with each grid cell marked as a feasible or obstacle area, and providing a starting point B and a target point for path planning; Step S2: Introducing azimuth cost to optimize the cost function, reducing the total number of search path points and improving search efficiency; Step S3: Pruning and B-spline curve optimization of the improved A* algorithm pathfinding results, reducing the total path length while making the travel path smoother; Step S4: Adding collision detection functionality to reserve sufficient safety distance between the USV and obstacles. This invention introduces azimuth cost to optimize the cost function of the traditional A* algorithm, and prunes and optimizes the improved A* algorithm pathfinding results using B-spline curve optimization, reducing the total path length while making the travel path smoother.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning, and in particular to an unmanned vessel path planning method based on an improved A* algorithm. Background Technology

[0002] Path planning technology is an important technology for mobile robots to achieve autonomous navigation and obstacle avoidance, and many research results have been achieved in this area in recent years. The principle is that, in the working environment, the robot, based on its own sensor perception of the environment, autonomously plans an optimal or near-optimal path from the starting point to the target point, enabling the robot to avoid obstacles in the working environment and reach the destination safely and efficiently.

[0003] The A* algorithm is a heuristic global path search algorithm that combines the advantages of Dijkstra's algorithm and the best-first search algorithm. It is simple, efficient, flexible, and accurate, and is widely used in global path planning. However, the traditional A* algorithm places a large number of access points in the Openlist during the evaluation process, using F(n) to estimate their costs and sort them. If the unmanned surface vessel (USV) is in a large water environment with a long planned path, the number of points in the Openlist will be particularly large, resulting in a huge computational burden on the evaluation function. The storage and sorting of Openlist points will also become more complex, leading to unsatisfactory path planning efficiency. Furthermore, although the final path planned by the algorithm is the optimal path, there may be excessively large turns or violations of the USV's motion characteristics at some turning points or grid connections, which may hinder or prevent the USV from navigating normally. Further optimization is needed.

[0004] While the traditional A* algorithm can find the shortest path among many paths, it still has some shortcomings that need to be improved. Therefore, it is essential to study a path planning method that can improve the path planning performance of unmanned vessels, addressing the shortcomings of the A* algorithm such as the computation of a large number of unnecessary nodes and the violation of the kinematic laws of unmanned vessels at corners. Summary of the Invention

[0005] To address the aforementioned problems, this invention discloses a path planning method for unmanned surface vessels based on an improved A*Sat algorithm. This method introduces azimuth cost to reduce the total number of search path points, improves search efficiency, and performs secondary optimization on the planned path, resulting in a more streamlined and smoother path while maintaining a safe distance.

[0006] This invention provides a path planning method for unmanned surface vessels based on an improved A* algorithm, specifically including the following steps:

[0007] Step S1: Build a grid map using the environmental information obtained from semantic segmentation. Each grid is marked as a feasible area or an obstacle area, and the starting point B and the target point G for path planning are given.

[0008] Step S2: Introduce azimuth cost to optimize the cost function f(n), reduce the total search path points, and improve search efficiency;

[0009] Step S3: Prune and optimize the improved A* algorithm pathfinding results using B-spline curves to reduce the total path length and make the driving path smoother.

[0010] Step S4: Add collision detection function to reserve a sufficient safe distance between the unmanned boat and obstacles.

[0011] Furthermore, in step S2, an azimuth cost (value) is introduced. The principle of the azimuth cost is to obtain a dynamic parameter (value) through the angle of the line connecting the current node and the midpoint of the starting point (BG). Let the starting and ending point vectors be... Based on the convergence of the search node and the endpoint vector, it is found that... The value of the parameter `value` is adjusted to change the cost incurred during movement, thus converging the search path towards the destination. The optimization cost function `f(n)` is as follows:

[0012] f(n)=value×g(n)×a+h(n)×b (1)

[0013] Where `value` represents the directional cost of the current search point, and `a` is a decimal between [0.4, 0.6], with `a` and `b` satisfying the relationship `a = 1 - b`. For the current search node C, Compared to If the direction angle is θ, then the azimuth angle is... satisfy:

[0014]

[0015] The final azimuth cost coefficient is obtained as follows:

[0016]

[0017] Further, in step S3, the original n x 2 matrix path is iterated. First, the first point is selected and designated as O. Then, the loop iterates backward from the last trajectory point, assuming the distance from that point to point O is L. First, the slope between that point and point O is calculated, and the length of this slope direction is increased from the current node from 1 to L. If an obstacle is encountered during the extension, the point is retained and enters the next loop. If there is no obstacle on the straight line between the two points, the point is deleted. The remaining point is the node with the shortest distance obtained by pruning based on the A* algorithm.

[0018] Next, the advantages of B-spline curves are used to complete the post-fitting of path points to generate a smooth path. Assuming there are n+1 control points, which define the direction and limits of the spline curve, the definition of a k-order B-spline curve is:

[0019]

[0020] Among them, B i,k (u) is the i-th k-th B-spline basis function, corresponding to the control point, k≥1; u is the independent variable, and the basis function has the following derivation:

[0021]

[0022]

[0023] Choose a quasi-uniform B-spline curve, k=3, and set both the starting point and the target point to 3 nodes.

[0024] Furthermore, in step S4, a shortest distance D is set between the path point and the obstacle. min Let h be the distance between a path point and the nearest obstacle. If h <D min If the target moves to the opposite direction of the nearest obstacle, it will continue searching according to the original plan until it reaches a safe distance.

[0025] Compared with existing methods, the present invention has the following advantages and beneficial effects:

[0026] (1) Adding azimuth cost to the movement function makes the path planning direction more purposeful, which greatly reduces the number of nodes stored in the OPEN table, reduces the storage space occupied by the OPEN table, and improves the pathfinding efficiency of the algorithm.

[0027] (2) To address the issues of excessive path curvature and violation of kinematic laws, the optimized algorithm is combined with path pruning and B-spline smoothing, which effectively solves the problems of non-optimal path length and numerous turning points when the traditional A* algorithm performs path planning on grid maps, making it more suitable for unmanned vessel operation.

[0028] (3) A collision detection function was added during the path planning process to avoid collisions between the unmanned vessel and obstacles during operation, thus ensuring the safety of the unmanned vessel during operation. Attached Figure Description

[0029] Figure 1 This is a schematic diagram of the process of the present invention;

[0030] Figure 2 This is a schematic diagram of the principle of azimuth cost;

[0031] Figure 3 These are simulation results of the paths planned and the total search points by the method of this invention and the traditional A* algorithm;

[0032] Figure 4 This is a comparison of simulation results between the search path of the algorithm of this invention and the path after secondary optimization;

[0033] Figure 5 This is a simulation comparison of the path after adding collision detection and the planned path before adding collision detection in this invention. Detailed Implementation

[0034] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. It should be noted that the terms "front," "rear," "left," "right," "up," and "down" used in the following description refer to directions in the accompanying drawings, and the terms "inner" and "outer" refer to directions toward or away from the geometric center of a specific component, respectively.

[0035] This invention provides a path planning method for unmanned surface vessels based on an improved A* algorithm, specifically including the following steps:

[0036] Step S1: Build a grid map using the environmental information obtained from semantic segmentation. Each grid is marked as a feasible area or an obstacle area, and the starting point B and the target point G for path planning are given.

[0037] Step S2: Introduce azimuth cost to optimize the cost function f(n), reduce the total search path points, and improve search efficiency;

[0038] Step S3: Prune and optimize the improved A* algorithm pathfinding results using B-spline curves to reduce the total path length and make the driving path smoother.

[0039] Step S4: Add collision detection function to reserve a sufficient safe distance between the unmanned boat and obstacles.

[0040] In step S2, the azimuth cost (value) is introduced, the principle of which is as follows: Figure 2 As shown, a dynamic parameter `value` is obtained by using the angle of the line connecting the current node and the midpoint of the starting point (BG). Let the starting and ending point vectors be... Based on the convergence of the search node and the endpoint vector, it is found that... The value of the parameter is adjusted to change the cost of movement, thus converging the search path towards the destination. As shown in the figure, in the basic grid diagram, for the current search node C, the cost of moving upwards and moving to the right is both 10. However, moving upwards to C1 is more convenient to reach the target point because the vector is smaller than that of C2. with the starting and ending vectors The vector angle θ1 is lower than and The vector angle θ2, that is, the former converges more to the line connecting the start and end points, therefore, it is necessary to converge to the line connecting the start and end points. The search direction is incentivized. The optimized cost function f(n) is:

[0041] f(n)=value×g(n)×a+h(n)×b (1)

[0042] Where `value` represents the directional cost of the current search point, and `a` is a decimal between [0.4, 0.6], with `a` and `b` satisfying the relationship `a = 1 - b`. For the current search node C, Compared to If the direction angle is θ, then the azimuth angle is... satisfy:

[0043]

[0044] The final azimuth cost coefficient is obtained as follows:

[0045]

[0046] Figure 2 In the diagram, light gray represents the nodes that have been searched, dark gray represents the final planned path, and black represents obstacles. It can be seen that the A* algorithm, after adding azimuth cost, significantly reduces the number of search points during the pathfinding process, improves search efficiency, and reduces search time.

[0047] In step S3, the original n x 2 matrix path is iterated. First, the first point is selected and designated as O. Then, the loop iterates backward from the last trajectory point, denoted as L, where the distance from the last point to point O is L. The slope between the last point and point O is calculated, and the length of this slope direction is increased from the current node from 1 to L. If an obstacle is encountered during the extension, the point is retained and added to the next loop. If there is no obstacle on the straight line between the two points, the point is deleted. The remaining point is the node with the shortest distance obtained by pruning based on the A* algorithm.

[0048] Next, the advantages of B-spline curves are used to complete the post-fitting of path points to generate a smooth path. Assuming there are n+1 control points, which define the direction and limits of the spline curve, the definition of a k-order B-spline curve is:

[0049]

[0050] Among them, B i,k (u) is the i-th k-th B-spline basis function, corresponding to the control point, k≥1; u is the independent variable, and the basis function has the following derivation:

[0051]

[0052]

[0053] In the B-spline curve formula, the k value represents the smoothness of the curve. The larger the k value, the smoother the curve, but the higher the computational complexity; if the k value is too small, the smoothness of the curve is poor. To balance the smoothness of the unmanned vessel trajectory and the computational complexity, a quasi-uniform B-spline curve is chosen, with a k value of 3, and both the starting point and the target point are set to triple nodes.

[0054] Comparison of the improved results with the original results Figure 4 As shown in the figure, the optimized path is shorter and smoother.

[0055] In step S4, a shortest distance D is set between the path point and the obstacle. min Let h be the distance between a path point and the nearest obstacle. If h <D min If the obstacle is not found, move along the line extending in the opposite direction from the nearest obstacle until you reach a safe distance, then continue the search according to the original plan. The result is as follows: Figure 5 As shown, the optimized path provides sufficient safety space for the unmanned vessel.

[0056] The technical means disclosed in this invention are not limited to those disclosed in the above embodiments, but also include technical solutions composed of any combination of the above technical features.

Claims

1. An unmanned ship path planning method based on an improved A-star algorithm, characterized in that, The method comprises the following steps: Step S1: establishing a grid map using the environment information obtained from the semantic segmentation result, each grid being marked as a feasible area or an obstacle area, and a starting point B and a target point G being given for path planning; Step S2: introducing an azimuth angle cost optimization cost function f(n) to reduce the total search path points and improve the search efficiency; the step S2 specifically comprises the following process: A azimuth cost value is introduced. The principle of the azimuth cost value is to obtain a dynamic parameter value through the angle of the line (BG) connecting the current node with the start point and the end point. Let the start point and end point vectors be The size of the parameter value is adjusted according to the degree of convergence of the search node and the end point vector to to change the cost consumed when moving, so as to make the search path converge to the end point; and an optimization cost function f(n) is optimized. f(n) = value x g(n) x a + h(n) x b (1) wherein value is the direction angle cost of the current search point, a is a decimal number between 0.4 and 0.6, and a and b satisfy the relationship a = 1 - b; for the current search node C point, Compared with The direction angle of is θ, and the azimuth angle of satisfies: Finally, the azimuth angle cost coefficient is obtained: Step S3: pruning the improved A* algorithm path finding result and optimizing the B-spline curve to reduce the total path length and make the driving path smoother; the step S3 specifically comprises the following process: The original n rows and 2 columns of matrix paths are saved and looped, the first point is selected and set as O, then the last trajectory point is looped forward, the distance from the point to O is L, the slope between the point and O is calculated first, and the length in the slope direction is increased from the current node by 1 to L, if an obstacle is encountered in the extension process, the point is retained for the next loop, if there is no obstacle on the straight line between the two points, the point is deleted; finally, the remaining points are the shortest distance nodes obtained by pruning based on the A* algorithm; Then, the advantages of the B-spline curve are used to complete the late fitting of the path points to generate a smooth path; Supposing that there are n+1 control points, the control points are used to define the direction and limit range of the spline curve, and the definition of the k-order B-spline curve is: u is the independent variable, and the base function has the following derivation formula: where B i,k (u) is the i-th k-th order B-spline basis function corresponding to the control point, k ≥ 1; The quasi-uniform B-spline curve is selected, k=3, and the starting point and the target point are both set as 3-order nodes; Step S4: adding a collision detection function to reserve sufficient safety distance between the unmanned ship and the obstacle. The step S4 specifically comprises the following process:

2. The unmanned ship path planning method based on the improved A-star algorithm according to claim 1, wherein: ​ Set a minimum distance D between path point and obstacle min Set the distance between path point and nearest obstacle as h, if h < D min Move to the opposite extension line of the nearest obstacle until it is outside the safety distance, then continue the search according to the original planning method.

Images