Path planning method for offshore berthing scene

Abstract

The invention, which belongs to the technical field of unmanned ship path planning, discloses a path planning method for an offshore ship berthing scene, comprising the following steps: S1, acquiring ship attitude information and obstacle position information; s2, setting autonomous berthing task storage location information; s3, pre-parking point information is set; s4, establishing a curve ship mooring path planning model; s5, establishing a ship berthing path smoothing model; s6, sending a path planning result to a control module for execution; s7, cyclically monitoring and feeding back the berthing path in real time, and judging whether to re-plan or not; s8, establishing a warehousing matching model, and judging whether warehousing succeeds or not; according to the invention, when the unmanned ship faces an offshore ship berthing scene, a reliable ship berthing path plan is provided, so that the unmanned ship can flexibly adjust a driving strategy when approaching a port and berth, collision and safety accidents are avoided, the unmanned ship can realize safe, efficient and stable automatic ship berthing operation in an offshore environment, and the ship berthing efficiency is improved. And the task execution efficiency of the unmanned ship is improved.

Description

Technical Field

[0001] This invention belongs to the field of unmanned vessel path planning technology, specifically relating to a path planning method for offshore vessel mooring scenarios. Background Technology

[0002] With the rapid growth of global maritime trade, the demand for ship berthing in ports has increased significantly. Precise navigation and efficient berthing, in particular, have become key technologies for port scheduling and operation. Therefore, providing safe and efficient path planning methods for ships at sea to reduce collision risks and improve port entry efficiency has become an important research direction for current maritime intelligent navigation and port scheduling systems.

[0003] Mooring ships at sea requires not only a smooth path but also various constraints, including safe distances, speed limits, and precise location of port berths. Therefore, a path planning method for the scenario of mooring ships at sea is needed to solve the above problems. Summary of the Invention

[0004] The purpose of this invention is to provide a path planning method for offshore mooring scenarios, in order to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a path planning method for a ship mooring scenario at sea, comprising the following steps: S1. Obtain ship attitude information and obstacle location information; S2. Set autonomous berthing task location information; S3. Set the pre-parking point information; S4. Establish a curved berthing route planning model; S5. Establish a smoothing model for the mooring path; S6. Send the path planning results to the control module for execution; S7. Real-time cyclical monitoring and feedback of the berthing path to determine whether replanning is necessary; S8. Establish an inbound matching model to determine whether the inbound process was successful.

[0006] As a preferred embodiment, in "Step S1", the step of obtaining the ship's attitude information and the obstacle's position information includes the following steps: S11. The ship itself is equipped with a global positioning system, which can obtain the current GPS latitude and longitude in real time and convert it into a Cartesian coordinate system through a coordinate system transformation module; S12. The ship obtains its current heading angle in real time through its own heading calculation module; S13. The ship obtains the boundary position information of all obstacles through the perception and prediction module.

[0007] As a preferred embodiment, in "Step S2", the step of setting the autonomous berthing task location information includes the following steps: Storage location information includes the location, orientation angle, and inbound direction of the storage location, which can be obtained in two ways: (1) The ship is moored in the warehouse. By obtaining the ship's attitude information, the return point is calibrated. The latitude and longitude of the ship's current position are converted into Cartesian coordinates through the coordinate system conversion module. (2) Mark the mooring area and obtain multiple storage location information in the mooring area through the perception and prediction module. Users can select the storage location where the ship is moored through the App interaction to obtain the storage location information.

[0008] As a preferred embodiment: In "Step S3", the step of setting the pre-parking point information includes the following steps: S31. Set the orientation angle of the pre-parking point; S32. Set the location of the pre-parking point.

[0009] As a preferred embodiment, in "Step S4", the step of establishing the curved berthing path planning model includes the following steps: S41. Establish the first curve model; S411. Define the starting point and the target point; S412. Initialize model parameters; S413, Construct a child node generation model; S414, Extended Node; S415, Termination Condition; S42, Path backtracking; S43. Establish the second curve model; S44, Path Combination.

[0010] As a preferred embodiment, in "Step S5", the step of establishing the smoothing model of the mooring path includes the following steps: S51. Segmentation is performed based on the results of the curved mooring path planning. The planning results are segmented according to the same gear level, and those with the same gear level and direction are grouped into the same segment path; S52. Perform path smoothing based on segmentation results; For each segmented path, a discrete point smoothing method is used to smooth the path, ensuring that the path has high feasibility and stability while meeting relevant constraints, making it easy for ships to execute.

