A method and system for ship dynamic path planning based on an improved DRRT algorithm

Abstract

This invention discloses a method and system for dynamic path planning of ships based on an improved DRRT algorithm, belonging to the field of ship automatic navigation technology. The method includes: acquiring two-dimensional planar environmental parameters of the ship's navigation area, defining the sea area boundary, collecting the position and contour data of static obstacles, collecting the state information of dynamic obstacles in real time, and determining the starting point and target point of the ship's path planning; employing an improved DRRT algorithm, determining sampling points through target and path point bias sampling, determining the expansion step size through adaptive step size adjustment, determining the path node set through path node pruning and reconnection, and generating an initial path through cubic B-spline smoothing; monitoring obstacles to determine if some path results are invalid, and if invalidity is found, replanning using the improved DRRT algorithm to obtain the dynamic path. This invention can meet the comprehensive requirements of real-time performance, safety, and path quality in intelligent ship navigation.

Description

Technical Field

[0001] This invention relates to the field of ship automatic navigation technology, specifically to a ship dynamic path planning method and system based on an improved DRRT algorithm. It is applicable to complex sea areas where static and dynamic obstacles coexist, and can be applied to the intelligent navigation systems of merchant ships, unmanned surface vessels, and other vessels, providing strong support for real-time obstacle avoidance and efficient path generation. Background Technology

[0002] With the rapid development of the marine economy, the navigation environment for ships is becoming increasingly complex. It includes not only static obstacles such as reefs, islands, and fixed fishing nets, but also dynamic obstacles such as other vessels and moving fishing nets. Furthermore, the uncertainty of environmental factors such as waves and ocean currents further increases the difficulty of path planning. Ship path planning must simultaneously meet the comprehensive requirements of real-time performance to cope with dynamic obstacle changes, safety (collision-free), and path quality to maintain short paths and low turning energy consumption. Traditional path planning algorithms are no longer adequate for these requirements.

[0003] Existing path planning algorithms have significant limitations: algorithms such as A*, Rapidly Exploring Random Tree (RRT), and others are only applicable to static environments. When dynamic obstacles or sudden changes in environmental parameters occur in the sea area, the path needs to be reconstructed from the starting point, leading to planning delays and failing to meet the requirements of real-time ship navigation. Although the traditional Dynamic Rapidly Exploring Random Tree (DRRT) algorithm supports dynamic environments, its sampling method is completely random sampling within the state space, which easily generates a large number of invalid branches and redundant nodes, resulting in low planning efficiency, such as a large number of iterations and long planning time. At the same time, this algorithm uses a fixed expansion step size, which can easily cause collision risks in areas with dense obstacles due to excessively large step sizes, while in open areas, the step size is too small, resulting in slow exploration speed. In addition, the path planned by the traditional DRRT algorithm consists of multiple straight segments, with large angle changes at turns and poor smoothness, which does not conform to the physical constraints of ship navigation and easily increases turning energy consumption and operational difficulty.

[0004] Against this backdrop, there is an urgent need to make targeted improvements to the DRRT algorithm to address its efficiency and path quality issues in ship dynamic path planning, so as to adapt to the intelligent navigation needs of ships in complex and dynamic sea areas. Summary of the Invention

[0005] An embodiment of the present invention provides a ship dynamic path planning method and system based on an improved DRRT algorithm to meet the comprehensive requirements of real-time, safety, and path quality for path planning during the intelligent navigation of ships, and provide a reliable path planning scheme for ships to sail safely and efficiently in waters where static and dynamic obstacles coexist.

[0006] In the first aspect, the present invention provides a ship dynamic path planning method based on an improved DRRT algorithm, including:

[0007] Obtain the two-dimensional plane environmental parameters of the ship navigation waters, clarify the sea area boundary range, collect the position and contour data of static obstacles, and collect the state information of dynamic obstacles in real time. At the same time, determine the starting point and target point of the ship path planning;

[0008] Adopt an improved DRRT algorithm, determine sampling points through target and path point offset sampling, determine the expansion step size through step size adaptive adjustment to generate an original initial path, determine the path node set through path node pruning and reconnecting based on the original initial path, and smooth the path node set through cubic B-spline to generate the final initial path;

[0009] Monitor obstacles to determine whether part of the path result fails. If there is a failure phenomenon, use the improved DRRT algorithm to re-plan the time-consuming part of the path to obtain a dynamic path.

