Ship dynamic path planning method and system based on improved DRRT algorithm

Abstract

The invention discloses a ship dynamic path planning method and system based on an improved DRRT algorithm, and relates to the technical field of ship automatic sailing, and the method comprises the steps: obtaining the two-dimensional plane environment parameters of a ship sailing sea area, determining the boundary range of the sea area, collecting the position and contour data of a static obstacle, and collecting the state information of a dynamic obstacle in real time, determining a starting point and a target point of ship path planning; an improved DRRT algorithm is adopted, sampling points are determined through target and path point bias sampling, the extended step length is determined through step length adaptive adjustment, a path node set is determined through path node trimming and reconnection, and an initial path is generated through cubic B-spline smoothing; obstacles are monitored, whether part of path results are invalid or not is judged, if the failure phenomenon exists, an improved DRRT algorithm is adopted for re-planning, and a dynamic path is obtained. According to the invention, the comprehensive requirements of the real-time performance, the safety and the path quality of the path planning in the intelligent navigation process of the ship can be met.

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 performance, safety, and path quality for path planning during the intelligent navigation of ships, and to 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: Obtain 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, collect the state information of dynamic obstacles in real time, and at the same time determine the starting point and target point of the ship path planning; Adopt an improved DRRT algorithm to 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 a path node set based on the original initial path through path node pruning and reconnecting, and smooth the path node set through cubic B-spline to generate a final initial path; Monitor obstacles to judge 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.

[0007] In some examples, the determination of sampling points through target and path point offset sampling includes: Preset a target offset threshold k and a path point offset threshold c, and generate a random number rand() within the interval [0, 1]; 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 .

[0008] In some examples, the determination of the expansion step size through step size adaptive adjustment to generate an original initial path includes: Set an initial expansion step size ε and a 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; 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 a collision detection function; 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 ε.

[0009] In some instances, 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.

[0010] In some instances, the process of smoothly generating the final initial path by applying cubic B-splines to the set of path nodes 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.

[0011] Secondly, the present invention provides a ship dynamic path planning system based on an improved DRRT algorithm, comprising: 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 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.

[0012] In some instances, determining the sampling point through target and path point offset sampling includes: Preset the target bias threshold k and the path point bias threshold c, and generate a random number rand() within 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 the dynamic obstacle as the sampling point. When rand () > k + c, randomly generate the sampling point q in the sea area state space rand .

[0013] In some instances, the determination of the expansion step size to generate the original initial path through step size adaptive adjustment includes: Set the initial expansion step size ε and the step size adjustment amplitude α, and screen out the parent node q closest to the sampling point q from the current random tree T sample ; near ; Starting from q near , expand in the direction of q at the current step size to generate a new node q sample . Use the collision detection function to judge whether there is an intersection between the connection line of q s new and q near and the two nodes with obstacles; new ; If the detection result is no collision, the next expansion step size is updated to , where Lt is the current expansion step size; If the detection result is a collision, the next expansion step size is reset to the initial step size ε.

[0014] s In some instances, the determination of the path node set through path node pruning and reconnecting based on the original initial path includes: First, determine the original initial path node set including the starting point, the target point and the intermediate nodes, and initialize the pruned 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, make a straight-line connection and collision detection between each待检测节点 (to be detected node) and the starting point, and include the farthest collision-free node in the pruned set; Repeat the operation with the farthest collision-free node as the new starting point until the target point is included.

[0015] In some instances, the smoothing of the path node set through cubic B-spline to generate the final initial path includes: Taking the pruned node set as the control vertices, by using the cubic B-spline curve for path fitting, where P(u) is the two-dimensional coordinate point corresponding to the parameter u on the smoothed curve, P i is the control vertex, n + 1 is the total number of control vertices, Ni,3 (u) is a cubic B-spline basis function.

