A path planning method and system for autonomous docking of unmanned surface vessels

Abstract

A path planning method for autonomous berthing of unmanned surface vessels (USVs) is characterized by the following steps: Step S1: The feasible space is rasterized, with each raster representing a node. Step S2: The berthing position is set as the starting point, and the current position as the ending point. A first path is obtained by searching using a hybrid A* method. Step S3: The first path is reversed to obtain the actual path. Step S4: Real-time collision detection is performed on the actual path, and the actual path is adjusted based on the collision detection results. Compared to traditional ship berthing paths, the path generated by the reverse hybrid A* method with steering penalties in this invention is more suitable for the actual operational needs of ships in confined waters. By adding a steering penalty term, the path planning is more reasonable, avoiding unnecessary complex turns and improving the applicability and safety of the path. Simultaneously, the hybrid A* method can reduce the search cost near narrow berthing endpoints.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation control technology, and in particular to a path planning method and system for autonomous berthing of unmanned surface vessels. Background Technology

[0002] In recent years, with breakthroughs and advancements in computer technology, communication technology, sensor technology, and materials science, unmanned vessel technology has also been continuously developing. Unmanned vessel technology involves multiple disciplines and fields, and autonomous berthing and unberthing technology is an important research direction. Autonomous berthing and unberthing technology is not only a key technology for achieving unmanned navigation but also an important means of improving ship safety. It can improve the unmanned vessel technology system and is of great significance to promoting the development of unmanned vessel technology. The autonomous berthing and unberthing process not only controls the ship's course and speed but also includes environmental perception, path planning, dynamic positioning, and route tracking, among other things.

[0003] Currently, traditional path planning algorithms often fall short in the complex and crucial scenario of ship berthing. These algorithms fail to fully incorporate the specific characteristics of ship berthing during their design, particularly neglecting the critical practical factor that the ship must maintain parallelism with the shore upon final berthing. Therefore, even if these algorithms can generate a path from start to finish, this path often fails to meet the actual requirements for safe and smooth berthing. During berthing, ships not only need precise control of their direction of travel but also close monitoring of their heading angle, speed, and potential external disturbances to ensure they approach and parallel the shore with the correct attitude. Because traditional algorithms fail to adequately consider these complex factors, the paths they generate are often overly idealized, lacking practicality and reliability, and difficult to apply effectively in real-world operations. Summary of the Invention

[0004] To address the aforementioned shortcomings, the present invention aims to propose a path planning method and system for autonomous berthing of unmanned surface vessels, thereby solving the problems of excessive resource consumption during the generation of the final path and low applicability and safety of existing path planning methods.

[0005] To achieve this objective, the present invention adopts the following technical solution: a path planning method for autonomous berthing of unmanned surface vessels, characterized by comprising the following steps:

[0006] Step S1: Rasterize the feasible space, where each grid cell is treated as a node;

[0007] Step S2: Set the moored position as the starting point and the current position as the ending point, and search for the first path using the mixed A* method;

[0008] Step S3: Reverse the first path to obtain the actual path;

[0009] Step S4: Perform real-time collision detection on the actual path and adjust the actual path based on the collision detection results.

[0010] Preferably, the specific steps for obtaining the first path using the hybrid A* method in step S2 are as follows:

[0011] Step S21: Construct a first set and a second set, store the starting point in the first set, store other nodes in the second set, and mark the starting point as the first node;

[0012] Step S22: Within the second set, obtain the penalty values ​​of the adjacent nodes of the first node;

[0013] Step S23: Obtain the node with the smallest penalty value as the second node, remove the second node from the second set, and store the second node in the first set;

[0014] Step S24: Determine whether the second node is the endpoint. If yes, construct the first path according to the storage order of the nodes in the second set. If no, replace the second node as the first node and re-execute steps S22 to S24.

[0015] Preferably, the formula for obtaining the penalty value in step S23 is as follows:

[0016] f(s) = g(s) + h(s);

[0017] Where g(s) is the multi-constraint extended cost function, and h(s) is the heuristic function;

