Path planning method and system, electronic equipment and storage medium
By combining the unicycle model and the RRT* algorithm, a path tree is constructed and dynamic constraints are added, which solves the problem of insufficient kinematic constraints in unmanned surface vessel (USV) path planning, generates USV paths that are closer to real paths, and improves the safety and adaptability of path planning.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-25
- Publication Date
- 2026-04-10
AI Technical Summary
The unmanned surface vessel (USV) has poor autonomous path planning performance and fails to effectively consider kinematic constraints, resulting in suboptimal planned paths and an inability to travel along the planned paths.
By combining a unicycle model and the RRT* algorithm, a path tree is constructed and dynamic constraints are added. The steering inertia of the boat is described by a mathematical model to generate a target path that closely resembles the real path. The static or dynamic state of obstacles is considered in the path planning, and the path curvature is adjusted by an optimization algorithm to ensure smoothness.
It improves the safety, smoothness, and adaptability of path planning, generates paths that are closer to the actual motion of a ship, adapt to complex environments and avoid obstacles, and enhances the robustness and adaptability of the paths.
Smart Images
Figure CN121829544A_ABST
Abstract
Description
[0001] This application claims priority to Chinese Patent Application No. 202411964377.9, filed on December 30, 2024, entitled "A Path Planning Method, Path Planning Apparatus and Readable Storage Medium", the entire contents of which are incorporated herein by reference. Technical Field
[0002] This invention belongs to the field of ship technology and navigation control, and specifically relates to a path planning method, a path planning system, an electronic device, and a storage medium. Background Technology
[0003] Among the relevant sampling-based path planning techniques, the autonomous path planning performance of unmanned surface vessels (USVs) is poor. The planned path does not take into account the kinematic constraints of the USV and cannot guarantee that the resulting feasible path is relatively optimized. As a result, the USV cannot travel according to the planned path. Summary of the Invention
[0004] This invention provides a path planning method, a path planning system, an electronic device, and a storage medium.
[0005] Specifically, the present invention is achieved through the following technical solution:
[0006] According to a first aspect of the present invention, a path planning method is provided, comprising: obtaining a starting position and a target position; constructing a path tree based on a mathematical model and the starting position; obtaining a plurality of first child nodes between the starting position and the target position based on the overlap of nodes in the path tree with the target position; and generating a target path based on the plurality of first child nodes.
[0007] The path planning method provided by this invention specifically includes: first, obtaining the ship's starting position and the target position the ship wants to reach; then, constructing a path tree using a mathematical model and the starting position. Specifically, the mathematical model can be a unicycle model. Dynamic constraints based on the unicycle model are added during the path tree construction process to ensure that the path tree is as close as possible to the feasible space. Further, during the path tree construction process, when a node in the path tree coincides with the target position, nodes are gradually added. After the added node's position coincides with the target position, multiple first child nodes between the target position and the starting position are obtained in the path tree. Then, the target path is generated based on these multiple first child nodes. This invention combines a mathematical model with the RRT* (Rapidly-exploring RandomTrees Star) algorithm. Compared to the traditional RRT* algorithm which uses a mass model for path planning, using a mathematical model can effectively describe the ship's turning inertia, and the planned reference points are closer to the ship's actual path, meeting the planning requirements.
[0008] In some embodiments, optionally, the step of constructing a path tree based on a mathematical model and a starting position includes: taking the starting position as the root node of the path tree, setting a second child node at an adjacent position of the root node using the mathematical model; obtaining a third child node that is closest to the second child node; obtaining a fourth child node along the direction between the second and third child nodes with a distance value of a first distance value from the third child node; detecting whether the path between the third and fourth child nodes intersects with the obstacle range; and, based on the absence of intersection, obtaining a fifth child node with a distance value of the first distance value from the fourth child node, and detecting whether the path between the fourth and fifth child nodes intersects with the obstacle range, until the target position is reached.
[0009] In this embodiment, the steps of constructing a path tree based on a mathematical model and a starting position include: first, using the starting position as the root node of the path tree, and then starting a spatial search. Specifically, a second child node is randomly set at an adjacent position of the root node using the mathematical model. Further, the third child node closest to the second child node is obtained, and a fourth child node with a distance of a first distance value from the third child node is obtained along the direction between the second and third child nodes. The first distance value can be set arbitrarily. Further, it is detected whether the path between the third and fourth child nodes intersects with the obstacle range, i.e., whether the path between the third and fourth child nodes collides with the obstacle range. If there is no collision, a spatial search expansion is successfully completed. The above process is repeated, i.e., a fifth child node with a distance of the first distance from the fourth child node is obtained, and whether the path between the fifth and fourth child nodes collides with the obstacle range. This process is repeated until the mathematical model reaches the target position. However, if there is a collision, it means that the path is not feasible, so the second child node needs to be reset in a different direction. The above method can minimize the path cost of newly generated nodes, and at the same time reduce redundant paths and path costs in the random tree after generating new nodes.
[0010] In some embodiments, optionally, before the step of constructing a path tree based on a mathematical model and a starting position, the method further includes: obtaining the state of the obstacle; when the state of the obstacle is stationary, increasing the size of the obstacle and using the increased size of the obstacle as the obstacle range; when the state of the obstacle is in motion, predicting the path range of the obstacle and using the path range as the obstacle range.
[0011] In this embodiment, before constructing the path tree based on the mathematical model and starting position, the method further includes: determining the obstacle range. Specifically, first, the state of the obstacle is obtained, i.e., whether the obstacle is moving or fixed. When the obstacle is stationary, i.e., fixed, the size of the static obstacle can be appropriately enlarged, and the area near the obstacle can also be identified as the obstacle range, thereby ensuring sufficient safety margin in subsequent path smoothing and tracking. When the obstacle is moving, i.e., moving, the movement path of the obstacle can be predicted, and the area within the prediction range can be defined as the obstacle range, thereby ensuring that the planned path will not collide with the obstacle range.
[0012] In some embodiments, the path planning method may optionally further include: modifying the curvature of the target path according to an optimization algorithm to make the target path smooth.
[0013] In this embodiment, the path planning method further includes: the solution result of the RRT* algorithm is a series of reference points, but due to the complexity of the guidance function, the generated target path may have excessive curvature. To ensure smooth ship movement during tracking, a smoothing operation can be performed on the generated target path. Specifically, traditional optimization algorithms such as gradient descent can be used to solve this optimization problem, thereby generating a new smooth target path that meets the curvature requirements.
[0014] According to a second aspect of the present invention, a path planning system is provided. The path planning system includes: a first acquisition module for acquiring a starting position and a target position; a construction module for constructing a path tree based on a mathematical model and the starting position; a second acquisition module for acquiring multiple first child nodes between the starting position and the target position based on the overlap of nodes in the path tree with the target position; and a generation module for generating a target path based on the multiple first child nodes.
[0015] The path planning system provided by this invention mainly includes: a first acquisition module, a construction module, a second acquisition module, and a generation module. First, the first acquisition module acquires the ship's starting position and its desired target position. Then, the construction module constructs a path tree using a mathematical model and the starting position. Specifically, the data model can be a unicycle model. Dynamic constraints based on the unicycle model are added during the path tree construction process to ensure that the path tree is as close as possible to the feasible space. Further, during the path tree construction process, when a node in the path tree coincides with the target position, the second acquisition module acquires multiple first child nodes between the target position and the starting position in the path tree. Then, the generation module generates the target path based on these multiple first child nodes. This invention combines a mathematical model with the RRT* algorithm. Compared to the traditional RRT* algorithm which uses a mass model for path planning, the mathematical model effectively describes the ship's turning inertia, resulting in a more accurate reference point that closely approximates the ship's actual path and meets the planning requirements.
[0016] In some embodiments, optionally, the construction module includes: a setting module, which is used to set a second child node at an adjacent position of the root node using a mathematical model, with the starting position as the root node; a third acquisition module, which is used to acquire the third child node that is closest to the second child node; a fourth acquisition module, which is used to acquire a fourth child node along the direction between the second and third child nodes with a distance value of a first distance value from the third child node; a detection module, which is used to detect whether the path between the third and fourth child nodes intersects with the obstacle range; and a fifth acquisition module, which is used to acquire a fifth child node with a distance value of the first distance value from the fourth child node if there is no intersection, and to detect whether the path between the fourth and fifth child nodes intersects with the obstacle range, until the target position is reached.
[0017] In this embodiment, the construction module includes a setting module, a third acquisition module, a fourth acquisition module, a detection module, and a fifth acquisition module. First, the setting module sets the starting position as the root node of the path tree and then begins a spatial search. Specifically, it uses a mathematical model to randomly set a second child node at an adjacent position to the root node. Further, the third acquisition module acquires the third child node that is closest to the second child node, and simultaneously, along the direction between the second and third child nodes, the fourth acquisition module acquires a fourth child node whose distance to the third child node is a first distance value, where the first distance value can be set arbitrarily. Further, the detection module detects whether the path between the third and fourth child nodes intersects with the obstacle range, i.e., whether the path between the third and fourth child nodes collides with the obstacle range. If there is no collision, a spatial search expansion is successfully completed. The above process is repeated, i.e., the fifth acquisition module acquires a fifth child node whose distance to the fourth child node is the first distance, and determines whether the path between the fifth and fourth child nodes collides with the obstacle range, and so on until the mathematical model reaches the target position. However, if a collision occurs, it indicates that the path is not feasible, so the second child node needs to be reset in a different direction. This method minimizes the path cost of the newly generated node and reduces redundant paths in the resulting random tree, further lowering the path cost.
[0018] In some embodiments, the path planning system may optionally include: a judgment module, which is used to obtain the state of the obstacle; an expansion module, which is used to expand the size of the obstacle when the obstacle is in a stationary state, and use the expanded obstacle as the obstacle range; and a prediction module, which is used to predict the path range of the obstacle when the obstacle is in a moving state, and use the path range as the obstacle range.
[0019] In this embodiment, the path planning system further includes a judgment module, an expansion module, and a prediction module. Before construction, the obstacle range needs to be determined. Specifically, the judgment module first obtains the state of the obstacle, i.e., whether it is moving or stationary. When the obstacle is stationary, the expansion module can appropriately enlarge the size of the static obstacle, identifying the area near the obstacle as part of the obstacle range, thus ensuring sufficient safety margin during subsequent path smoothing and tracking. When the obstacle is moving, the prediction module can predict its movement path and define the area within the prediction range as the obstacle range, ensuring that the planned path will not collide with the obstacle range.
[0020] In some embodiments, the path planning system may optionally further include a processing module, which is configured to modify the curvature of the target path according to an optimization algorithm to make the target path smooth.
[0021] In this embodiment, the path planning system further includes a processing module. The solution result of the RRT* algorithm is a series of reference points, but due to the complexity of the guidance function, the generated target path may have excessive curvature. To ensure smooth ship movement during tracking, a smoothing operation can be performed on the generated target path. Specifically, the processing module can use traditional optimization algorithms such as gradient descent to solve this optimization problem, thereby generating a new smooth target path that meets the curvature requirements.
[0022] According to a third aspect of the present invention, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the path planning method in the first aspect or any possible implementation thereof.
[0023] According to a fourth aspect of the invention, a readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps of the path planning method in the first aspect or any possible implementation thereof.
[0024] The beneficial effects of this invention are as follows:
[0025] These improvements make path planning more adaptable to real-world application needs, enhancing path safety, smoothness, and adaptability. They have significant application value in fields such as autonomous navigation, robot motion planning, and autonomous driving.
[0026] Easily handles high-dimensional and complex spaces: The RRT* algorithm is suitable for handling high-dimensional configuration spaces and complex environments because it searches the solution space through random sampling and tree-structure expansion. Compared to other traditional path planning algorithms, the RRT* algorithm has better scalability and efficiency in high-dimensional spaces.
[0027] Fast convergence: The RRT* algorithm explores from the initial state to the target state by progressively expanding the tree structure. Since the direction of each expansion is random, the algorithm can quickly cover the entire search space and converge to the target state within a finite number of iterations.
[0028] Adaptability and Robustness: The RRT* algorithm adapts to different environments and obstacle configurations through random sampling and tree structure expansion. It can generate feasible paths in the presence of obstacles or complex environments and can adaptively adjust paths to avoid obstacles, improving the robustness and adaptability of the paths.
[0029] Scalability: The sampling and tree-structure expansion of the solution space by the RRT* algorithm enable it to handle large-scale problems. Whether it's robot motion planning or path planning in other fields, the RRT* algorithm can handle large-scale environments and find feasible paths in a short time.
[0030] Simple implementation and wide application: The RRT* algorithm is relatively simple to implement, easy to understand and implement. It has been widely used in fields such as robot navigation, autonomous driving, and drone path planning, and has achieved many successful application cases.
[0031] As can be seen from the above solutions, the embodiments of the present invention provide a path planning method, system, electronic device, and storage medium. Attached Figure Description
[0032] Figure 1 This is a flowchart illustrating one embodiment of a path planning method according to the present invention;
[0033] Figure 2 A second flowchart illustrating a path planning method according to an embodiment of the present invention;
[0034] Figure 3 The third flowchart illustrates a path planning method according to an embodiment of the present invention;
[0035] Figure 4 This diagram illustrates a unicycle model in a path planning method according to an embodiment of the present invention.
[0036] Figure 5 This diagram illustrates the RRT* algorithm in a path planning method according to an embodiment of the present invention.
[0037] Figure 6 This is one of the schematic diagrams of a guidance function in a path planning method according to an embodiment of the present invention;
[0038] Figure 7 This is a second schematic diagram of a guidance function in a path planning method according to an embodiment of the present invention.
[0039] Figure 8 One of the schematic block diagrams representing a path planning system according to an embodiment of the present invention;
[0040] Figure 9 A schematic block diagram illustrating a construction module in a path planning system according to an embodiment of the present invention;
[0041] Figure 10 A second schematic block diagram illustrating a path planning system according to an embodiment of the present invention;
[0042] Figure 11 A schematic block diagram (3) illustrating a path planning system according to an embodiment of the present invention;
[0043] Figure 12 This is a schematic block diagram illustrating an electronic device according to an embodiment of the present invention. Detailed Implementation
[0044] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.
[0045] Figure 1 This is a flowchart illustrating one embodiment of a path planning method according to the present invention; the method may include the following steps:
[0046] S102: Obtain the starting position and target position;
[0047] S104: Construct a path tree based on the mathematical model and the starting position;
[0048] S106: Based on the overlap between the nodes of the path tree and the target position, obtain multiple first child nodes between the starting position and the target position;
[0049] S108: Generate the target path based on multiple first child nodes.
[0050] The path planning method provided by this invention specifically includes: first, obtaining the ship's starting position and the target position the ship wants to reach; then, constructing a path tree using a mathematical model and the starting position. Specifically, the mathematical model can be a unicycle model. Dynamic constraints based on the unicycle model are added during the path tree construction process to ensure that the path tree is as close as possible to the feasible space. Further, during the path tree construction process, when a node in the path tree coincides with the target position, multiple first child nodes between the target position and the starting position are obtained in the path tree, and then the target path is generated based on these multiple first child nodes. This invention combines a mathematical model with the RRT* algorithm. Compared to the traditional RRT* algorithm which uses a mass model for path planning, using a mathematical model can effectively describe the ship's turning inertia, and the planned reference points are closer to the ship's actual path, meeting the planning requirements.
[0051] Furthermore, such as Figure 4 As shown, a schematic diagram of a unicycle model is presented, from which we can obtain...
[0052]
[0053] Where x and y represent the ship's position in the absolute coordinate system, and θ represents the ship's deflection angle. This represents the ship's propulsion speed and angular velocity. Compared to the traditional RRT* algorithm, which uses a point mass model for path planning, using a mathematical model can effectively describe the ship's steering inertia, and the planned reference points are closer to the ship's actual path, thus meeting the planning requirements.
[0054] Figure 2 The second flowchart illustrates a path planning method according to an embodiment of the present invention; wherein the step of constructing a path tree based on a mathematical model and a starting position includes:
[0055] S202: Take the starting position as the root node of the path tree, and use a mathematical model to set a second child node at an adjacent position of the root node.
[0056] S204: Get the third child node that is closest to the second child node;
[0057] S206: Obtain the fourth child node whose distance from the third child node is the first distance value, along the direction between the second and third child nodes;
[0058] S208: Detect whether the path between the third and fourth child nodes intersects with the obstacle range;
[0059] S210: If there is no intersection, obtain the fifth child node whose distance from the fourth child node is the first distance value, and check whether the path between the fourth child node and the fifth child node intersects with the obstacle range until the target position is reached.
[0060] In this embodiment, the steps of constructing a path tree based on a mathematical model and a starting position include: first, using the starting position as the root node of the path tree, and then starting a spatial search. Specifically, a second child node is randomly set at an adjacent position of the root node using the mathematical model. Further, the third child node closest to the second child node is obtained, and a fourth child node with a distance of a first distance value from the third child node is obtained along the direction between the second and third child nodes. The first distance value can be set arbitrarily. Further, it is detected whether the path between the third and fourth child nodes intersects with the obstacle range, i.e., whether the path between the third and fourth child nodes collides with the obstacle range. If there is no collision, a spatial search expansion is successfully completed. The above process is repeated, i.e., a fifth child node with a distance of the first distance from the fourth child node is obtained, and whether the path between the fifth and fourth child nodes collides with the obstacle range. This process is repeated until the mathematical model reaches the target position. However, if there is a collision, it means that the path is not feasible, so the second child node needs to be reset in a different direction. The above method can minimize the path cost of newly generated nodes, and at the same time reduce redundant paths and path costs in the random tree after generating new nodes.
[0061] Specifically, such as Figure 5 This diagram illustrates the RRT* algorithm, specifically the process of constructing a path tree. First, q... init It is the starting position, q goal This is the target location. The path space search process starts from the starting point, first randomly scattering points q. rand Then find the distance q. rand The nearest node q near Then along q near to q rand The distance q is obtained by moving forward in the direction by Step Size (change). new CollisionFree(M,E) i Method to detect Edge(q) new ,q near ) Whether there is a collision with obstacles in the map environment, where CollisionFree() represents the no-collision function, M and E i This represents a node, Edge(q) new ,q near ) refers to q new With q near The connection between them, that is, the detection of q using a collision-free function. new With q nearCheck if the connecting lines collide with obstacles in the map environment. If there is no collision, a spatial search expansion is successfully completed. Repeat the above process until the target location is reached.
[0062] Furthermore, such as Figure 6 As shown, Figure 6 This is a schematic diagram of the Steer (guided) function. In this invention, to ensure the generation of a new reference point q... new To satisfy the motion path of the unicycle model, the Steer function (i.e., the guiding function) needs to be modified, such as... Figure 6 As shown, due to constraints such as the turning radius, maneuverability, and related performance limits of the unmanned surface vessel, the accessibility of the unicycle can be determined by calculating the guiding function, thereby limiting q during the planning process. new The unordered expansion of q makes new The reference points are concentrated only near the reachability, thus ensuring that the generated reference points satisfy kinematic constraints, turning radius constraints, and performance constraints.
[0063] Figure 3 This is a third flowchart illustrating a path planning method according to an embodiment of the present invention; wherein, before the step of constructing a path tree based on a mathematical model and a starting position, the method further includes:
[0064] S302: Obtain the status of the obstacle;
[0065] S304: When the obstacle is in a stationary state, increase the size of the obstacle and use the increased size of the obstacle as the obstacle range;
[0066] S306: When the obstacle is in motion, predict the path range of the obstacle and use the path range as the obstacle range.
[0067] In this embodiment, before constructing the path tree based on the mathematical model and starting position, the method further includes: determining the obstacle range. Specifically, first, the state of the obstacle is obtained, i.e., whether the obstacle is moving or stationary. When the obstacle is stationary, i.e., in a fixed state, the size of the static obstacle can be appropriately enlarged, and the area near the obstacle can also be identified as the obstacle range, thereby ensuring sufficient safety margin in subsequent path smoothing and tracking. When the obstacle is moving, i.e., in a moving state, the movement path of the obstacle can be predicted, and the area within the prediction range can be defined as the obstacle range, thereby ensuring that the planned path will not collide with the obstacle range.
[0068] Furthermore, the path planning method also includes modifying the curvature of the target path according to the optimization algorithm to make the target path smooth.
[0069] In this embodiment, the path planning method further includes: the solution result of the RRT* algorithm is a series of reference points, but due to the complexity of the guidance function, the generated target path may have excessive curvature. To ensure smooth ship movement during tracking, a smoothing operation can be performed on the generated target path. Specifically, traditional optimization algorithms such as gradient descent can be used to solve this optimization problem, thereby generating a new smooth reference path that meets the curvature requirements. Specifically, such as... Figure 7 As shown, where P0(x0,y0), P K (x k ,y k ) and P n (x n ,y n Let x0 and x' be the coordinates on the path. k and x n Let y0 and y1 be the x-coordinates. k and y n Let x be the ordinate. The path smoothing problem can be viewed as an optimization problem of a series of new reference points, i.e., given (x... i,ref ,y i,ref ) and solution variables (x) i ,y i We have the following objective function:
[0070]
[0071] Where cost1 is the curvature cost, describing the degree of collinearity of three adjacent points; cost2 is the density cost, describing the distance between two adjacent points; cost3 is the distance cost, describing the distance between each point and the original reference point; x and y are the x and y coordinates of the path points, respectively; x i-1 y i11 x represents the x-coordinate and y-coordinate of the (i-1)th path point; i y i Let x be the x and y coordinates of the i-th path point; i-ref y i-ref Let x be the x and y coordinates of the i-th reference point. i+1 and y i+1 Let x and y be the x and y coordinates of the (i+1)th path point. The constraints of the optimization problem are as follows:
[0072]
[0073] Where x and y are the x-coordinate and y-coordinate of the path point, respectively. i-1 y i-1 x represents the x-coordinate and y-coordinate of the (i-1)th path point; i y iLet x be the x and y coordinates of the i-th path point; i-ref y i-ref Let x be the x and y coordinates of the i-th reference point. i+1 and y i+1 Let cur be the x and y coordinates of the (i+1)th path point. cstr This represents the maximum constraint, Δs represents the distance between two path points, and stack... i x represents the curvature of the i-th path point. u and y u Here are the x and y coordinates of the reference point. In the constraints, the first two constraints are positional constraints on the new reference point, ensuring it lies between the original reference points; the latter two constraints are curvature constraints, guaranteeing that the new reference path satisfies the maximum curvature constraint cur. cstr Therefore, we can use traditional optimization algorithms such as gradient descent to solve this optimization problem, thereby generating a new smooth target path that meets the curvature requirement.
[0074] Figure 8 A schematic block diagram illustrating one embodiment of a path planning system according to the present invention; wherein, the path planning system 80 includes:
[0075] The first acquisition module 802 is used to acquire the starting position and the target position;
[0076] Module 804 is used to construct a path tree based on a mathematical model and the starting position.
[0077] The second acquisition module 806 is used to acquire multiple first child nodes between the starting position and the target position based on the overlap of the nodes in the path tree with the target position;
[0078] The generation module 808 is used to generate a target path based on multiple first child nodes.
[0079] The path planning system 80 provided by this invention mainly includes: a first acquisition module 802, a construction module 804, a second acquisition module 806, and a generation module 808. First, the first acquisition module 802 acquires the ship's starting position and the target position the ship wants to reach. Then, the construction module 804 constructs a path tree using a mathematical model and the starting position. Specifically, dynamic constraints based on the mathematical model are added during the path tree construction process to ensure that the path tree is as close as possible to the feasible space. Further, during the path tree construction process, when a node in the path tree coincides with the target position, the second acquisition module 806 acquires multiple first child nodes between the target position and the starting position in the path tree. Then, the generation module 808 generates the target path based on these multiple first child nodes. This invention combines a mathematical model with the RRT* algorithm. Compared to the traditional RRT* algorithm which uses a mass model for path planning, the mathematical model effectively describes the ship's turning inertia, and the planned reference points are closer to the ship's actual path, meeting the planning requirements.
[0080] Furthermore, Figure 9 A schematic block diagram illustrating a construction module in a path planning system according to an embodiment of the present invention; wherein, the construction module 804 includes:
[0081] The module 8042 is used to set the starting position as the root node of the path tree and to set the second child node at the adjacent position of the root node using a mathematical model.
[0082] The third acquisition module 8044 is used to acquire the third child node that is closest to the second child node;
[0083] The fourth acquisition module 8046 is used to acquire the fourth child node whose distance from the third child node is a first distance value along the direction between the second child node and the third child node;
[0084] The detection module 8048 is used to detect whether the path between the third child node and the fourth child node intersects with the obstacle range;
[0085] The fifth acquisition module 8050 is used to acquire the fifth child node whose distance to the fourth child node is the first distance value if there is no intersection, and to detect whether the path between the fourth child node and the fifth child node intersects with the obstacle range until the target position is reached.
[0086] In this embodiment, the construction module 804 includes: a setting module 8042, a third acquisition module 8044, a fourth acquisition module 8046, a detection module 8048, and a fifth acquisition module 8050. First, the setting module 8042 sets the starting position as the root node of the path tree and then begins a spatial search. Specifically, it randomly sets a second child node at an adjacent position to the root node using a mathematical model. Further, the third acquisition module 8044 acquires the third child node that is closest to the second child node, and the fourth acquisition module 8046 acquires a fourth child node at a distance of a first distance value from the third child node along the direction between the second and third child nodes. The first distance value can be set independently. Further, the detection module 8048 detects whether the path between the third and fourth child nodes intersects with the obstacle range, i.e., it determines whether the path between the third and fourth child nodes collides with the obstacle range. If there is no collision, a spatial search expansion is successfully completed. The above process is repeated: the fifth acquisition module 8050 acquires the fifth child node whose distance to the fourth child node is the first distance; it then determines whether the path between the fifth and fourth child nodes collides with the obstacle range. This process is repeated until the mathematical model reaches the target position. However, if a collision occurs, it means the path is unacceptable, and the second child node needs to be reset in a different direction. This method minimizes the path cost of newly generated nodes and reduces redundant paths in the random tree after generating new nodes, thus reducing path cost.
[0087] Furthermore, Figure 10 A second schematic block diagram illustrating a path planning system according to an embodiment of the present invention; wherein, the path planning system 80 further includes:
[0088] The judgment module 810 is used to obtain the state of the obstacle;
[0089] The enlargement module 812 is used to enlarge the size of the obstacle when the obstacle is in a stationary state, and to use the enlarged obstacle as the obstacle range;
[0090] The prediction module 814 is used to predict the path range of the obstacle when the obstacle is in motion, and to use the path range as the obstacle range.
[0091] In this embodiment, the path planning system 80 further includes a judgment module 810, an expansion module 812, and a prediction module 814. Before constructing the path, the obstacle range needs to be determined. Specifically, the judgment module 810 first determines the state of the obstacle, i.e., whether it is moving or stationary. When the obstacle is stationary, the expansion module 812 can appropriately enlarge the size of the static obstacle, identifying the area near the obstacle as part of the obstacle range, thus ensuring sufficient safety margin during subsequent path smoothing and tracking. When the obstacle is moving, the prediction module 814 can predict the obstacle's movement path and define the area within the prediction range as the obstacle range, ensuring that the planned path will not collide with the obstacle range.
[0092] Furthermore, Figure 11 A third schematic block diagram illustrating a path planning system according to an embodiment of the present invention; wherein, the path planning system 80 further includes:
[0093] The processing module 816 is used to modify the curvature of the target path according to the optimization algorithm to make the target path smooth.
[0094] In this embodiment, the path planning system 80 further includes a processing module 816. The solution result of the RRT* algorithm is a series of reference points, but due to the complexity of the guidance function, the generated target path may have excessive curvature. To ensure smooth ship movement during tracking, a smoothing operation can be performed on the generated target path. Specifically, the processing module 816 can use traditional optimization algorithms such as gradient descent to solve this optimization problem, thereby generating a new smooth target path that meets the curvature requirements.
[0095] Figure 12 This is a schematic block diagram illustrating an electronic device according to an embodiment of the present invention. The electronic device 120 includes a memory 1202, a processor 1204, and a computer program stored in the memory and executable on the processor. When the processor 1204 executes the program, it implements the steps of the path planning method in the first aspect or any possible implementation of the first aspect.
[0096] According to a fourth aspect of the invention, a readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps of the path planning method in the first aspect or any possible implementation thereof.
[0097] The above are preferred embodiments of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.
Claims
1. A path planning method, characterized in that, include: Obtain the starting position and the target position; Construct a path tree based on the mathematical model and the starting position; Based on the fact that the nodes of the path tree coincide with the target position, obtain multiple first child nodes between the starting position and the target position; Generate a target path based on multiple first child nodes.
2. The path planning method according to claim 1, characterized in that, The step of constructing a path tree based on the mathematical model and the starting position includes: The starting position is taken as the root node of the path tree, and a second child node is set at an adjacent position of the root node using the mathematical model. Find the third child node that is closest to the second child node; Obtain a fourth child node whose distance from the third child node is a first distance value, along the direction between the second child node and the third child node; Detect whether the path between the third child node and the fourth child node intersects with the obstacle range; Since there is no intersection, a fifth child node with a distance value of the first distance value from the fourth child node is obtained, and it is detected whether the path between the fourth child node and the fifth child node intersects with the obstacle range, until the target position is reached.
3. The path planning method according to claim 2, characterized in that, Before the step of constructing the path tree based on the mathematical model and the starting position, the method further includes: Obtain the status of obstacles; When the obstacle is in a stationary state, the size of the obstacle is increased, and the enlarged obstacle is taken as the obstacle range. When the obstacle is in motion, predict the path range of the obstacle and use the path range as the obstacle range.
4. The path planning method according to any one of claims 1 to 3, characterized in that, Also includes: The curvature of the target path is modified according to the optimization algorithm to make the target path smooth.
5. A path planning system, characterized in that, include: A first acquisition module is used to acquire the starting position and the target position; Construction module, the construction module is used to construct a path tree based on the mathematical model and the starting position; The second acquisition module is used to acquire multiple first child nodes between the starting position and the target position based on the overlap between the nodes of the path tree and the target position; A generation module is used to generate a target path based on multiple first child nodes.
6. The path planning system according to claim 5, characterized in that, The construction module includes: The setting module is used to set the starting position as the root node of the path tree and to set a second child node at an adjacent position of the root node using the mathematical model. The third acquisition module is used to acquire the third child node that is closest to the second child node; The fourth acquisition module is used to acquire a fourth child node whose distance from the third child node is a first distance value along the direction between the second child node and the third child node; The detection module is used to detect whether the path between the third child node and the fourth child node intersects with the obstacle range; The fifth acquisition module is used to acquire a fifth child node whose distance from the fourth child node is the first distance value if there is no intersection, and to detect whether the path between the fourth child node and the fifth child node intersects with the obstacle range, until the target position is reached.
7. The path planning system according to claim 6, characterized in that, Also includes: The judgment module is used to obtain the state of the obstacle; An expansion module is used to expand the size of the obstacle when the obstacle is in a stationary state, and to use the expanded obstacle as the obstacle range. The prediction module is used to predict the path range of the obstacle when the obstacle is in motion, and to use the path range as the obstacle range.
8. The path planning system according to any one of claims 5 to 7, characterized in that, Also includes: The processing module is used to modify the curvature of the target path according to the optimization algorithm to make the target path smooth.
9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the path planning method as described in any one of claims 1 to 4.
10. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the path planning method as described in any one of claims 1 to 4.