[0016] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects: (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.

[0017] (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%.

[0018] (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.

[0019] (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.

[0020] 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.

[0021] The accompanying drawings are provided to more clearly illustrate the technical solutions in the embodiments of the present invention. The drawings used in the following description of the embodiments will be briefly introduced. Obviously, the 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.

[0022] Figure 1 This is a schematic diagram of the overall process of the improved DRRT algorithm provided in this embodiment of the invention; Figure 2 This is a schematic diagram of the path node pruning and reconnection strategy provided in an embodiment of the present invention; Figure 3 This is an example diagram of cubic B-spline curve fitting provided in an embodiment of the present invention; 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; Figure 5 This is a schematic diagram of the system structure provided in an embodiment of the present invention. Detailed Implementation

[0023] 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.

[0024] 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.

[0025] 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.

[0026] 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.

[0027] 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: 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. The algorithm monitors obstacles to determine if any part of the path is invalid. If invalidity is found, the improved DRRT algorithm is used to replan the path and obtain a dynamic path.

[0028] In this embodiment of the invention, dynamic environmental information acquisition specifically includes: acquiring two-dimensional planar environmental parameters of the ship's navigation area, defining the sea area boundary, and acquiring the position and outline data of static obstacles, including reefs, islands, fixed fishing nets, etc.; acquiring real-time status information of dynamic obstacles through ship-borne detection equipment (such as Automatic Identification System (AIS), radar, sonar), including other ships and moving fishing nets, with the status information specifically covering real-time position, speed, and heading. Simultaneously, the starting point q of the ship's path planning is determined. start With target point q goal We will construct a complete marine environment input model to provide a precise data foundation for subsequent path planning.

[0029] In this embodiment of the invention, the improved DRRT algorithm path planning specifically includes: based on the dynamic environment information collected in the first step, starting the improved Dynamic Fast Exploration Random Tree (DRRT) algorithm to generate an initial feasible path. This improved algorithm achieves performance improvement through four collaborative optimization strategies, including target and path point bias sampling, adaptive step size adjustment, path node pruning and reconnection, and cubic B-spline curve smoothing. The specific implementation logic and operation flow of each strategy are as follows: 1) Target and pathpoint bias sampling. Sampling efficiency is improved by controlling the sampling direction using probability thresholds: A target bias threshold k (range 0.05-0.2) and a pathpoint bias threshold c (range 0.5-0.7) are pre-set. An algorithm generates a random number rand() within the interval [0,1]. When rand() ≤ k, the pathpoint target q is selected. goalAs a sampling point, it guides the random tree to converge rapidly towards the target direction; when k < rand() ≤ k + c, select valid path nodes that are not affected by dynamic obstacles as sampling points, and reuse the existing valid path information to reduce ineffective exploration; when rand() > k + c, randomly generate a sampling point q in the sea area state space rand , ensuring the coverage ability of unknown areas, reducing redundant branches through this hierarchical sampling strategy, and lowering the algorithm iteration cost. The real-time position and motion state of dynamic obstacles are collected by ship detection equipment, the data sampling frequency is not less than 1Hz, and the environmental information update delay is controlled within ≤0.5s. By combining static obstacles with the pre-surveyed electronic nautical chart of the sea area, the detection data is calibrated in real time.

[0030] 2). Adaptive step size adjustment. Dynamically adjust the expansion step size according to the environmental collision feedback to balance the obstacle avoidance accuracy and search speed: Set the initial expansion step size ε and the step size adjustment amplitude α (about 15% - 25%). Select the parent node q sample nearest to the sampling point q near from the current random tree T; starting from q near , expand and generate a new node q sample in the direction of q new at the current step size. Use the collision detection function Collision(q near , q new ) to determine whether there is an intersection between the line connecting q near and q new and the obstacles; 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 is a collision (returns "True"), the next expansion step size is reset to the initial step size ε to ensure the obstacle avoidance safety in areas with dense obstacles. The step size adjustment logic is quantified by the following formula:

[0031] 3). Prune and reconnect path nodes. Trim redundant nodes through cross-node collision detection to shorten the path length: First, obtain the initial path node set P = {q1, q2,..., qn} (q1 is the starting point, qn is the target point), initialize the trimmed node set Ppruned and include the starting point q1; starting from the latest added node in P pruned , traverse all the remaining nodes in the initial set, connect each node to be detected to the starting point in a straight line, and verify whether there is a collision in the connection segment through the collision detection function; include the farthest node without collision in P pruned , and repeat the above traversal and detection process with this node as the new starting point until the target point qn is included in Ppruned 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.

[0032] 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:

[0033] 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:

[0034] 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.

[0035] (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 an overlap between the actual position of the dynamic obstacle and 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. This involves expanding tree nodes and branches that have not been affected by dynamic obstacles and still satisfy the no-collision constraint. An 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.

[0036] 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.

[0037] 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.

[0038] 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.

[0039] 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.

[0040] 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.

[0041] 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: The parameter acquisition module 501 is used to acquire the two-dimensional plane environment parameters of the ship navigation area, clarify the sea area boundary range, collect the position and contour 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 path planning at the same time; 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 an original initial path, determine a set of path nodes by pruning and reconnecting path nodes based on the original initial path, and smooth the set of path nodes through cubic B-spline to generate a final initial path; 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 a dynamic path.

[0042] In another specific example, the above-mentioned determination of sampling points by offset sampling of the target and path points includes: Preset a target offset threshold k and a path point offset threshold c, and generate a random number rand() within the interval [0,1]; When rand () ≤ k, select the path target point q goal as the sampling point. When k < rand () ≤ k + c, select an effective path node that is not affected by dynamic obstacles as the sampling point. When rand () > k + c, randomly generate a sampling point q in the sea area state space rand .

[0043] In another specific example, the above-mentioned determination of the expansion step size by adaptive adjustment of the step size to generate an original initial path includes: Set an initial expansion step size ε and a step size adjustment amplitude α, and select the parent node q sample closest to the sampling point q from the current random tree T near ; 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; If the detection result is no collision, the next expansion step size is updated to , where Lt is the current expansion step size; If the detection result is a collision, the next expansion step size is reset to the initial step size ε.

[0044] 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: 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.

[0045] 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: 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.

[0046] 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.

[0047] 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, The application relates to a ship path planning method and device. The application comprises the following steps: An improved DRRT algorithm is adopted to determine sampling points through target and path point bias sampling, to generate an original initial path through step length self-adaptive adjustment and expansion step length, to determine a path node set through path node pruning and reconnection based on the original initial path, and to generate a final initial path through three B-splines smoothing of the path node set. The application further comprises the following steps:

2. The method of claim 1, wherein, The target bias threshold k and the path point bias threshold c are preset, and a random number rand() in the interval [0, 1] is generated. If the detection result is collision, the next expansion step length is reset to the initial step length epsilon. When rand ()≤k, the path target point q is selected goal As a sampling point, when k<rand ()≤k+c, the effective path node not affected by the dynamic obstacle is selected as the sampling point, and when rand ()>k+c, the sampling point q is randomly generated in the sea state space rand .

3. The method of claim 2, wherein, The original initial path node set containing the starting point, the target point and the intermediate nodes is determined, and the pruned node set is initialized, wherein the first node is the starting point. Set initial expansion step size ε and step size adjustment amplitude α, select 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 The connection between the two nodes and whether they intersect with the obstacle; If the detection result is no collision, the next time the expansion step length is updated as , Lt is the current expansion step length; The remaining nodes of the original initial path node set are traversed with the latest node of the pruned node set as the starting point, and the straight line connection and collision detection are performed on each to-be-detected node and the starting point, and the farthest node without collision is included in the pruned node set.

4. The method of claim 3, wherein, The farthest node without collision is taken as a new starting point to repeat the operation until the target point is included. The application further comprises the following steps: The application comprises the following steps: The parameter acquisition module is used for acquiring two-dimensional plane environment parameters of a ship sailing sea area, determining the sea area boundary range, collecting position and contour data of static obstacles, collecting state information of dynamic obstacles in real time, and determining a starting point and a target point of ship path planning.

5. The method of claim 4, wherein, The path planning module is used for adopting an improved DRRT algorithm to determine sampling points through target and path point bias sampling, to generate an original initial path through step length self-adaptive adjustment and expansion step length, to determine a path node set through path node pruning and reconnection based on the original initial path, and to generate a final initial path through three B-splines smoothing of the path node set. With the pruned node set as the control vertex, by The path is fitted by using a cubic B-spline curve, wherein P(u) is a two-dimensional coordinate point corresponding to a parameter u on the smooth curve, P i is a control vertex, and n+1 is the total number of control vertices, N i,3 (u) is a cubic B-spline base function.

6. A ship dynamic path planning system based on an improved DRRT algorithm, characterized in that, The path updating module is used for monitoring obstacles to determine whether part of the path result is invalid, and if the invalid phenomenon exists, the improved DRRT algorithm is adopted to re-plan the time-effective part of the path to obtain a dynamic path. The target bias threshold k and the path point bias threshold c are preset, and a random number rand() in the interval [0, 1] is generated. If the detection result is collision, the next expansion step length is reset to the initial step length epsilon. ​ 7. The system of claim 6, wherein, ​ ​ When rand ()≤k, the path target point q is selected goal As a sampling point, when k<rand ()≤k+c, the effective path node not affected by the dynamic obstacle is selected as the sampling point, and when rand ()>k+c, the sampling point q is randomly generated in the sea state space rand .

8. The system of claim 7, wherein, ​ Set initial expansion step size ε and step size adjustment amplitude α, select a sampling point q 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 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; If the detection result is no collision, the next time the expansion step length is updated as , Lt is the current expansion step length; ​ 9. The system of claim 8, wherein, The method for determining the path node set based on the original initial path through path node pruning and reconnection comprises the following steps: First, determine the original initial path node set containing the starting point, the target point and the intermediate nodes, and initialize the pruned node set, wherein the first node is the starting point; Take the latest node in the pruned set as the starting point, traverse the remaining nodes in the original initial path node set, make a straight line connection between each to-be-detected node and the starting point, and perform collision detection, and then add the farthest node without collision into the pruned set; Repeat the operation by taking the farthest node without collision as a new starting point until the target point is added.

10. The system of claim 9, wherein, The method for generating the final initial path by smoothing the path node set through three B-splines comprises the following steps: With the pruned node set as the control vertex, by The path is fitted by using a cubic B-spline curve, wherein P(u) is a two-dimensional coordinate point corresponding to a parameter u on the smooth curve, P i is a control vertex, n+1 is the total number of control vertices, N i,3 (u) is a cubic B-spline base function.

Images