[0018]

[0019] s i For the i-th node, d(s) i-1 ,s i ) represents the distance between two adjacent nodes, δ i δ represents the driving state at the i-th node. i =1 indicates forward travel and δ i =-1 Reverse driving, w r w represents the cost coefficient for reversing. t A coefficient representing the cost of switching, risk i This represents the penalty term for the distance between the i-th node and the obstacle;

[0020] h(s) = max(h1(s) i h2(s) i ));

[0021] in

[0022] s i This represents the state of the current node, s goal =(x goal ,y goal ,θ goal ), (x goal ,y goal ) represents the position of the starting point, θ goal Indicates the orientation of the starting point, d Reed-Shepp This represents the path length calculated using the Reeds-Shepp curve. Let dx(τ) represent the set of all Reeds-Shepp curves, dx(τ) represent the length of the τth curve in the X direction, and dy(τ) represent the length of the τth curve in the y direction.

[0023]

[0024] in Let p = {(x1,y1),(x2,y2),…,(x...}} represent the set of all possible barrier-free paths. N ,y N )} is a discrete path, where N is the total number of nodes, (x k ,y k ) represents the coordinates of the k-th node.

[0025] Preferably, the unmanned surface vessel follows the following two-dimensional kinematics:

[0026]

[0027] Where k max The maximum curvature of the hull during motion is represented by Δθ, where L represents the length of the kinematic element. max Δx represents the maximum heading angle of the unmanned surface vessel at the current moment, Δy represents the difference between the two nodes in the x-direction, and Δy represents the difference between the two nodes in the y-direction.

[0028] Preferably, before executing step S3, step A is also required: obtaining the turning points and starting point range in the first path, and optimizing the path for the turning points and starting point range.

[0029] The method for identifying turns is as follows: obtain the turning angle of the unmanned surface vessel at the node; if the turning angle is greater than the set angle, it indicates that the node is a turn.

[0030] The formula for obtaining the turning angle is as follows:

[0031]

[0032] Where Pi+1 P represents the coordinates of the next node after the i-th node. i Represents the coordinates of the i-th node;

[0033] Specifically, path optimization at turning points and within the starting area involves:

[0034] Insert virtual control nodes at the turning points and in the starting area;

[0035] After the virtual control node and target node are inserted, a safe pipeline is generated for the path after the inserted node using Euclidean distance information, constraining subsequent path optimization to not be performed outside the safe pipeline.

[0036] Preferably, after performing step A, step B also needs to be performed;

[0037] Step B involves smoothing the first path using the conjugate gradient method.

[0038] The conjugate gradient function is as follows:

[0039]

[0040] Where |X i -O i | is the minimum obstacle distance, d min It is the minimum safe distance from obstacles, ΔX i =x i -x i-1 It is a vector pointing from the (i-1)th node to the (i-1)th node. w is the reciprocal of the Euclidean distance from the i-th node to the j-th target point. o w κ w s w a These are the weighting coefficients, X represents the change in angle between adjacent path points; gj This represents the coordinates of the j-th target location. i Indicates distance from the current node X i The coordinates of the obstacle with the smallest distance.

[0041] Preferably, the specific steps for real-time collision detection of the actual path in step S4 are as follows:

[0042] Get the current state of all static and dynamic objects;