[0010] In some examples, the determining sampling points through target and path point offset sampling includes:

[0011] Preset the target offset threshold k and the path point offset threshold c, and generate a random number rand() within the interval [0,1];

[0012] When rand () ≤ k, select the path target point q goal as the sampling point. When k < rand () ≤ k + c, select an effective path node not affected by dynamic obstacles as the sampling point. When rand () > k + c, randomly generate a sampling point q rand .

[0013] In some examples, the determining the expansion step size through step size adaptive adjustment to generate an original initial path includes:

[0014] Set the initial expansion step size ε and the step size adjustment amplitude α, and screen out the parent node q sample nearest to the sampling point q near from the current random tree T;

[0015] Taking q near as the starting point, and moving towards q sampleDirectional expansion generates a new node q new q is determined by the collision detection function. near With q new The connection between the two nodes and whether they intersect with the obstacle;

[0016] If the detection result is no collision, the next expansion step size will be updated to... Lt is the step size for this expansion;

[0017] If the detection result indicates a collision, the next expansion step size will be reset to the initial step size ε.

[0018] In some instances, determining the set of path nodes based on the original initial path through path node pruning and reconnection includes:

[0019] First, determine the original initial path node set containing the starting point, target point, and intermediate nodes. Then, initialize the trimmed node set, where the first node is the starting point.

[0020] Starting from the latest node in the pruned set, traverse the remaining nodes in the original initial path node set, perform straight-line connection and collision detection between each node to be detected and the starting point, and include the farthest node without collision into the pruned set.

[0021] Repeat the operation starting from the farthest node without collisions until the target point is included.

[0022] In some instances, the process of smoothly generating the final initial path by applying cubic B-splines to the set of path nodes includes:

[0023] Using the pruned node set as the control vertices, by Path fitting is performed using cubic B-spline curves, where P(u) represents the two-dimensional coordinates of the parameter u on the smooth curve. i To control the vertices, n+1 represents the total number of control vertices, and N... i,3 (u) is a cubic B-spline basis function.

[0024] Secondly, the present invention provides a ship dynamic path planning system based on an improved DRRT algorithm, comprising:

[0025] The parameter acquisition module is used to acquire two-dimensional planar environmental parameters of the ship's navigation area, define the sea area boundary, collect the position and outline data of static obstacles, collect the status information of dynamic obstacles in real time, and determine the starting point and target point of the ship's path planning.

[0026] The path planning module is used to determine sampling points by offset sampling of the target and path points using an improved DRRT algorithm, determine the expansion step size by adaptive step size adjustment to generate the original initial path, determine the path node set by pruning and reconnecting path nodes based on the original initial path, and smooth the path node set through cubic B-spline to generate the final initial path;

[0027] The path update module is used to monitor obstacles to determine whether part of the path result fails. If there is a failure, the improved DRRT algorithm is used to replan the time-consuming part of the path to obtain a dynamic path.

[0028] In some instances, the determination of sampling points by offset sampling of the target and path points includes:

[0029] Preset the target offset threshold k and the path point offset threshold c, and generate a random number rand() within the interval [0, 1];

[0030] When rand() ≤ k, select the path target point q goal as the sampling point. When k < rand() ≤ k + c, select the valid path node not affected by dynamic obstacles as the sampling point. When rand() > k + c, randomly generate a sampling point q rand .

[0031] In some instances, the determination of the expansion step size by adaptive step size adjustment to generate the original initial path includes:

[0032] Set the initial expansion step size ε and the step size adjustment amplitude α, and screen out the parent node q sample nearest to the sampling point q near from the current random tree T;

[0033] Taking q near as the starting point, expand in the direction of q sample at the current step size to generate a new node q new , and use the collision detection function to judge whether the connection line between q near and q new and the two nodes intersect with obstacles;

[0034] If the detection result is no collision, the next expansion step size is updated to , where Lt is the current expansion step size;

[0035] If the detection result is a collision, the next expansion step size is reset to the initial step size ε.

[0036] In some instances, the determination of the path node set by pruning and reconnecting path nodes based on the original initial path includes:

[0037] First, determine the original initial path node set containing the starting point, target point, and intermediate nodes. Then, initialize the trimmed node set, where the first node is the starting point.

[0038] Starting from the latest node in the pruned set, traverse the remaining nodes in the original initial path node set, perform straight-line connection and collision detection between each node to be detected and the starting point, and include the farthest node without collision into the pruned set.

