Path planning method and system, electronic equipment and storage medium
By combining mathematical models and RRT* algorithms, the path tree is constructed and dynamic constraints and obstacle states are considered, the problem of poor performance in unmanned boat path planning is solved, and the generated path is closer to actual needs, improving the safety and adaptation of the path sex.
Patent Information
- Application Number
- CN202411160118.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-22
- Publication Date
- 2024-12-03
AI Technical Summary
The autonomous path planning performance of unmanned boats is poor and fails to effectively consider kinematic constraints, resulting in the planned path not being optimized and the driving of the planned path cannot be driven according to the planned path.
Using a combination of mathematical model and RRT* algorithm, a path tree is constructed and dynamic constraints are added. By gradually increasing nodes to generate target paths, considering obstacle state and steering inertia, a unicycle model is used to describe the ship's motion, and the path curvature is optimized.
The generated path is closer to the real path of the ship, enhancing the safety, smoothness and adaptability of the path, suitable for high-dimensional and complex spaces, and improving the robustness and adaptability of path planning.
Smart Images

Figure CN119063728A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of ship technology and navigation control, and in particular relates to a path planning method, a path planning system, an electronic device and a storage medium. Background Art
[0002] Among the relevant sampling-based path planning technologies, the autonomous path planning performance of the unmanned boat is poor. The planned path does not take into account the kinematic constraints of the unmanned boat, and cannot guarantee that the feasible path obtained is relatively optimized, so the unmanned boat cannot travel according to the planned path. Summary of the invention
[0003] Embodiments of the present invention provide a path planning method, a path planning system, an electronic device and a storage medium.
[0004] Specifically, the present invention is achieved through the following technical solutions:
[0005] 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 according to a mathematical model and the starting position; obtaining a plurality of first sub-nodes between the starting position and the target position based on the coincidence of the nodes of the path tree with the target position; and generating a target path according to the plurality of first sub-nodes.
[0006] The path planning method provided by the present invention specifically includes: first obtaining the starting position of the ship and the target position that the ship wants to reach, and then constructing a path tree using a mathematical model and the starting position. Specifically, the mathematical model can be a wheelbarrow model, and the dynamic constraints based on the wheelbarrow model are added during the construction of the path tree, so as to ensure that the path tree is as close to the feasible space as possible during the construction process. Further, in the process of constructing the path tree, when the nodes of the path tree coincide with the target position, that is, in the process of constructing the path tree, the nodes of the path tree will be gradually increased, and after the positions of the increased nodes coincide 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 according to the multiple first child nodes. The present invention combines the mathematical model with the RRT* (Rapidly-exploring RandomTrees Star*) algorithm, so that compared with the traditional RRT* algorithm using the particle model for path planning, the use of the mathematical model can effectively describe the steering inertia of the ship, and the planned reference point is closer to the actual path of the ship, meeting the planning requirements.
[0007] In some embodiments, optionally, the step of constructing a path tree according to a mathematical model and a starting position includes: taking the starting position as the root node of the path tree, and setting a second subnode at an adjacent position of the root node using a mathematical model; obtaining a third subnode that is closest to the second subnode; obtaining a fourth subnode whose distance value to the third subnode is a first distance value along the direction of the second subnode and the third subnode; detecting whether the path between the third subnode and the fourth subnode intersects with the obstacle range; based on the absence of intersection, obtaining a fifth subnode whose distance value to the fourth subnode is the first distance value, and detecting whether the path between the fourth subnode and the fifth subnode intersects with the obstacle range, until the target position is reached.
[0008] In this embodiment, the step of constructing a path tree according to the mathematical model and the starting position includes: first, taking the starting position as the root node of the path tree, and then starting a spatial search, specifically, using the mathematical model to randomly set the second subnode at the adjacent position of the root node. Further, obtain the third subnode closest to the second subnode, and obtain the fourth subnode whose distance from the third subnode is the first distance value along the direction of the second subnode and the third subnode, wherein the first distance value can be set independently. Further, detect whether the path between the third subnode and the fourth subnode intersects with the obstacle range, that is, determine whether the path between the third subnode and the fourth subnode collides with the obstacle range, and if there is no collision, a spatial search expansion will be successfully completed. Repeat the above process, that is, obtain the fifth subnode whose distance value from the fourth subnode is the first distance, determine whether the path between the fifth subnode and the fourth subnode collides with the obstacle range, and repeat until the mathematical model reaches the target position. However, if there is a collision, it means that the path is not possible, so it is necessary to change the direction and reset the second subnode. The above method can make the path cost of the newly generated node as small as possible, and at the same time, the random tree after generating the new node can reduce redundant paths and reduce the path cost.
[0009] In some embodiments, optionally, before the step of constructing a path tree according to a mathematical model and a starting position, it also includes: obtaining the state of an obstacle; when the state of the obstacle is a stationary state, enlarging the size of the obstacle, and using the enlarged obstacle as the obstacle range; when the state of the obstacle is a moving state, predicting the path range of the obstacle, and using the path range as the obstacle range.
[0010] In this embodiment, before the step of constructing the path tree according to the mathematical model and the starting position, it also includes: determining the obstacle range, specifically, first obtaining the state of the obstacle, that is, determining whether the obstacle is moving or fixed. When the state of the obstacle is static, that is, the obstacle is 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, so as to ensure that there is sufficient safety margin in the subsequent path smoothing and tracking process. When the state of the obstacle is in a moving state, that is, the obstacle is in a moving state, the movement path of the obstacle can be predicted, and the area within the predicted range can be defined as the obstacle range, so as to ensure that the planned path will not collide with the obstacle range.
[0011] In some embodiments, optionally, the path planning method further includes: modifying the curvature of the target path according to an optimization algorithm to make the target path smooth.
[0012] In this embodiment, the path planning method further includes: the solution result of the RRT* algorithm is a series of reference points, and due to the complexity of the guidance function, the generated target path may have a problem of excessive curvature. In order to ensure the smooth operation of the ship during the tracking process, the generated target path can be smoothed. Specifically, the optimization problem can be solved using traditional optimization algorithms such as the gradient descent method, thereby generating a new smooth target path that meets the curvature requirements.
[0013] According to a second aspect of the present invention, a path planning system is provided. The path planning system comprises: a first acquisition module, the first acquisition module is used to acquire a starting position and a target position; a construction module, the construction module is used to construct a path tree according to a mathematical model and the starting position; a second acquisition module, the second acquisition module is used to acquire a plurality of first sub-nodes between the starting position and the target position based on the coincidence of the nodes of the path tree with the target position; and a generation module, the generation module is used to generate a target path according to the plurality of first sub-nodes.
[0014] The path planning system provided by the present invention mainly includes: a first acquisition module, a construction module, a second acquisition module and a generation module. Among them, first, the first acquisition module acquires the starting position of the ship and the target position that the ship wants to reach, and then the construction module constructs a path tree using a mathematical model and the starting position. Specifically, the data model can be a wheelbarrow model, and the dynamic constraints based on the wheelbarrow model are added during the construction of the path tree, so as to ensure that the path tree is as close to the feasible space as possible during the construction process. Further, in the process of constructing the path tree, when the node of the path tree coincides with the target position, the second acquisition module acquires multiple first sub-nodes between the target position and the starting position in the path tree, and then the generation module generates the target path according to the multiple first sub-nodes. The present invention combines the mathematical model with the RRT* algorithm, so that compared with the traditional RRT* algorithm using the particle model for path planning, the use of the mathematical model can effectively describe the steering inertia of the ship, and the planned reference point is closer to the actual path of the ship, meeting the planning requirements.
[0015] In some embodiments, optionally, the construction module includes: a setting module, the setting module is used to take the starting position as the root node of the path tree, and use a mathematical model to set a second subnode at an adjacent position of the root node; a third acquisition module, the third acquisition module is used to acquire a third subnode that is closest to the second subnode; a fourth acquisition module, the fourth acquisition module is used to acquire a fourth subnode whose distance value to the third subnode is a first distance value along the direction of the second subnode and the third subnode; a detection module, the detection module is used to detect whether the path between the third subnode and the fourth subnode intersects with the obstacle range; a fifth acquisition module, the fifth acquisition module is used to acquire a fifth subnode whose distance value to the fourth subnode is the first distance value based on the absence of intersection, and detect whether the path between the fourth subnode and the fifth subnode intersects with the obstacle range until the target position is reached.
[0016] 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. Among them, the setting module first takes the starting position as the root node of the path tree, and then starts the spatial search. Specifically, the second sub-node is randomly set at the adjacent position of the root node using the mathematical model. Further, the third acquisition module obtains the third sub-node with the closest distance to the second sub-node, and at the same time, the fourth acquisition module along the direction of the second sub-node and the third sub-node obtains the fourth sub-node with a first distance value from the third sub-node, wherein the first distance value can be set independently. Further, the detection module detects whether the path between the third sub-node and the fourth sub-node intersects with the obstacle range, that is, determines whether the path between the third sub-node and the fourth sub-node collides with the obstacle range. If there is no collision, a spatial search expansion will be successfully completed. Repeat the above process, that is, the fifth acquisition module obtains the fifth sub-node with a distance value of the first distance from the fourth sub-node, and determines whether the path between the fifth sub-node and the fourth sub-node collides with the obstacle range, and repeats until the mathematical model reaches the target position. However, if there is a collision, it means that the path is not possible, so it is necessary to change the direction and reset the second child node. The above method can make the newly generated node path cost as small as possible, and at the same time, the random tree after generating the new node reduces redundant paths and reduces the path cost.
[0017] In some embodiments, optionally, the path planning system also includes: 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 based on the state of the obstacle being a stationary state, and use the enlarged obstacle as the obstacle range; and a prediction module, which is used to predict the path range of the obstacle based on the state of the obstacle being a moving state, and use the path range as the obstacle range.
[0018] In this embodiment, the path planning system also includes: a judgment module, an expansion module and a prediction module. Among them, it is also necessary to determine the obstacle range before construction. Specifically, the judgment module first obtains the state of the obstacle, that is, determines whether the obstacle is moving or fixed. When the state of the obstacle is static, that is, the obstacle is in a fixed state, the expansion module can appropriately expand the size of the static obstacle, and also identify the area near the obstacle as the obstacle range, so as to ensure that there is sufficient safety margin in the subsequent path smoothing and tracking process. When the state of the obstacle is in a moving state, that is, the obstacle is in a moving state, the prediction module can predict the movement path of the obstacle, and define the area within the prediction range as the obstacle range, so as to ensure that the planned path will not collide with the obstacle range.
[0019] In some embodiments, optionally, the path planning system further includes: a processing module, the processing module being configured to modify the curvature of the target path according to an optimization algorithm to make the target path smooth.
[0020] In this embodiment, the path planning system also includes: a processing module. The solution result of the RRT* algorithm is a series of reference points, and due to the complexity of the guidance function, the generated target path may have a problem of excessive curvature. In order to ensure the smooth operation of the ship during the tracking process, the generated target path can be smoothed. Specifically, the processing module can use traditional optimization algorithms such as gradient descent method to solve this optimization problem, thereby generating a new smooth target path that meets the curvature requirements.
[0021] According to a third aspect of the present invention, there is provided an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the path planning method in the first aspect or any possible implementation of the first aspect are implemented.
[0022] According to a fourth aspect of the present invention, there is provided a readable storage medium 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 of the first aspect.
[0023] The beneficial effects brought by the present invention are as follows:
[0024] The above improvements can make path planning more adaptable to practical application needs and enhance the safety, smoothness and adaptability of the path. They have important application value in realizing autonomous navigation, robot motion planning and autonomous driving.
[0025] Easy to handle 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 with other traditional path planning algorithms, the RRT* algorithm has better scalability and efficiency in high-dimensional spaces.
[0026] Fast convergence: The RRT* algorithm explores from the starting state to the target state by gradually 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 limited number of iterations.
[0027] Adaptability and robustness: The RRT* algorithm adapts to different environments and obstacle configurations through random sampling and expansion of the tree structure. It can generate feasible paths in the presence of obstacles or complex environments, and can adaptively adjust the path to avoid obstacles, improving the robustness and adaptability of the path.
[0028] Scalability: The sampling of the RRT* algorithm in the solution space and the expansion of the tree structure enable it to handle large-scale problems. Whether it is robot mobility planning or path planning in other fields, the RRT* algorithm can handle large-scale environments and find feasible paths in a shorter time.
[0029] Simple implementation and wide application: The RRT* algorithm is relatively simple to implement and easy to understand and implement. It has been widely used in robot navigation, autonomous driving, drone path planning and other fields, and has achieved many successful application cases.
[0030] It can be seen from the above solutions that the embodiments of the present invention provide a path planning method and system, an electronic device and a storage medium. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] Figure 1 A schematic diagram showing a flow chart of a path planning method according to an embodiment of the present invention;
[0032] Figure 2 A second flowchart diagram showing a path planning method according to an embodiment of the present invention;
[0033] Figure 3 A third flowchart diagram showing a path planning method according to an embodiment of the present invention;
[0034] Figure 4 A schematic diagram showing a unicycle model in a path planning method according to an embodiment of the present invention;
[0035] Figure 5 A schematic diagram of an RRT* algorithm in a path planning method according to an embodiment of the present invention is shown;
[0036] Figure 6 One of the schematic diagrams of a guidance function in a path planning method according to an embodiment of the present invention is shown;
[0037] Figure 7 A second schematic diagram showing a guidance function in a path planning method according to an embodiment of the present invention;
[0038] Figure 8 One of the schematic block diagrams of a path planning system according to an embodiment of the present invention;
[0039] Fig. 9 A schematic block diagram showing a construction module in a path planning system according to an embodiment of the present invention;
[0040] Fig.10 A second schematic block diagram showing a path planning system according to an embodiment of the present invention;
[0041] Fig.11A third schematic block diagram showing a path planning system according to an embodiment of the present invention;
[0042] Fig.12 A schematic block diagram of an electronic device according to an embodiment of the present invention is shown. DETAILED DESCRIPTION
[0043] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0044] Figure 1 A schematic diagram of a flow chart of a path planning method according to an embodiment of the present invention is shown; the method may include the following steps:
[0045] S102: Acquire the starting position and the target position;
[0046] S104: constructing a path tree according to the mathematical model and the starting position;
[0047] S106: based on the coincidence of the node of the path tree with the target position, obtaining a plurality of first child nodes between the starting position and the target position;
[0048] S108: Generate a target path according to the multiple first sub-nodes.
[0049] The path planning method provided by the present invention specifically includes: first obtaining the starting position of the ship and the target position that the ship wants to reach, and then constructing a path tree using a mathematical model and the starting position. Specifically, the mathematical model can be a wheelbarrow model, and the dynamic constraints based on the wheelbarrow model are added during the construction of the path tree, so as to ensure that the path tree is as close to the feasible space as possible during the construction process. Further, in the process of constructing the path tree, when the node of the path tree coincides with the target position, multiple first sub-nodes between the target position and the starting position are obtained in the path tree, and then the target path is generated according to the multiple first sub-nodes. The present invention combines the mathematical model with the RRT* algorithm, so that compared with the traditional RRT* algorithm using the particle model for path planning, the use of the mathematical model can effectively describe the steering inertia of the ship, and the planned reference point is closer to the actual path of the ship, meeting the planning requirements.
[0050] Furthermore, if Figure 4 As shown, a schematic diagram of a unicycle model is shown, and it can be obtained
[0051]
[0052] Among them, x and y represent the position of the ship in the absolute coordinate system, and θ represents the deviation angle of the ship. It represents the propulsion speed and angular velocity of the ship. Compared with the traditional RRT* algorithm that uses the particle model for path planning, the use of mathematical models can effectively describe the steering inertia of the ship, and the planned reference points are closer to the actual path of the ship and meet the planning requirements.
[0053] Figure 2 A second flow chart of a path planning method according to an embodiment of the present invention is shown; wherein the step of constructing a path tree according to a mathematical model and a starting position includes:
[0054] S202: taking the starting position as the root node of the path tree, and setting a second child node at an adjacent position of the root node using a mathematical model;
[0055] S204: Acquire the third child node that is closest to the second child node;
[0056] S206: Acquire a fourth subnode whose distance value to the third subnode is the first distance value along the direction between the second subnode and the third subnode;
[0057] S208: Detect whether the path between the third sub-node and the fourth sub-node intersects with the obstacle range;
[0058] S210: Based on the absence of intersection, a fifth subnode whose distance value to the fourth subnode is the first distance value is obtained, and whether the path between the fourth subnode and the fifth subnode intersects with the obstacle range is detected until the target position is reached.
[0059] In this embodiment, the step of constructing a path tree according to the mathematical model and the starting position includes: first, taking the starting position as the root node of the path tree, and then starting a spatial search, specifically, using the mathematical model to randomly set the second subnode at the adjacent position of the root node. Further, obtain the third subnode closest to the second subnode, and obtain the fourth subnode whose distance from the third subnode is the first distance value along the direction of the second subnode and the third subnode, wherein the first distance value can be set independently. Further, detect whether the path between the third subnode and the fourth subnode intersects with the obstacle range, that is, determine whether the path between the third subnode and the fourth subnode collides with the obstacle range, and if there is no collision, a spatial search expansion will be successfully completed. Repeat the above process, that is, obtain the fifth subnode whose distance value from the fourth subnode is the first distance, determine whether the path between the fifth subnode and the fourth subnode collides with the obstacle range, and repeat until the mathematical model reaches the target position. However, if there is a collision, it means that the path is not possible, so it is necessary to change the direction and reset the second subnode. The above method can make the path cost of the newly generated node as small as possible, and at the same time, the random tree after generating the new node can reduce redundant paths and reduce the path cost.
[0060] Specifically, Figure 5 The schematic diagram of the RRT* algorithm is shown, that is, the process of building a path tree. First, q init is the starting position, q goal 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 follow q near to q rand The distance of the Step Size (change amount) in the direction is q 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 a collision-free function, M and E i It represents the node, Edge(q new ,q near ) refers to q new With q near The connection between them, that is, the collision-free function is used to detect q new With q near Whether the line between them collides with obstacles in the map environment, if not, a spatial search expansion will be successfully completed. Repeat the above process until the target position is reached.
[0061] Furthermore, if Figure 6 As shown, Figure 6 is a schematic diagram of the Steer (guiding) function. In the present invention, in order to ensure the generation of a new reference point q new When , the motion path of the unicycle model is satisfied, so the Steer function (i.e., the guidance function) needs to be modified, such as Figure 6 As shown in Figure 1, due to the constraints of the turning radius, maneuverability and related performance limits of the unmanned boat, the accessibility of the unicycle can be determined by calculating the guidance function, thereby limiting the q in the planning process. new The disordered expansion of q new Concentrate only near reachability so that the resulting reference points satisfy kinematic constraints, turning radius constraints, and performance constraints.
[0062] Figure 3 A third flow chart of a path planning method according to an embodiment of the present invention is shown; wherein, before the step of constructing a path tree according to a mathematical model and a starting position, the method further includes:
[0063] S302: Obtain the state of the obstacle;
[0064] S304: when the state of the obstacle is a stationary state, enlarging the size of the obstacle, and using the enlarged obstacle as the obstacle range;
[0065] S306: When the state of the obstacle is a moving state, predict the path range of the obstacle, and use the path range as the obstacle range.
[0066] In this embodiment, before the step of constructing the path tree according to the mathematical model and the starting position, it also includes: determining the obstacle range, specifically, first obtaining the state of the obstacle, that is, determining whether the obstacle is moving or fixed. When the state of the obstacle is static, that is, the obstacle is 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, so as to ensure that there is sufficient safety margin in the subsequent path smoothing and tracking process. When the state of the obstacle is in a moving state, that is, the obstacle is in a moving state, the movement path of the obstacle can be predicted, and the area within the predicted range can be defined as the obstacle range, so as to ensure that the planned path will not collide with the obstacle range.
[0067] Furthermore, the path planning method also includes: modifying the curvature of the target path according to an optimization algorithm to make the target path smooth.
[0068] In this embodiment, the path planning method further includes: the solution result of the RRT* algorithm is a series of reference points, and due to the complexity of the guidance function, the generated target path may have a problem of excessive curvature. In order to ensure the smooth operation of the ship during the tracking process, the generated target path can be smoothed. Specifically, the traditional optimization algorithm such as the gradient descent method can be used to solve this optimization problem, thereby generating a new smooth reference path that meets the curvature requirements. Specifically, Figure 7 As shown, where P0(x0,y0), P K (x k ,y k ) and P n (x n ,y n ) are the coordinates on the path, x0, x k and x n is the horizontal coordinate, y0, y k and n The path smoothing problem can be regarded as an optimization problem for a series of new reference points. i,ref ,y i,ref ) and solve for the variable (x i ,y i ), we have the following objective function:
[0069]
[0070] Among them, cost1 is the curvature cost, which describes the degree of collinearity of three adjacent points; cost2 is the density cost, which describes the distance between two adjacent points; cost3 is the distance cost, which describes the distance between each point and the original reference point. x and y are the horizontal and vertical coordinates of the path point respectively; x i-1 ,y i-1 is the horizontal and vertical coordinates of the i-1th path point; x i ,y i is the horizontal and vertical coordinates of the i-th path point; x i-ref ,y i-ref is the horizontal and vertical coordinates of the i-th reference point, x i+1 and i+1 are the horizontal and vertical coordinates of the i+1th path point. The constraints of the optimization problem are as follows:
[0071]
[0072] Among them, x and y are the horizontal and vertical coordinates of the path point respectively. i-1 ,y i-1 is the horizontal and vertical coordinates of the i-1th path point; x i ,y i is the horizontal and vertical coordinates of the i-th path point; xi-ref ,y i-ref is the horizontal and vertical coordinates of the i-th reference point, x i+1 and i+1 is the horizontal and vertical coordinates of the i+1th path point, cur cstr represents the maximum constraint, Δs represents the distance between two path points, and stack i represents the curvature of the i-th path point, x u and u are the horizontal and vertical coordinates of the reference point. Among the constraints, the first two constraints are the position constraints of the new reference point, which are between the original reference points; the last two constraints are curvature constraints, which ensure that the new reference path satisfies the maximum curvature constraint cur cstr Based on this, 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 requirements.
[0073] Figure 8 One of the schematic block diagrams of a path planning system according to an embodiment of the present invention is shown; wherein the path planning system 80 includes:
[0074] A first acquisition module 802 is used to acquire a starting position and a target position;
[0075] A construction module 804, for constructing a path tree according to a mathematical model and a starting position;
[0076] A second acquisition module 806, configured to acquire a plurality of first sub-nodes between the starting position and the target position based on the coincidence of the nodes of the path tree with the target position;
[0077] The generating module 808 is used to generate a target path according to a plurality of first sub-nodes.
[0078] The path planning system 80 provided by the present invention mainly includes: a first acquisition module 802, a construction module 804, a second acquisition module 806 and a generation module 808. Among them, first, the first acquisition module 802 obtains the starting position of the ship and the target position that the ship wants to reach, and then the construction module 804 constructs a path tree using the mathematical model and the starting position. Specifically, in the process of constructing the path tree, dynamic constraints based on the mathematical model are added to ensure that the path tree is as close to the feasible space as possible during the construction process. Further, in the process of constructing the path tree, when the node of the path tree coincides with the target position, the second acquisition module 806 obtains multiple first sub-nodes between the target position and the starting position in the path tree, and then the generation module 808 generates the target path according to the multiple first sub-nodes. The present invention combines the mathematical model with the RRT* algorithm, so that compared with the traditional RRT* algorithm using the particle model for path planning, the use of the mathematical model can effectively describe the steering inertia of the ship, and the planned reference point is closer to the actual path of the ship, meeting the planning requirements.
[0079] Further, Fig. 9 A schematic block diagram showing a construction module in a path planning system according to an embodiment of the present invention; wherein the construction module 804 includes:
[0080] A setting module 8042, used to use the starting position as the root node of the path tree, and set a second child node at an adjacent position of the root node using a mathematical model;
[0081] A third acquisition module 8044 is used to acquire a third sub-node that is closest to the second sub-node;
[0082] A fourth acquisition module 8046 is configured to acquire, along a direction between the second subnode and the third subnode, a fourth subnode whose distance value to the third subnode is a first distance value;
[0083] A detection module 8048 is used to detect whether the path between the third sub-node and the fourth sub-node intersects with the obstacle range;
[0084] The fifth acquisition module 8050 is used to acquire a fifth subnode whose distance value to the fourth subnode is the first distance value based on the absence of intersection, and detect whether the path between the fourth subnode and the fifth subnode intersects with the obstacle range until reaching the target position.
[0085] 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. Among them, the setting module 8042 first takes the starting position as the root node of the path tree, and then starts the spatial search. Specifically, the second sub-node is randomly set at the adjacent position of the root node using a mathematical model. Further, the third acquisition module 8044 obtains the third sub-node closest to the second sub-node, and the fourth acquisition module 8046 along the direction of the second sub-node and the third sub-node obtains the fourth sub-node whose distance from the third sub-node is the first distance value, wherein the first distance value can be set independently. Further, the detection module 8048 detects whether the path between the third sub-node and the fourth sub-node intersects with the obstacle range, that is, determines whether the path between the third sub-node and the fourth sub-node collides with the obstacle range. If there is no collision, a spatial search expansion will be successfully completed. Repeat the above process, that is, the fifth acquisition module 8050 acquires the fifth subnode whose distance value to the fourth subnode is the first distance, and determines whether the path between the fifth subnode and the fourth subnode collides with the obstacle range, and repeats this process until the mathematical model reaches the target position. However, if there is a collision, it means that the path is not possible, so it is necessary to change the direction and reset the second subnode. The above method can make the newly generated node path cost as small as possible, and at the same time, the random tree after the new node is generated reduces redundant paths and reduces the path cost.
[0086] Further, Fig.10 A second schematic block diagram of a path planning system according to an embodiment of the present invention is shown; wherein the path planning system 80 further includes:
[0087] The judging module 810 is used to obtain the state of the obstacle;
[0088] An enlargement module 812, configured to enlarge the size of the obstacle when the obstacle is in a stationary state, and use the enlarged obstacle as the obstacle range;
[0089] The prediction module 814 is used to predict the path range of the obstacle when the state of the obstacle is in a moving state, and use the path range as the obstacle range.
[0090] In this embodiment, the path planning system 80 also includes: a judgment module 810, an expansion module 812 and a prediction module 814. Among them, it is also necessary to determine the obstacle range before construction. Specifically, the judgment module 810 first judges the state of the obstacle, that is, determines whether the obstacle is moving or fixed. When the state of the obstacle is static, that is, the obstacle is in a fixed state, the expansion module 812 can appropriately expand the size of the static obstacle and identify the area near the obstacle as the obstacle range, thereby ensuring that there is sufficient safety margin in the subsequent path smoothing and tracking process. When the state of the obstacle is in a moving state, that is, the obstacle is in a moving state, the prediction module 814 can predict the movement path of the obstacle, and define the area within the prediction range as the obstacle range, thereby ensuring that the planned path will not collide with the obstacle range.
[0091] Further, Fig.11 A third schematic block diagram of a path planning system according to an embodiment of the present invention is shown; wherein the path planning system 80 further includes:
[0092] 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.
[0093] 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, and due to the complexity of the guidance function, the generated target path may have a problem of excessive curvature. In order to ensure the smooth operation of the ship during the tracking process, the generated target path can be smoothed. Specifically, the processing module 816 can use traditional optimization algorithms such as the gradient descent method to solve this optimization problem, thereby generating a new smooth target path that meets the curvature requirements.
[0094] Fig.12 A schematic block diagram of an electronic device according to an embodiment of the present invention is shown, wherein the electronic device 120 includes a memory 1202, a processor 1204, and a computer program stored in the memory and executable on the processor, and when the processor 1204 executes the program, the steps of the path planning method in the first aspect or any possible implementation of the first aspect are implemented.
[0095] According to a fourth aspect of the present invention, there is provided a readable storage medium 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 of the first aspect.
[0096] The above are preferred embodiments of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.
Claims
1. A path planning method, It is characterized in that include: Get the starting position and target position; Constructing a path tree according to the mathematical model and the starting position; Based on the coincidence of the nodes of the path tree with the target position, acquiring a plurality of first sub-nodes between the starting position and the target position; A target path is generated according to the plurality of first sub-nodes.
2. The path planning method according to claim 1, It is characterized in that The step of constructing a path tree according to the mathematical model and the starting position comprises: Taking the starting position as the root node of the path tree, and setting a second child node at an adjacent position of the root node using the mathematical model; Obtain a third child node that is closest to the second child node; Acquire a fourth subnode whose distance value to the third subnode is a first distance value along a direction between the second subnode and the third subnode; Detecting whether the path between the third sub-node and the fourth sub-node intersects with the obstacle range; Based on the absence of intersection, a fifth subnode whose distance value to the fourth subnode is the first distance value is obtained, and it is detected whether the path between the fourth subnode and the fifth subnode intersects with the obstacle range until the target position is reached.
3. The path planning method according to claim 2, It is characterized in that Before the step of constructing the path tree according to the mathematical model and the starting position, the method further includes: Get the status of the obstacle; When the state of the obstacle is a stationary state, enlarging the size of the obstacle, and using the enlarged obstacle as the obstacle range; When the state of the obstacle is a moving state, a path range of the obstacle is predicted, and the path range is used as the obstacle range.
4. The path planning method according to any one of claims 1 to 3, It is characterized in that Also includes: The curvature of the target path is modified according to an optimization algorithm to make the target path smooth.
5. A path planning system, It is characterized in that include: A first acquisition module, wherein the first acquisition module is used to acquire a starting position and a target position; A construction module, the construction module is used to construct a path tree according to a mathematical model and the starting position; A second acquisition module, the second acquisition module is used to acquire a plurality of first sub-nodes between the starting position and the target position based on the coincidence of the nodes of the path tree with the target position; A generation module is used to generate a target path according to a plurality of the first sub-nodes.
6. The path planning system according to claim 5, It is characterized in that The construction module comprises: A setting module, the setting module is used to use the starting position as the root node of the path tree, and use the mathematical model to set a second child node at an adjacent position of the root node; A third acquisition module, configured to acquire a third sub-node that is closest to the second sub-node; A fourth acquisition module, the fourth acquisition module is used to acquire a fourth subnode whose distance value to the third subnode is a first distance value along a direction between the second subnode and the third subnode; A detection module, the detection module is used to detect whether the path between the third sub-node and the fourth sub-node intersects with the obstacle range; A fifth acquisition module, wherein the fifth acquisition module is used to acquire a fifth subnode whose distance value to the fourth subnode is the first distance value based on the absence of intersection, and detect whether the path between the fourth subnode and the fifth subnode intersects with the obstacle range until reaching the target position.
7. The path planning system according to claim 6, It is characterized in that Also includes: A judgment module, the judgment module is used to obtain the state of the obstacle; An expansion module, the expansion module is used to expand the size of the obstacle when the state of the obstacle is a stationary state, and use the obstacle after the enlargement as the obstacle range; A prediction module is used to predict the path range of the obstacle when the state of the obstacle is a moving state, and use the path range as the obstacle range.
8. A path planning system according to any one of claims 5 to 7, It is characterized in that Also includes: A processing module is used to modify the curvature of the target path according to an 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, It is characterized in that When the processor executes the computer program, the steps of the path planning method according to any one of claims 1 to 4 are implemented.
10. A storage medium having a computer program stored thereon, It is characterized in that When the computer program is executed by a processor, the steps of the path planning method according to any one of claims 1 to 4 are implemented.
Citation Information
Cited By
Dental implantation robot path generation method
CN121059322A
A dental implant robot path generation method
CN121059322B