[0043] Based on dynamic motion model v i and control input u i (t), the predicted time interval T = [t0, t HThe trajectory of a dynamic object within ().

[0044] Obtain the minimum Euclidean distance from the unmanned surface vessel to each object. If there is a minimum Euclidean distance of 0, it indicates that the unmanned surface vessel is about to collide.

[0045] Preferably, the step of adjusting the actual path based on the collision detection results in step S4 is as follows:

[0046] If the collision detection result is that no collision has occurred, then continue driving along the original actual path;

[0047] If the collision detection result indicates that a collision is imminent, then obtain the collision time TTC;

[0048] The obstacle avoidance cutoff time is determined based on the collision time TTC and the maximum reserved obstacle avoidance time.

[0049] On the Frenét coordinates generated based on Dijkstra, a series of optimized paths are generated from the obstacle avoidance cutoff time points using a fifth-order polynomial.

[0050] Design an evaluation function, and select the two optimization paths of different classes with the largest evaluation function as the second path;

[0051] Obtain the obstacle density on the two second paths, and select the second path with the lower obstacle density as the final adjustment path.

[0052] A path planning system for autonomous docking of unmanned surface vessels (USVs) is provided, which uses a path planning method for autonomous docking of USVs, including a gridding module, a preliminary search module, a path generation module, and a path local optimization module.

[0053] The rasterization module is used to rasterize the feasible space;

[0054] The preliminary search module is used to set the moored position as the starting point and the current position as the ending point, and to obtain the first path by searching using the hybrid A* method;

[0055] The path generation module is used to reverse the first path to obtain the actual path;

[0056] The path local optimization module is used to perform real-time collision detection on the actual path and adjust the actual path based on the collision detection results.

[0057] One of the above technical solutions has the following advantages or beneficial effects:

[0058] 1. Compared to traditional ship berthing paths, the path generated by the inverse hybrid A* method with steering penalties in this invention is more suitable for the actual operational needs of ships in restricted waters. By adding a steering penalty term, the path planning is more reasonable, avoiding unnecessary complex turns and improving the applicability and safety of the path. At the same time, the inverse hybrid A* method can reduce the search cost near narrow berthing endpoints.

[0059] 2. Inserting sequence points at the turns and endpoints of the generated initial path facilitates subsequent trajectory optimization, resulting in a more controllable path. Introducing safety pipe constraints based on Euclidean distance ensures path safety, and setting virtual target points allows for advance heading adjustments for better berth entry. These designs enable more precise and safer control when the ship is docked.

[0060] 3. Traditional local obstacle avoidance algorithms are prone to getting trapped in local optima and have poor path flexibility. The proposed local trajectory repair method avoids this problem, offers more options in complex environments, and can handle a wider range of scenarios. Trajectory repair can return to the original global path, saving the cost of replanning. Attached Figure Description

[0061] Figure 1 This is a flowchart of an embodiment of the method of the present invention.

[0062] Figure 2 This is a schematic diagram of the structure of one embodiment of the system of the present invention.

[0063] Figure 3 This is a schematic diagram of inserting a virtual control node according to an embodiment of the present invention.

[0064] Figure 4 This is a schematic diagram illustrating the generation of a secure channel according to an embodiment of the present invention.

[0065] Figure 5 This is a smoothed path according to an embodiment of the present invention.

[0066] Figure 6 This is a schematic diagram illustrating the generation of avoidance points according to an embodiment of the present invention.

[0067] Figure 7 This is a schematic diagram illustrating the generation of an optimized path according to an embodiment of the present invention. Detailed Implementation

[0068] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0069] In the description of embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the stated features. In the description of embodiments of the present invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0070] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0071] like Figures 1-7 As shown, a path planning method for autonomous berthing of unmanned surface vessels includes the following steps:

[0072] Step S1: Rasterize the feasible space, where each grid cell is treated as a node;

[0073] Step S2: Set the moored position as the starting point and the current position as the ending point, and search for the first path using the mixed A* method;

[0074] Step S3: Reverse the first path to obtain the actual path;

[0075] Step S4: Perform real-time collision detection on the actual path and adjust the actual path based on the collision detection results.

[0076] In this invention, the berthing position is set as the starting point, and the current position is set as the ending point. A first path is then obtained through a hybrid A* method search. The generation order of this first path is essentially the reverse of the normal path. Starting the search with the berthing position as the starting point filters out redundant paths generated near the ending point in a traditional forward search, improving search efficiency. Furthermore, compared to traditional ship berthing paths, the path generated by the reverse hybrid A* method with a steering penalty in this invention is more suitable for the actual operational needs of ships in restricted waters. By adding a steering penalty term, path planning is more rational, avoiding unnecessary complex turns and improving the applicability and safety of the path. Simultaneously, the hybrid A* method reduces search costs near narrow berthing endpoints.