[0039] Repeat the operation starting from the farthest node without collisions until the target point is included.

[0040] In some instances, the process of smoothly generating the final initial path by applying cubic B-splines to the set of path nodes includes:

[0041] Using the pruned node set as the control vertices, by Path fitting is performed using cubic B-spline curves, where P(u) represents the two-dimensional coordinates of the parameter u on the smooth curve. i To control the vertices, n+1 represents the total number of control vertices, and N... i,3 (u) is a cubic B-spline basis function.

[0042] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects:

[0043] (1) This invention uses the “target and path point bias sampling” strategy combined with threshold probability control to achieve dynamic switching of sampling points. Compared with traditional completely random sampling, it can reduce more than 30% of invalid branches, and can more flexibly adapt to marine environments of different complexity, significantly improving planning efficiency.

[0044] (2) This invention relies on the step size adaptive adjustment mechanism of collision feedback to dynamically balance obstacle avoidance accuracy and exploration speed, thereby improving planning efficiency by about 40% in complex environments, while controlling the collision risk to below 0.1%.

[0045] (3) This invention uses a two-stage process of “node trimming and reconnection + cubic B-spline smoothing” to shorten the path length by about 25%, reduce the turning curvature to ≤0.05rad / m, meet the ship turning constraints, reduce energy consumption by 22.6%, and improve navigation practicality.

[0046] (4) The present invention adopts the "position and path collision triggering" mechanism and effective tree reuse mode, with a replanning time of ≤0.7s, which improves the efficiency of traditional whole tree reconstruction by 72% and can respond to various changes of dynamic obstacles in real time.

[0047] Compared with the basic DRRT, the number of initial planning and replanning iterations, planning time and path length of the present invention are significantly improved, making it suitable for complex sea areas and providing a reliable solution for intelligent navigation of ships. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0049] Figure 1 This is a schematic diagram of the overall process of the improved DRRT algorithm provided in this embodiment of the invention;

[0050] Figure 2 This is a schematic diagram of the path node pruning and reconnection strategy provided in an embodiment of the present invention;

[0051] Figure 3 This is an example diagram of cubic B-spline curve fitting provided in an embodiment of the present invention;

[0052] Figure 4 This is a path planning result diagram of the improved DRRT algorithm provided in the embodiment of the present invention, wherein (a) represents the initial path planning process and (b) represents the replanning path process;

[0053] Figure 5 This is a schematic diagram of the system structure provided in an embodiment of the present invention. Detailed Implementation

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

[0055] In the following description, specific embodiments of the invention will be illustrated with reference to steps and symbols performed by one or more computers, unless otherwise stated. Therefore, these steps and operations will be referred to several times as being performed by a computer, and computer execution as referred to herein includes operations by a computer processing unit representing electronic signals of data in a structured format. This operation transforms the data or maintains it at a location in the computer's memory system, which can be reconfigured or otherwise alter the operation of the computer in a manner well known to those skilled in the art. The data structure maintained by the data is the physical location of the memory, which has specific characteristics defined by the data format. However, the principles of the invention described above are not intended to be limiting, and those skilled in the art will understand that many of the following steps and operations can also be implemented in hardware.

[0056] The terms "module" or "unit" as used herein can be considered as software objects executing on the computing system. Different components, modules, engines, and services described herein can be considered as implementations on the computing system. The apparatus and methods described herein are preferably implemented in software, but can also be implemented in hardware, both of which are within the scope of this invention.

[0057] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this specification means the presence of features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when we say an element is “connected” or “coupled” to another element, it can be directly connected or coupled to the other element, or there may be intermediate elements. Furthermore, “connected” or “coupled” as used herein can include wireless connections or wireless coupling. The term “and / or” as used herein includes all or any units and all combinations of one or more associated listed items.

[0058] In this embodiment of the invention, a ship dynamic path planning method based on an improved DRRT algorithm is provided, such as... Figure 1 As shown, it includes:

[0059] The system acquires two-dimensional planar environmental parameters of the sea area in which the ship navigates, clarifies the sea area boundary, collects the position and outline data of static obstacles, collects the status information of dynamic obstacles in real time, and determines the starting point and target point of the ship's path planning.

[0060] An improved DRRT algorithm is adopted. Sampling points are determined by offset sampling of the target and path points, and the expansion step size is determined by adaptive adjustment of the step size to generate the original initial path. Based on the original initial path, a set of path nodes is determined by pruning and reconnecting the path nodes, and the final initial path is generated by smoothing the set of path nodes with a cubic B-spline.