[0011] As a preferred embodiment, in "Step S6", the step of sending the path planning result to the control module for execution includes the following steps: The mooring path planning results from the above steps are sent to the control module for execution via the unmanned vessel communication module.

[0012] As a preferred approach, in "Step S7", the step of continuously monitoring and providing feedback on the berthing path in real time to determine whether to perform replanning includes the following steps: Since new information is received from the perception module in each frame, the collision detection of the mooring path in the previous frame needs to be performed again. If there is a collision with an obstacle, steps S4 and S5 are executed to generate a new mooring path and send it to the control module; otherwise, in order to improve computational efficiency, the result of the mooring path planning in the previous frame is reused.

[0013] As a preferred approach, in "Step S8", the step of establishing an import matching model and determining whether the import is successful includes the following steps: S81. Based on the coordinates and direction of the storage location center point, set the length and width of the storage location to generate a rectangular area as the storage location area; based on the coordinates and direction of the ship's center point, as well as the ship's length and width, generate a rectangular area of ​​the ship's hull. S82. Calculate the percentage of the area overlapping between the rectangular area of ​​the hull and the storage area. When the percentage is greater than the set overlap threshold, it indicates that the berthing is successful and the current autonomous berthing task ends.

[0014] Compared with the prior art, the beneficial effects of the present invention are: This invention provides reliable berthing path planning for unmanned surface vessels (USVs) in maritime berthing scenarios, enabling USVs to flexibly adjust their navigation strategies as they approach ports and berths, avoiding collisions and safety accidents. This allows USVs to achieve safe, efficient, and stable automated berthing operations in the maritime environment, improving the mission execution efficiency of USVs. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the overall process of the present invention; Figure 2 This is a flowchart illustrating step S1 of the present invention; Figure 3 This is a flowchart illustrating step S3 of the present invention; Figure 4 This is a flowchart illustrating step S4 of the present invention; Figure 5 This is a flowchart illustrating step S5 of the present invention; Figure 6 This is a flowchart illustrating step S6 of the present invention; Figure 7 This is a flowchart illustrating step S7 of the present invention; Figure 8This is a flowchart illustrating step S8 of the present invention. Detailed Implementation

[0016] The present invention will be further described below with reference to embodiments.

[0017] The following embodiments are used to illustrate the present invention, but should not be used to limit the scope of protection of the present invention. The conditions in the embodiments can be further adjusted according to specific conditions, and simple improvements to the method of the present invention under the premise of the concept of the present invention are all within the scope of protection claimed by the present invention.

[0018] Please see Figure 1-8 This invention provides a path planning method for ship mooring scenarios at sea, comprising the following steps: S1. Obtain ship attitude information and obstacle location information; S2. Set autonomous berthing task location information; S3. Set the pre-parking point information; S4. Establish a curved berthing route planning model; S5. Establish a smoothing model for the mooring path; S6. Send the path planning results to the control module for execution; S7. Real-time cyclical monitoring and feedback of the berthing path to determine whether replanning is necessary; S8. Establish an inbound matching model to determine whether the inbound process was successful.