[0077] Preferably, the specific steps for obtaining the first path using the hybrid A* method in step S2 are as follows:

[0078] Step S21: Construct a first set and a second set, store the starting point in the first set, store other nodes in the second set, and mark the starting point as the first node;

[0079] Step S22: Within the second set, obtain the penalty values ​​of the adjacent nodes of the first node;

[0080] Step S23: Obtain the node with the smallest penalty value as the second node, remove the second node from the second set, and store the second node in the first set;

[0081] Step S24: Determine whether the second node is the endpoint. If yes, construct the first path according to the storage order of the nodes in the second set. If no, replace the second node as the first node and re-execute steps S22 to S24.

[0082] Preferably, the formula for obtaining the penalty value in step S23 is as follows:

[0083] f(s) = g(s) + h(s);

[0084] Where g(s) is the multi-constraint extended cost function, and h(s) is the heuristic function;

[0085]

[0086] s i For the i-th node, d(s) i-1 ,s i ) represents the distance between two adjacent nodes, δ i δ represents the driving state at the i-th node. i =1 indicates forward travel and δ i =-1 Reverse driving, w r w represents the cost coefficient for reversing. t A coefficient representing the cost of switching, risk i This represents the penalty term for the distance between the i-th node and the obstacle;

[0087] To ensure a smooth, safe initial path planned by the unmanned surface vessel (USV) with minimal backtracking, the constraints of the extended cost function are improved. The multi-constraint extended cost function g(s) in this invention incorporates a driving state penalty term, i.e., for node s. i-1 to s i expansion cost i-1 ,s i By setting cost(s) i-1 ,s iThis allows for less reversing and direction changes in the planned path. This setting effectively solves the control problems caused by frequent reversing or turning in complex sea areas for unmanned surface vessels, while also providing a suitable initial path for subsequent trajectory optimization.

[0088] h(s) = max(h1(s) i h2(s) i ));

[0089] in

[0090] s i This represents the state of the current node, s goal =(x goal ,y goal ,θ goal ), (x goal ,y goal ) represents the position of the starting point, θ goal Indicates the orientation of the starting point, d Reed-Shepp This represents the path length calculated using the Reeds-Shepp curve. Let dx(τ) represent the set of all Reeds-Shepp curves, dx(τ) represent the length of the τth curve in the X direction, and dy(τ) represent the length of the τth curve in the y direction.

[0091] The first heuristic function h1(st i Considering the non-integrity constraints of the unmanned surface vessel (USV), ignoring obstacles, its value is the condition that the current node (x, y, θ) reaches the target point (x). goal ,y goal ,θ goal The shortest path for non-integrity constraints is obtained from the Reeds-Shepp curve. This curve calculates the distance from the starting point to any offline point, and the heuristic value from any point to the target point is obtained by rotating and translating a real-time 3D vector.

[0092]

[0093] in Let p = {(x1,y1),(x2,y2),…,(x...}} represent the set of all possible barrier-free paths. N ,y N )} is a discrete path, where N is the total number of nodes, (x k ,y k ) represents the coordinates of the k-th node.

[0094] The second heuristic function h2(s) i The result was obtained by considering the obstacle environment while ignoring the kinematics of the unmanned surface vessel.

[0095] Preferably, the unmanned surface vessel follows the following two-dimensional kinematics:

[0096]

[0097] Where k max The maximum curvature of the hull during motion is represented by Δθ, where L represents the length of the kinematic element. max Δx represents the maximum heading angle of the unmanned surface vessel at the current moment, Δy represents the difference between the two nodes in the x-direction, and Δy represents the difference between the two nodes in the y-direction.

[0098] The two-dimensional kinematics defines six directions in which a node expands when searching for a second node: forward and backward, left forward turn, right forward turn, left backward turn, and right backward turn.

[0099] Preferably, before executing step S3, step A is also required: obtaining the turning points and starting point range in the first path, and optimizing the path for the turning points and starting point range.