[0061] Obstacles are monitored to determine whether part of the path result fails. If there is a failure phenomenon, the improved DRRT algorithm is used for replanning to obtain a dynamic path.

[0062] In the embodiment of the present invention, the acquisition of dynamic environment information specifically includes: obtaining two-dimensional plane environment parameters of the ship navigation sea area, clarifying the sea area boundary range, and collecting the position and contour data of static obstacles, where the static obstacles include reefs, islands, fixed fishing nets, etc.; real-time collecting the state information of dynamic obstacles through detection devices carried on the ship (such as the ship automatic identification system AIS, radar, sonar), where the dynamic obstacles include other navigating ships, mobile fishing nets, etc., and the state information specifically covers the real-time position, movement speed, and heading. At the same time, the starting point q of the ship path planning is determined. start and the target point q goal , to construct a complete sea area environment input model and provide an accurate data basis for subsequent path planning.

[0063] In the embodiment of the present invention, the path planning of the improved DRRT algorithm specifically includes: based on the dynamic environment information collected in the first step, starting the improved dynamic rapid random tree (DRRT) algorithm to generate an initial feasible path. This improved algorithm realizes performance improvement through four collaborative optimization strategies, including target and path point offset sampling, step size adaptive adjustment, path node pruning and reconnecting, and cubic B-spline curve smoothing. The specific implementation logic and operation process of each strategy are as follows:

[0064] 1). Target and path point offset sampling. The sampling direction is controlled by a probability threshold to improve the sampling efficiency: the target offset threshold k (value range 0.05 - 0.2) and the path point offset threshold c (value range 0.5 - 0.7) are preset in advance, and a random number rand() in the interval [0, 1] is generated by the algorithm; when rand() ≤ k, the path target point q goal is selected as the sampling point to guide the rapid convergence of the random tree towards the target direction; when k < rand() ≤ k + c, an effective path node not affected by dynamic obstacles is selected as the sampling point to reuse the existing effective path information to reduce ineffective exploration; when rand() > k + c, a sampling point q is randomly generated in the sea area state space. randTo ensure coverage of unknown areas, this layered sampling strategy reduces redundant branches and lowers algorithm iteration costs. The real-time location and motion status of dynamic obstacles are collected by shipboard detection equipment, with a data sampling frequency of no less than 1Hz and an environmental information update delay controlled within ≤0.5s. The detection data is then calibrated in real-time by combining static obstacles with pre-mapped electronic nautical charts of the sea area.

[0065] 2) Adaptive step size adjustment. The extended step size is dynamically adjusted based on environmental collision feedback to balance obstacle avoidance accuracy and search speed: An initial extended step size ε and a step size adjustment range α (approximately 15%~25%) are set, and points matching the sampling point q are selected from the current random tree T. sample The nearest parent node q near ; with q near Starting from the current step size, move towards q sample Directional expansion generates a new node q new Through the collision detection function Collision(q) near q new Determine q near With q new Check if the connection between the two nodes intersects with the obstacle; if the detection result is no collision (returns "False"), the next expansion step size is updated to... (Lt is the current expansion step size), accelerating the exploration speed in open areas; if the detection result indicates a collision (returns "True"), the next expansion step size is reset to the initial step size ε, ensuring obstacle avoidance safety in areas with dense obstacles. The step size adjustment logic is quantified by the following formula:

[0066]

[0067] 3) Path node pruning and reconnection: Redundant nodes are reduced through cross-node collision detection to shorten the path length. First, an initial path node set P={q1,q2,...,qn} is obtained (q1 is the starting point, qn is the target point). The pruned node set Ppruned is initialized and the starting point q1 is included in it. Then, P... pruned Starting from the newly added node, iterate through all remaining nodes in the initial set, connect each node to be checked to the starting point with a straight line, and verify whether the connection segment has a collision using a collision detection function; add the farthest node without collision to P. pruned Then, using this node as a new starting point, the above traversal and detection process is repeated until the target point qn is included in P. pruned Meanwhile, a minimum trimming step size threshold of 0.8-1.2m is set to avoid loss of path accuracy due to excessive trimming. Ultimately, this strategy removes redundant nodes from the initial path, making the path shape closer to a straight line and shortening the actual sailing distance.