[0019] In "Step S1", the step of obtaining the ship's attitude information and obstacle position information includes the following steps: S11. The seagoing vessel is equipped with a Global Positioning System (GPS) and can obtain its current GPS latitude and longitude in real time, denoted as S11. The coordinates are converted to Cartesian coordinates using the coordinate system transformation module, denoted as . ; S12. The ship obtains its current heading angle in real time through its own heading calculation module, denoted as... ; S13. The ship obtains the location information of all obstacle boundaries through the perception and prediction module. In "Step S2", the step of setting the autonomous berthing task parking information includes the following steps: Storage location information includes the location of the storage location ( Orientation angle ( ) and the direction of entry (forward, sideways, and reverse); among which, This refers to the specific location coordinates of the target berth. This refers to the orientation of the berth; Storage location information can be obtained in two ways: (1) The ship is moored in the storage position. By acquiring the ship's attitude information, the return point is determined. The latitude and longitude of the ship's current position are converted to Cartesian coordinates through the coordinate system transformation module, which is used as the location of the storage position to be moored. The current driving heading angle is used as the parking space orientation angle. ); (2) Mark the mooring area and obtain multiple storage location information in the mooring area through the sensing module. Users can select the storage location where the ship is moored through the App interaction to obtain the storage location information; In "Step S3", the step of setting the pre-parking point information includes the following steps: S31, Angle of orientation of pre-berthing point : Based on the obtained storage location information, The settings are as follows: (1) When the direction of entry into the warehouse is positive, = ; (2) When the direction of entry into the warehouse is reversed, = ; (3) When the direction of entry into the warehouse is lateral, = , where the rotation angle It can be set according to actual needs; S32, Pre-parking point location : From the location of the storage , towards the angle from the pre-berth point In the opposite direction, extend outwards a certain distance to determine a target point; this point will be the pre-berthing point. ; Let the extended distance be denoted as The distance is usually set to one ship length, but can be adjusted according to actual needs to ensure that the ship can be smoothly moored into the berth. Pre-parking point location The calculation is as follows: ; ; In "Step S4", the step of establishing the curved berthing path planning model includes the following steps: S41. Establish the first curve model and plan the curve from the current position of the ship. to the pre-parking point The mooring route includes the following steps: S411. Define the starting point and the target point: Starting point: The ship's current location and heading angle To describe the initial state of a seagoing vessel; Target point: Pre-parking location Located near the berth, this point serves as the approach point for seagoing vessels. It defines the direction of approach to the berth and prepares the subsequent path, facilitating berthing control. S412. Initialize model parameters: Open List: Initialize using a priority queue (such as a min-heap), starting from... When added, the priority of each node is determined by the cost function. Determined, among which: The value starts from the starting point The actual cost to the current node is calculated using path length, turning angle, and changes in turning angle as costs. Value is the distance from the current node to the target node. The heuristic cost is typically estimated using Euclidean distance or Manhattan distance; Closed List: Used to record nodes that have been expanded, avoiding duplicate searches; S413. Construct a child node generation model: Based on the current coordinates, calculate the coordinates of the point after the movement, and use it as a child node. The specific steps are as follows: Given the coordinates of the current point heading angle distance of movement Steering angle And the captain The motion increment is calculated as follows: ; ; ; The motion information of the child node is as follows: ; S414, Extended Node: Retrieve the node with the lowest cost from the open list and set it as the current node. And move it from the open list to the closed list; for The node first performs equal-interval sampling based on the maximum steering angle, with directions divided into forward and reverse directions, and travels a certain distance. A series of child nodes are generated based on the child node generation model. ; Perform a feasibility check on the child nodes: Check Node to If a node's path collides with an obstacle, discard the node. Calculate the cost of feasible child nodes: calculate from arrive The cost of a node , from the starting point to The cost of a node The sum is used as the starting point to the child nodes. The actual cost, i.e. And calculate from the heuristic function Heuristic cost from node to target point The final total cost of the child nodes is: ; Update or add child nodes: If The node is not in the closed list, or is currently being calculated. If the node's value is lower, update the open list and... A node is added or updated, and its parent node is recorded. ; S415, Termination Condition: During the expansion process, the current condition is first determined. Nodes and target points Is it possible to generate an optimal curve path based on a combination of arcs and straight lines with a fixed radius? If it exists, the path planning is successful, and no further expansion is needed; the process proceeds directly to the path backtracking stage. In determining the current node With the target point Whether a valid curved path can be generated between them includes the following steps: S4151. Determine the start and end point parameters: starting point: Orientation angle is ; Target point: Orientation angle is ; Maximum curvature limit of path ; Minimum turning radius ; S4152, Pose Transformation: Before solving for the trajectory, to facilitate calculation, it is first necessary to transform the poses of the starting point and the target point. The vector from the starting point to the target point is denoted as: ; vector Translate to the origin The starting point is rotated to face the positive x-axis, and the rotated vector is represented as: ; After rotation, the target orientation is updated to After scaling down to 1 based on the minimum turning radius, the initial attitude and target attitude become: ; ; S4153, Six Basic Motion Formulas: The basic motion representation of a curve combination path is as follows: Turn left forward. Turn left. Turn right forward. Turn right Walk straight ahead. Walk straight back; The ship from its current position Based on the above basic movements, the distance of the arc traveled is calculated respectively. (When traveling straight, this represents the straight-line distance) The corresponding posture is as follows: ; ; ; ; ; ; S4154. Calculate all possible curve paths: Combining different motions can yield various curved paths. To improve computational efficiency, three three-segment combinations are considered: SCS, CSC, and CCC. Here, S represents a straight segment, and C represents a curved segment, which can be a left-turning arc (L) or a right-turning arc (R). Through different combinations, multiple feasible curved paths can be obtained. In CSC For example: Find the starting point after pose change. To the target point The trajectory is determined by first substituting the starting pose into the six basic motions mentioned above. In the formula, the arc length is The result is: ; Substitute the result into In the formula, the straight-line distance is... The result is: ; Finally, substitute the result into... In the formula, the straight-line distance is... The result is: ; get Post-motion pose Equivalent to the known target point pose, we can obtain three equations: ; The unknown is in the basic motion formula Solving the three equations, we get... You can get Trajectory under form; To further simplify the calculation, by leveraging symmetry and applying the following three transformations, the formula for solving a certain trajectory can be used to find solutions for other trajectories: Time transformation: By swapping the superscript signs + and - on the trajectory letters, the direction of the ship's movement is reversed. For example, starting from the starting point... To the target point of A trajectory can be traced from the starting point. Time of The trajectory was obtained; Reflection transformation: By swapping the letters L and R, the ship's direction is reversed; for example, from the starting point... To the target point of A trajectory can be traced from the starting point. Time of The trajectory was obtained; Reverse transformation: This involves moving along the original path in the opposite direction, i.e., moving in the reverse order; for example, starting from the starting point... To the target point of A trajectory can be traced from the starting point. Time of The trajectory was obtained.