[0100] The method for identifying turns is as follows: obtain the turning angle of the unmanned surface vessel at the node; if the turning angle is greater than the set angle, it indicates that the node is a turn.

[0101] The formula for obtaining the turning angle is as follows:

[0102]

[0103] Where P i+1 P represents the coordinates of the next node after the i-th node. i Represents the coordinates of the i-th node;

[0104] Specifically, path optimization at turning points and within the starting area involves:

[0105] like Figure 3 As shown, virtual control nodes are inserted at the turning points and in the starting area.

[0106] like Figure 4 As shown, after the virtual control node is inserted, a safe pipeline is generated for the path after the inserted node using Euclidean Signed Distance Field (ESDF) information, which constrains subsequent path optimization to not be performed outside the safe pipeline.

[0107] For the generated initial path, the control nodes at the turns can generate more controllable paths in subsequent trajectory optimization. Introducing a safety pipeline constraint based on Euclidean distance ensures path safety. For the ship berthing scenario, the introduced virtual target point allows for advance heading adjustment to better enter the berth. Compared to traditional path planning methods, these designs make ship berthing control more precise and safer.

[0108] Preferably, after performing step A, step B also needs to be performed;

[0109] Step B involves smoothing the first path using the conjugate gradient method.

[0110] The conjugate gradient function is as follows:

[0111]

[0112] Where |X i -O i | is the minimum obstacle distance, d min It is the minimum safe distance from obstacles, ΔX i =x i -x i-1 It is a vector pointing from the (i-1)th node to the (i-1)th node. w is the reciprocal of the Euclidean distance from the i-th node to the j-th target point. o w κ w s w a These are the weighting coefficients, X represents the change in angle between adjacent path points; gj This represents the coordinates of the j-th target location. i Indicates distance from the current node X i The coordinates of the obstacle with the smallest distance.

[0113] like Figure 5 As shown, the dashed lines represent the smoothed path trajectory. In the conjugate gradient function, the first term is the obstacle distance penalty term, which guides the unmanned surface vessel (USV) away from obstacles in various scenarios; the second term is the curvature change penalty term, which avoids large curves in the path; the third term is the smoothing term, which ensures the smoothness of the path; the fourth term is the reciprocal of the endpoint distance; and the fifth term is the endpoint attraction term. For the requirement that the ship's attitude at the berth needs to be parallel to the shore, adding an attraction term for the virtual endpoint helps to make heading adjustments in advance, making the path more reasonable during the berthing process.

[0114] Preferably, the specific steps for real-time collision detection of the actual path in step S4 are as follows:

[0115] Get the current state of all static and dynamic objects;