[0068] 4) Smoothing of cubic B-spline curves. A continuous path is constructed using high-order polynomial interpolation to adapt to ship steering constraints. The resulting P path is obtained after pruning and reconnecting the path nodes. pruned To control the vertices, a cubic B-spline curve is used for path fitting, and its curve equation is defined by the following formula:

[0069]

[0070] In the formula, P(u) is the two-dimensional coordinate point corresponding to parameter u on the smooth curve, P i For P pruned The number of control vertices (n+1 is the total number of control vertices), N i,3 (u) is a cubic B-spline basis function, obtained recursively from first-order basis functions. The specific recursive logic is as follows:

[0071]

[0072] In the above formula, the node vector U=[u0,u1,...,u m (m=n+3), the parameter u takes values ​​in the range [u2, u3] n+1 This ensures that the first and second derivatives of the curve are continuous at the control apex; at the same time, the number of interpolation points is set to 50-100, so that the turning curvature of the smoothed path is ≤0.05rad / m, avoiding sharp inflection points, reducing the energy consumption and operational difficulty of ship turning, and conforming to the physical constraints of actual ship navigation.

[0073] (3) Dynamic Path Update and Output. The system monitors the state changes of dynamic obstacles in real time and uses a "position and path collision trigger" mechanism to determine whether to initiate replanning. It continuously performs spatial intersection detection between the real-time position of the dynamic obstacle and the currently generated path planning trajectory. If the detection result shows that the actual position of the dynamic obstacle overlaps with the path trajectory (i.e., a collision occurs), the path replanning process is immediately triggered. The replanning process does not require rebuilding a complete random tree from the starting point. Instead, it expands based on the effective random tree structure retained from the initial planning stage, that is, the tree nodes and branches that have not been affected by dynamic obstacles and still satisfy the no-collision constraint. The improved DRRT algorithm is called, and only the effective tree structure is locally expanded and adjusted to quickly generate a new collision avoidance path in the obstacle environment. The final output is a dynamic path that meets the real-time and safety requirements of ship navigation and has good smoothness, providing the ship navigation control unit with direct and executable new path instructions.

[0074] Figure 1The diagram shows the overall flowchart of the improved DRRT algorithm. The algorithm consists of three stages: First, it collects information on the sea boundary and static / dynamic obstacles (sampling frequency ≥ 1Hz) to determine the start and end points of the path; then, it uses the improved DRRT algorithm for planning, generating an initial path through four strategies: target and path point bias sampling, adaptive step size adjustment, path node pruning and reconnection, and cubic B-spline smoothing; finally, it monitors obstacles, using "position-path collision" to determine if some path results are invalid. If invalidity exists, it reuses an effective random tree for fast replanning, outputting a dynamic path that meets the requirements.

[0075] Figure 2 This paper presents the core principle of path node pruning and reconnection. First, an initial set of path nodes containing the starting point, target point, and intermediate nodes is determined, and the pruned node set is initialized (with the first node as the starting point). Starting from the latest node in the pruned set, the remaining nodes in the initial set are traversed, and each node to be tested is connected to the starting point with a straight line and a collision detection is performed. The farthest node without a collision is included in the pruned set. This process is repeated with this node as the new starting point until the target point is included, visually demonstrating the optimization process of redundant node removal and path straightening.

[0076] Figure 3 The smoothing process of a cubic B-spline curve path is presented. The result is the set of nodes after trimming (P...). pruned Using the node vector U (m=n+3, n+1 being the number of control vertices) and cubic B-spline basis functions as control vertices, a continuous curve is fitted and generated; the number of interpolation points (50-100) and the constraint of turning curvature ≤0.05rad / m are labeled to intuitively show the optimization results of the path without sharp inflection points and adapted to the turning characteristics of ships.

[0077] Figure 4 The image shows the improved DRRT algorithm's path planning performance in complex sea areas. Using a two-dimensional sea area containing both static obstacles (reefs, islands, etc.) and dynamic obstacles (other vessels, etc.) as a background, the start and end points of the path are marked. The collision-free path generated by the algorithm is clearly displayed; after post-processing optimization, this path has no redundant branches and exhibits a continuous and smooth shape.

[0078] In another embodiment of the present invention, to facilitate better implementation of the method provided in the embodiments of the present invention, the present invention also provides a system based on the above method. The meanings of the terms are the same as in the above method, and specific implementation details can be found in the description of the method embodiments.

[0079] Please see Figure 5 , Figure 5 This is a schematic diagram of the system structure provided in an embodiment of the present invention. The system may include a parameter acquisition module 501, a path planning module 502, and a path update module 503, wherein:

[0080] The parameter acquisition module 501 is used to acquire the two-dimensional plane environment parameters of the ship navigation sea area, clarify the sea area boundary range, collect the position and contour data of static obstacles, collect the state information of dynamic obstacles in real time, and determine the starting point and target point of ship path planning at the same time;

[0081] The path planning module 502 is used to adopt an improved DRRT algorithm, determine sampling points by offset sampling of the target and path points, determine the expansion step size by adaptive adjustment of the step size to generate the original initial path, determine the path node set by pruning and reconnecting path nodes based on the original initial path, and smooth the path node set by cubic B-spline to generate the final initial path;

[0082] The path update module 503 is used to monitor obstacles to judge whether part of the path result fails. If there is a failure phenomenon, the improved DRRT algorithm is used to re-plan the time-consuming part of the path to obtain the dynamic path.

[0083] In another specific example, the above-mentioned determination of sampling points by offset sampling of the target and path points includes:

[0084] Preset the target offset threshold k and the path point offset threshold c, and generate a random number rand() in the range of [0, 1];

[0085] When rand () ≤ k, select the path target point q goal as the sampling point. When k < rand () ≤ k + c, select the valid path node not affected by dynamic obstacles as the sampling point. When rand () > k + c, randomly generate the sampling point q in the sea area state space rand .

[0086] In another specific example, the above-mentioned determination of the expansion step size by adaptive adjustment of the step size to generate the original initial path includes:

[0087] Set the initial expansion step size ε and the step size adjustment amplitude α, and screen out the parent node q sample nearest to the sampling point q near from the current random tree T;

[0088] Taking q near as the starting point, expand in the direction of q sample at the current step size to generate a new node q new , and judge whether the connection line between q near and q new and the two nodes intersect with obstacles through the collision detection function;

[0089] If the detection result is no collision, the next expansion step size is updated to , where Lt is the current expansion step size;

[0090] If the detection result indicates a collision, the next expansion step size will be reset to the initial step size ε.

[0091] In another specific example, the above method of determining the set of path nodes based on the original initial path through path node pruning and reconnection includes:

[0092] First, determine the original initial path node set containing the starting point, target point, and intermediate nodes. Then, initialize the trimmed node set, where the first node is the starting point.

[0093] Starting from the latest node in the pruned set, traverse the remaining nodes in the original initial path node set, perform straight-line connection and collision detection between each node to be detected and the starting point, and include the farthest node without collision into the pruned set.

[0094] Repeat the operation starting from the farthest node without collisions until the target point is included.

[0095] In another specific example, the above method of smoothly generating the final initial path by using cubic B-splines to smooth the set of path nodes includes:

[0096] Using the pruned node set as the control vertices, by Path fitting is performed using cubic B-spline curves, where P(u) represents the two-dimensional coordinates of the parameter u on the smooth curve. i To control the vertices, n+1 represents the total number of control vertices, and N... i,3 (u) is a cubic B-spline basis function.

[0097] The specific implementation methods of each module can be referred to the description of the above method embodiments, and the embodiments of the present invention will not be repeated.

[0098] The above provides a detailed description of a ship dynamic path planning method and system based on an improved DRRT algorithm provided by the embodiments of the present invention. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A ship dynamic path planning method based on an improved DRRT algorithm, characterized in that, include: The system acquires two-dimensional planar environmental parameters of the sea area in which the ship navigates, clarifies the sea area boundary, collects the position and outline data of static obstacles, collects the status information of dynamic obstacles in real time, and determines the starting point and target point of the ship's path planning. An improved DRRT algorithm is adopted. The sampling points are determined by sampling the target and path point biases. The original initial path is generated by adjusting the step size adaptively. The path node set is determined by pruning and reconnecting the path nodes based on the original initial path. The final initial path is generated by smoothing the path node set with cubic B-splines. Detect obstacles to determine if any part of the path results are invalid. If invalidity is found, the improved DRRT algorithm is used to replan the time-sensitive part of the path to obtain a dynamic path. The step of determining sampling points through target and path point offset sampling includes: Pre-set the target bias threshold k and the path point bias threshold c, and generate a random number rand() in the interval [0,1]. When rand() ≤ k, select the path target point q goal as the sampling point. When k < rand() ≤ k + c, select the valid path node not affected by dynamic obstacles as the sampling point. When rand() > k + c, randomly generate the sampling point q in the sea area state space rand ; The step of determining the extended step size and generating the original initial path through adaptive step size adjustment includes: Set the initial expansion step size ε and the step size adjustment range α, and select samples from the current random tree T that are similar to the sampling point q. sample The nearest parent node q near ; With q near Starting from the current step size, move towards q sample Directional expansion generates a new node q new q is determined by the collision detection function. near With q new Check whether the lines connecting the two nodes intersect with the obstacles; If the detection result is no collision, the next expansion step size will be updated to... Lt is the step size for this expansion; If the detection result indicates a collision, the next expansion step size will be reset to the initial step size ε.

2. The method according to claim 1, characterized in that, The process of determining the set of path nodes based on the original initial path through path node pruning and reconnection includes: First, determine the original initial path node set containing the starting point, target point, and intermediate nodes. Then, initialize the trimmed node set, where the first node is the starting point. Starting from the latest node in the pruned set, traverse the remaining nodes in the original initial path node set, perform straight-line connection and collision detection between each node to be detected and the starting point, and include the farthest node without collision into the pruned set. Repeat the operation starting from the farthest node without collisions until the target point is included.

3. The method according to claim 2, characterized in that, The process of smoothing the path node set using cubic B-splines to generate the final initial path includes: Using the pruned node set as the control vertices, by Path fitting is performed using cubic B-spline curves, where P(u) represents the two-dimensional coordinates of the parameter u on the smooth curve. i To control the vertices, n+1 represents the total number of control vertices, and N... i,3 (u) is a cubic B-spline basis function.

4. A ship dynamic path planning system based on an improved DRRT algorithm, characterized in that, include: The parameter acquisition module is used to acquire two-dimensional planar environmental parameters of the ship's navigation area, define the sea area boundary, collect the position and outline data of static obstacles, collect the status information of dynamic obstacles in real time, and determine the starting point and target point of the ship's path planning. The path planning module is used to employ an improved DRRT algorithm to determine sampling points by sampling the target and path point biases, generate the original initial path by adaptively adjusting the step size to determine the extended step size, determine the path node set by pruning and reconnecting the path nodes based on the original initial path, and generate the final initial path by smoothing the path node set with cubic B-splines. The path update module is used to monitor obstacles to determine if some path results are invalid. If invalidity is found, the improved DRRT algorithm is used to replan the time-sensitive part of the path to obtain a dynamic path. The step of determining sampling points through target and path point offset sampling includes: Pre-set the target bias threshold k and the path point bias threshold c, and generate a random number rand() in the interval [0,1]. When rand() ≤ k, select the path target point q goal as the sampling point. When k < rand() ≤ k + c, select the valid path node not affected by dynamic obstacles as the sampling point. When rand() > k + c, randomly generate the sampling point q in the sea area state space rand ; The step of determining the extended step size and generating the original initial path through adaptive step size adjustment includes: Set the initial expansion step size ε and the step size adjustment range α, and select samples from the current random tree T that are similar to the sampling point q. sample The nearest parent node q near ; With q near Starting from the current step size, move towards q sample Directional expansion generates a new node q new q is determined by the collision detection function. near With q new Check whether the lines connecting the two nodes intersect with the obstacles; If the detection result is no collision, the next expansion step size will be updated to... Lt is the step size for this expansion; If the detection result indicates a collision, the next expansion step size will be reset to the initial step size ε.

5. The system according to claim 4, characterized in that, The process of determining the set of path nodes based on the original initial path through path node pruning and reconnection includes: First, determine the original initial path node set containing the starting point, target point, and intermediate nodes. Then, initialize the trimmed node set, where the first node is the starting point. Starting from the latest node in the pruned set, traverse the remaining nodes in the original initial path node set, perform straight-line connection and collision detection between each node to be detected and the starting point, and include the farthest node without collision into the pruned set. Repeat the operation starting from the farthest node without collisions until the target point is included.

6. The system according to claim 5, characterized in that, The process of smoothing the path node set using cubic B-splines to generate the final initial path includes: Using the pruned node set as the control vertices, by Path fitting is performed using cubic B-spline curves, where P(u) represents the two-dimensional coordinates of the parameter u on the smooth curve. i To control the vertices, n+1 represents the total number of control vertices, and N... i,3 (u) is a cubic B-spline basis function.

Images