[0020] S4155. Select the optimal curve path: Select the optimal path from all possible curved paths. The optimal curved path must first meet two conditions: the total path length is less than a set length threshold, and there is no collision between the path and obstacles; Secondly, calculate the cost of the curved path that meets the above conditions. The path cost includes: reverse path length penalty, gear shifting penalty, steering penalty, and steering change penalty. Select the path with the minimum cost as the optimal curved path. If an optimal curved path exists, the path planning ends; otherwise, the operation of expanding nodes in "step S414" is repeated until the optimal curved path is found. S42, Path Backtracking: From the target point Start by backtracking along the parent node of each node until you return to the starting point. Generate the first complete path segment; Store the paths sequentially for later path smoothing and optimization; S43. Establish the second curve model and plan the route from the pre-parking point. to warehouse location The mooring route includes the following steps: S431. Select different planning models according to the direction of entry. When the direction of entry is lateral, select the RS curve model. When the direction of entry is forward or backward, select the straight line model. S432, The direction of entry into the warehouse is lateral: Call the curve path generation process in S415 and set the start and end point parameters as follows: starting point: Orientation angle is ; Target point: Orientation angle is ; Calculate all possible curved paths and select the optimal RS path as the second path segment. The optimal RS path must first ensure that there is no collision between the path and obstacles; secondly, calculate the cost of the RS path that meets the above conditions. The path cost includes: reverse path length penalty, total path length penalty, gear shift penalty, steering penalty, and steering change penalty. Select the path with the minimum cost as the second path segment. S433, The direction of entry into the warehouse is either forward or reverse: To save computation, a straight line is generated between the pre-berthing point and the berthing point, and a safety check is performed. This straight line serves as the second path, which simplifies the berthing process and allows the ship to maintain a stable orientation and attitude when approaching the berth. S44, Path Combinations: By combining the first and second planned routes, the planned rough mooring route is obtained. In "Step S5", the step of establishing the mooring path smoothing model includes the following steps: S51. Segmentation based on the results of curved mooring path planning: The planning results are segmented according to the same gear level, and those with the same gear level and direction are grouped into the same segment path; S52. Perform path smoothing based on segmentation results; For each segmented path, a discrete point smoothing method is used to smooth the path, ensuring that the path has high feasibility and stability while meeting relevant constraints, making it easy for ocean-going vessels to execute. In "Step S6", the step of sending the path planning result to the control module for execution includes the following steps: The mooring path planning results from the above steps are sent to the control module for execution via the unmanned vessel communication module. In "Step S7", the step of continuously monitoring and providing feedback on the berthing path in real time and determining whether to replan includes the following steps: Since new information is received from the perception module in each frame, the collision detection of the mooring path in the previous frame needs to be performed again. If there is a collision with an obstacle, steps S4 and S5 are executed to generate a new mooring path and send it to the control module; otherwise, in order to improve computational efficiency, the result of the mooring path planning in the previous frame is reused. In "Step S8", the step of establishing the database matching model and determining whether the database entry is successful includes the following steps: S81, Based on the center point of the storage location Coordinates and direction Set the length and width of the storage space to generate a rectangular area as the storage space area; based on the center point of the ship... Coordinates and direction As well as the hull length and width, generate a rectangular area for the hull; S82. Calculate the percentage of the area overlapping between the rectangular area of ​​the hull and the storage area. When the percentage is greater than the set overlap threshold, it indicates that the berthing is successful and the current autonomous berthing task ends.