[0116] Based on dynamic motion model v i and control input u i (t), the predicted time interval T = [t0, t H The trajectory of a dynamic object within a given time interval can be predicted. When predicting the trajectory of a dynamic object, a simple motion model can be created. The input is u(t) and the object's state at time t (pose, shape), and the output is the derivative of the object's state, i.e., its velocity. By integrating this model, the trajectory of the object within the predicted time interval can be predicted using its current state. u... i (t) is the control input function, which can be assumed to be a constant value, assuming that the dynamic obstacle follows a simple uniform linear motion.

[0117] Obtain the minimum Euclidean distance from the unmanned surface vessel to each object. If there is a minimum Euclidean distance of 0, it indicates that the unmanned surface vessel is about to collide.

[0118] Once the trajectory of a dynamic object is obtained, the minimum Euclidean distance between it and the actual trajectory can be obtained. When the minimum Euclidean distance is 0, it indicates that there is a collision between the trajectories of the two dynamic objects and the actual path. Similarly, since static objects do not move, it is only necessary to obtain the minimum Euclidean distance between the static object and the actual path to determine whether a collision may occur.

[0119] Preferably, the step of adjusting the actual path based on the collision detection results in step S4 is as follows:

[0120] If the collision detection result is that no collision has occurred, then continue driving along the original actual path;

[0121] If the collision detection result indicates that a collision is imminent, then obtain the collision time TTC;

[0122] Since the position at the minimum Euclidean distance is known, the collision time TTC can be obtained immediately by using the current position, the position at the minimum Euclidean distance, and the speed of the unmanned surface vessel.

[0123] Since unmanned surface vessels (USVs) require response time (maximum obstacle avoidance time) when changing routes and performing operations, the obstacle avoidance cutoff time can be obtained by subtracting the maximum obstacle avoidance time from the collision time (TTC). It's worth noting that when the difference between the collision time (TTC) and the maximum obstacle avoidance time is negative, the current time can be considered the obstacle avoidance cutoff time (t). cut .

[0124] On the Frenét coordinates generated based on Dijkstra, a series of optimized paths are generated from the obstacle avoidance cutoff time points using a fifth-order polynomial.

[0125] In the Frenét coordinate system, the motion of the unmanned surface vessel (USV) is decomposed into lateral motion s(t) along the curve and longitudinal motion d(t) perpendicular to the curve. Using Dijkstra's algorithm, a path is created assuming no new obstacles are added, and the obstacle avoidance cutoff point t, where trajectory repair is required, is determined. cut The optimized path from the pose to the target area (next node). When generating the optimized path, this path is set to the vertical axis of the Frenét coordinate system. A series of obstacle avoidance points are generated at both ends of the obstacle, such as... Figure 6 As shown, this prepares for generating optimized paths at both ends of the obstacle avoidance point. The rules for generating the obstacle avoidance point are as follows:

[0126] Avoidance point [s] a ,d a ] T It is from the first anomalous point (e.g., the first point of collision with an obstacle) [s] v ,d v ] T This is obtained by starting with a horizontal offset. The specific formula is as follows:

[0127] s a =s v ;

[0128]

[0129] Where, d min and d max These are the minimum and maximum allowed lateral offsets, n. d The number of path candidates can be specified based on the unmanned surface vessel's width ω, for example... s v d v These represent the lateral and longitudinal motions at the first collision point, s a d a These are the lateral and longitudinal movements of the avoidance point, respectively.

[0130] To connect the location of the obstacle avoidance cutoff point to the target region using a fifth-order polynomial, 12 unknown coefficients need to be determined; the polynomial forms of the longitudinal motion s(t) and the lateral motion d(t) are as follows:

[0131]

[0132] To avoid obstacles, the remaining time from the cutoff point to the planned target area is used. h This refers to the total timeframe for the planning period. Boundary conditions, i.e., the cutoff and target states, can be obtained through coordinate transformation; the following coefficients can be directly determined:

[0133] Initial state boundary conditions (cut-off state):

[0134] For s(t): Position: s(0) = s cut ;speed: Acceleration: s cut For the lateral movement at the time point of obstacle avoidance cutoff, d cut Longitudinal movement at the time point of obstacle avoidance cutoff;

[0135] For d(t): position: d(0) = d cut ;speed: Acceleration:

[0136] Target state boundary conditions:

[0137] For s(t): position: speed: Acceleration:

[0138] For d(t): position: speed: Acceleration:

[0139] s g and d g The lateral and longitudinal movements upon reaching the target area;

[0140] Initial state boundary condition coefficients:

[0141] At t=0:

[0142] s(0)=α5*0 5 +α4*0 4 +α3*0 3 +α2*0 2 +α1*0+α0=s cut

[0143] d(0)=β5*0 5 +β4*0 4 +β3*0 3 +β2*0 2 +β1*0+β0=d cut

[0144] Similarly, from the initial velocity and acceleration, we can obtain:

[0145]

[0146] Target state boundary condition coefficients:

[0147] exist hour:

[0148]

[0149] Given α0 = s cut , We can obtain:

[0150]

[0151] Similarly, the target's speed and the location of the obstacle avoidance point can be obtained:

[0152]

[0153] Using these conditions, the equations are rearranged into matrix form, and α3, α4, α5; β3, β4, β5 are solved.

[0154]

[0155]

[0156] in The time taken at the obstacle avoidance point is used to calculate the corresponding curve by solving for the polynomial parameters at each obstacle avoidance point. For example... Figure 7 As shown.

[0157] Design an evaluation function, and select the two optimization paths of different classes with the largest evaluation function as the second path;

[0158] In this invention, the evaluation function is specifically as follows:

[0159]

[0160] The first and second terms (integral longitudinal and lateral accelerations) are used to ensure path smoothness, reducing abrupt accelerations and turns to improve the comfort and stability of the unmanned surface vessel. p(π) represents the reciprocal of the distance between each obstacle avoidance point and its smallest obstacle, and ω1, ω2, and ω3 are weights that need to be manually adjusted.

[0161] In this invention, instead of obtaining only one optimized path, two optimized paths of different wheel types are selected. Paths of different wheel types cannot be continuously deformed without passing through obstacles. This approach can handle unexpected events that occur in dynamic scenarios. If only one path is generated, adjustments cannot be made immediately when the situation becomes unpredictable, requiring replanning. Generating two paths of different wheel types can address this situation.

[0162] Obtain the obstacle density on the two second paths, and select the second path with the lower obstacle density as the final adjustment path.

[0163] To avoid getting trapped in local optima, the two paths generated at both ends can switch to the other path if more obstacles are detected on one side, thus escaping the local optimum. The formula for obtaining the obstacle density is as follows:

[0164]

[0165] μ can be calculated from the data from the lidar. Here, L represents the amount of data obtained from a full lidar scan, and dr represents the lidar's measurement radius. When there are no obstacles, dj = dr. If obstacles exist, dj is the distance between the unmanned surface vessel (USV) and the obstacle. A smaller μ value indicates a lower obstacle density, and a larger μ value indicates a higher obstacle density.

[0166] A path planning system for autonomous docking of unmanned surface vessels (USVs) is provided, which uses a path planning method for autonomous docking of USVs, including a gridding module, a preliminary search module, a path generation module, and a path local optimization module.

[0167] The rasterization module is used to rasterize the feasible space;

[0168] The preliminary search module is used to set the moored position as the starting point and the current position as the ending point, and to obtain the first path by searching using the hybrid A* method;

[0169] The path generation module is used to reverse the first path to obtain the actual path;

[0170] The path local optimization module is used to perform real-time collision detection on the actual path and adjust the actual path based on the collision detection results.