[0021] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art 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 appended claims and their equivalents.

Claims

1. A path planning method for a ship mooring scenario at sea, characterized in that: Includes the following steps: S1. Obtain ship attitude information and obstacle location information; S2. Set autonomous berthing task location information; S3. Set the pre-parking point information; S4. Establish a curved berthing route planning model; S5. Establish a smoothing model for the mooring path; S6. Send the path planning results to the control module for execution; S7. Real-time cyclical monitoring and feedback of the berthing path to determine whether replanning is necessary; S8. Establish an inbound matching model to determine whether the inbound process was successful.

2. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S1", the step of obtaining the ship's attitude information and obstacle position information includes the following steps: S11. The ship itself is equipped with a global positioning system, which can obtain the current GPS latitude and longitude in real time and convert it into a Cartesian coordinate system through a coordinate system transformation module; S12. The ship obtains its current heading angle in real time through its own heading calculation module; S13. The ship obtains the boundary position information of all obstacles through the perception and prediction module.

3. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S2", the step of setting the autonomous berthing task parking information includes the following steps: Storage location information includes the location, orientation angle, and inbound direction of the storage location, which can be obtained in two ways: (1) The ship is moored in the warehouse. By obtaining the ship's attitude information, the return point is calibrated. The latitude and longitude of the ship's current position are converted into Cartesian coordinates through the coordinate system conversion module. (2) Mark the mooring area and obtain multiple storage location information in the mooring area through the perception and prediction module. Users can select the storage location where the ship is moored through the App interaction to obtain the storage location information.

4. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S3", the step of setting the pre-parking point information includes the following steps: S31. Obtain the orientation angle of the pre-parking point; S32. Obtain the location of the pre-parking point.

5. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S4", the step of establishing the curved berthing path planning model includes the following steps: S41. Establish the first curve model; S42, Path backtracking; S43. Establish the second curve model; S44, Path Combination.

6. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S5", the step of establishing the mooring path smoothing model includes the following steps: S51. Segmentation is performed based on the results of the curved mooring path planning. S52. Perform path smoothing based on the segmentation results.

7. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S6", the step of sending the path planning result to the control module for execution includes the following steps: The mooring path planning results from the above steps are sent to the control module for execution via the unmanned vessel communication module.

8. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S7", the step of continuously monitoring and providing feedback on the berthing path in real time to determine whether to replan includes the following steps: Since new information is received from the perception module in each frame, the collision detection of the mooring path in the previous frame needs to be performed again. If there is a collision with an obstacle, steps S4 and S5 are executed to generate a new mooring path and send it to the control module; otherwise, in order to improve computational efficiency, the result of the mooring path planning in the previous frame is reused.

9. The path planning method for a ship mooring scenario at sea according to claim 1, characterized in that: In "Step S8", the step of establishing the database matching model and determining whether the database entry is successful includes the following steps: S81. Based on the coordinates and direction of the storage location center point, set the length and width of the storage location to generate a rectangular area as the storage location area; based on the coordinates and direction of the ship's center point, as well as the ship's length and width, generate a rectangular area of ​​the ship's hull. S82. Calculate the percentage of the area overlapping between the rectangular area of ​​the hull and the storage area. When the percentage is greater than the set overlap threshold, it indicates that the berthing is successful and the current autonomous berthing task ends.

Images