[0171] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0172] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A path planning method for autonomous berthing of unmanned surface vessels, characterized in that, Includes the following steps: Step S1: Rasterize the feasible space, where each grid cell is treated as a node; Step S2: Set the moored position as the starting point and the current position as the ending point, and search for the first path using the mixed A* method; Step S3: Reverse the first path to obtain the actual path; Step S4: Perform real-time collision detection on the actual path and adjust the actual path based on the collision detection results; The specific steps for obtaining the first path using the hybrid A* method in step S2 are as follows: Step S21: Construct a first set and a second set, store the starting point in the first set, store other nodes in the second set, and mark the starting point as the first node; Step S22: Within the second set, obtain the penalty values ​​of the adjacent nodes of the first node; Step S23: Obtain the node with the smallest penalty value as the second node, remove the second node from the second set, and store the second node in the first set; Step S24: Determine whether the second node is the endpoint. If yes, construct the first path according to the storage order of the nodes in the second set. If no, replace the second node as the first node and re-execute steps S22 to S24. The formula for obtaining the penalty value in step S23 is as follows: ； in To extend the cost function with multiple constraints, It is a heuristic function; ; For the i-th node, This represents the distance between two adjacent nodes. This represents the driving state at the i-th node. Indicates forward travel and Reverse driving, This represents the cost coefficient for reversing. A coefficient representing the cost of turning around. This represents the penalty term for the distance between the i-th node and the obstacle; =max( , ); in ; This indicates the state of the current node. , ( () indicates the starting position. Indicates the direction of the starting point. This represents the path length calculated using the Reeds-Shepp curve. This represents the set of all Reeds-Shepp curves. Indicates the first The length of the curve in the X direction, Indicates the first The length of the curve in the y-direction; ； in Represents the set of all possible accessible paths. It is a discrete path, where N is the total number of nodes. This represents the coordinates of the k-th node.

2. The path planning method for autonomous berthing of unmanned surface vessels according to claim 1, characterized in that, Unmanned surface vessels follow the following two-dimensional kinematics: ； in The maximum curvature of the hull during motion is represented by L, and the length of the kinematic element is represented by L. This indicates the maximum heading angle of the unmanned surface vessel at the current moment. This represents the difference between two nodes in the x-direction. This represents the difference between two nodes in the y-direction.

3. The path planning method for autonomous berthing of unmanned surface vessels according to claim 1, characterized in that, Before executing step S3, step A also needs to be executed: obtain the turning points and starting point range in the first path, and optimize the path for the turning points and starting point range. The method for identifying turns is as follows: obtain the turning angle of the unmanned surface vessel at the node; if the turning angle is greater than the set angle, it indicates that the node is a turn. The formula for obtaining the turning angle is as follows: ； in This represents the coordinates of the next node after the i-th node. Represents the coordinates of the i-th node; Specifically, path optimization at turning points and within the starting area involves: Insert virtual control nodes at the turning points and in the starting area; After the virtual control node is inserted, a safe pipeline is generated for the path after the inserted node using Euclidean distance information, constraining subsequent path optimization to not be performed outside the safe pipeline.

4. The path planning method for autonomous berthing of unmanned surface vessels according to claim 3, characterized in that, After performing step A, step B also needs to be performed; Step B involves smoothing the first path using the conjugate gradient method. The conjugate gradient function is as follows: in It is the minimum obstacle distance. It is the minimum safe distance from obstacles. It is a vector pointing from the (i-1)th node to the (i-1)th node. It is the reciprocal of the Euclidean distance from the i-th node to the j-th target point. These are the weighting coefficients, , representing the angular change between adjacent path points; This represents the coordinates of the j-th target location. Indicates distance from the current node The coordinates of the obstacle with the smallest distance.

5. The path planning method for autonomous berthing of unmanned surface vessels according to claim 1, characterized in that, The specific steps for real-time collision detection of the actual path in step S4 are as follows: Get the current state of all static and dynamic objects; Based on dynamic motion model and control input Predicted time interval The trajectory of dynamic objects within; Obtain the minimum Euclidean distance from the unmanned surface vessel to each object. If there is a minimum Euclidean distance of 0, it indicates that the unmanned surface vessel is about to collide.

6. The path planning method for autonomous berthing of unmanned surface vessels according to claim 1, characterized in that, The specific steps in step S4 for adjusting the actual path based on the collision detection results are as follows: If the collision detection result is that no collision has occurred, then continue driving along the original actual path; If the collision detection result indicates that a collision is imminent, then obtain the collision time TTC; The obstacle avoidance cutoff time is determined based on the collision time TTC and the maximum reserved obstacle avoidance time. On the Frenét coordinates generated based on Dijkstra, a series of optimized paths are generated from the obstacle avoidance cutoff time points using a fifth-order polynomial. Design an evaluation function, and select the two optimization paths of different classes with the largest evaluation function as the second path; Obtain the obstacle density on the two second paths, and select the second path with the lower obstacle density as the final adjustment path.

7. A path planning system for autonomous berthing of unmanned surface vessels, using the path planning method for autonomous berthing of unmanned surface vessels as described in any one of claims 1 to 6, characterized in that, It includes a rasterization module, a preliminary search module, a path generation module, and a path local optimization module; The rasterization module is used to rasterize the feasible space; The preliminary search module is used to set the moored position as the starting point and the current position as the ending point, and to obtain the first path by searching using the hybrid A* method; The path generation module is used to reverse the first path to obtain the actual path; The path local optimization module is used to perform real-time collision detection on the actual path and adjust the actual path based on the collision detection results.

Images