Offshore path planning system and method for unmanned ship

Abstract

The invention relates to the technical field of ship control, in particular to an unmanned ship seaborne path planning system and method.The planning system comprises a vision module, a laser radar, a Beidou data receiving module, an inertial navigation system, a log, a data switch, an industrial personal computer, an encoder, a propelling device and a controller; the industrial personal computer is connected to the Beidou data receiving module, the inertial navigation system, the vision module, the laser radar and the data switch, the log is connected to the data switch, the encoder is connected to the propulsion device, the controller is connected to the encoder, the vision module is installed on the front portion of the unmanned ship, and the laser radar is installed on the front portion of the unmanned ship. The propelling device is installed on the rear portion of the unmanned ship. According to the invention, the technical problem that the path planned by the one-way path planning method of the unmanned ship in the prior art has many break points and is not smooth is solved, the calculation efficiency of the A star algorithm is improved, the calculation time is reduced, and the path planning speed is accelerated.

Description

Technical Field

[0001] This invention relates to the field of ship control technology, specifically to an unmanned vessel maritime path planning system and method. Background Technology

[0002] In the actual movement of unmanned surface vessels (USVs), we often need to consider many factors, such as reefs, fishing boats, and other obstacles at sea. Current path planning methods for mobile robots include A* search (A* algorithm, a pathfinding and graph traversal algorithm), artificial potential field method, and ant colony algorithm, among many others. Paths planned by the artificial potential field method often get stuck in local optima, while the A* algorithm is faster than the ant colony algorithm. Therefore, some existing unmanned devices often achieve path planning by improving the A* algorithm or integrating it with other path planning algorithms.

[0003] Chinese patent CN113359718B, entitled "Method and Device for Fusion of Global Path Planning and Local Path Planning for Mobile Robots," integrates node distance information into a heuristic function with coefficients of an exponentially decaying function to accelerate the global optimization search. It also considers the influence of previous generation parent nodes, reducing the probability of nodes going back and forth during the search. After obtaining the initial path, gradient descent is used to smooth the path. The improved global path planning algorithm (A* algorithm) and the local path planning algorithm are fused to complete the path planning for the mobile robot. However, applying this patented method to maps with large amounts of data, simply changing the coefficients in the heuristic function to accelerate the global optimization search, suffers from the high computational cost and low computational efficiency of the A* algorithm.

[0004] Chinese patent CN117109597A, entitled "UAV Path Planning Method and Device Based on Improved A* Algorithm," describes an improved A* algorithm that utilizes obstacle weight coefficients from a 3D grid map to weight the evaluation function, enabling the algorithm to plan a better path based on the number of obstacles. However, this algorithm uses a traditional unidirectional path planning method, resulting in relatively slow path planning speed.

[0005] Chinese patent CN116642510A, entitled "A Vehicle Navigation Method Based on an Improved A* Algorithm," improves the A* algorithm by incorporating steering costs into its cost function. While this method reduces the number of inflection points and broken lines in the planned path, the final route still contains broken lines, potentially causing sharp turns for the unmanned vessel.

[0006] In summary, given the problems of high computational cost and low computational efficiency of the A* algorithm for large-scale maps, slow speed of unidirectional path planning methods, and numerous and uneven planned paths, it is essential to research an unmanned surface vessel (USV) path planning method that is computationally efficient, fast in path planning, and has fewer and smoother paths. Summary of the Invention

[0007] The purpose of this invention is to provide an unmanned surface vessel (USV) maritime path planning system and method to solve the technical problem that existing unidirectional path planning methods for USVs have many inflection points and are not smooth. The specific technical solution is as follows:

[0008] This invention provides an unmanned surface vessel (USV) maritime path planning system, comprising a vision module, a lidar, a BeiDou data receiving module, an inertial navigation system, a odometer, a data exchange, an industrial control computer, an encoder, a propulsion device, and a controller. The vision module, lidar, BeiDou data receiving module, inertial navigation system, odometer, data exchange, industrial control computer, encoder, propulsion device, and controller are all installed on the USV. The industrial control computer is connected to the BeiDou data receiving module, inertial navigation system, vision module, lidar, and data exchange. The odometer is connected to the data exchange. The encoder is connected to the propulsion device. The controller is connected to the encoder. The vision module is installed at the front of the USV, the propulsion device is installed at the rear of the USV, and the inertial navigation system is installed at the center of the USV.

[0009] This invention also provides a path planning method using the unmanned vessel maritime path planning system described above, comprising the following steps: Step 1, obtaining an initial path, specifically: using a multi-scale map method to reduce the map data, and using an improved A* algorithm to first complete the initial path planning on the small-scale map data to obtain the initial path; the improved A* algorithm includes dynamically weighting the heuristic function in the A* algorithm and optimizing the search neighborhood in the A* algorithm;

[0010] Step 2: Obtain the optimized path. Specifically, the improved A* algorithm is used to optimize the large-scale map data to obtain the optimized path. During the optimization process, the neighborhood search optimization method is used to remove redundant nodes in the map, and the Bézier curve method is used for smoothing.

[0011] Step 3: Obtain the final path. Specifically, the unmanned surface vessel (USV) travels along the optimized path obtained in Step 2. During the journey, obstacles are identified using a vision module and lidar. The Monte Carlo particle filter algorithm is used to determine the USV's orientation. The BeiDou data receiving module is used to determine the USV's position. The encoder monitors the rotation speed of the propulsion device, and the controller controls the rotation speed of the propulsion device based on the data fed back by the encoder. The travel distance and speed information of the USV are obtained through a speedometer. The DWA algorithm is used for local path planning to avoid obstacles. The angle, speed, and attitude information of the USV are updated in real time based on the inertial navigation system data to obtain the final path of the USV's sea navigation.

[0012] A further improvement of the unmanned vessel maritime path planning method of the present invention is that the path search is completed in both steps one and two by a bidirectional search method. The bidirectional search method searches from the starting point to the ending point, and at the same time searches from the ending point to the starting point. If either of the two search methods completes the path planning first, then the path planning ends.

[0013] A further improvement of the unmanned vessel maritime path planning method of this invention lies in the multi-scale mapping method, which uses the original raster map data as raster map data of scale M1. Based on the raster map data of scale M1, every 16 graticules are merged into one raster to form a raster map data of scale M2. Based on the raster map data of scale M2, every 16 graticules are merged into one raster to form a raster map data of scale M3. Based on the raster map data of scale M3, every 16 graticules are merged into one raster to form a raster map data of scale M4. Based on the raster map data of scale M4, every 16 graticules are merged into one raster to form a raster map data of scale M5. Based on the raster map data of scale M5, every 16 graticules are merged into one raster to form a raster map data of scale M6.

[0014] Using the previously obtained raster map data as the source of the next raster map data, we obtain raster map data with a maximum size of M6. The obtained raster map data of different scales {M1, M2}, {M3, M4}, and {M5, M6} are then divided into small, medium, and large scale map data.

[0015] A further improvement to the unmanned surface vessel (USV) maritime path planning method of this invention lies in the dynamic weighting of the heuristic function in the A* algorithm. When the A* algorithm traverses a large number of nodes, the weight of the heuristic function is increased to accelerate the search speed of the algorithm and find a path to the destination. The improved formula for calculating the total cost function of the A* algorithm is as follows:

[0016]

[0017] Where (x1, y1) are the coordinates of the current node n; (x o y o (x) represents the coordinates of the starting point; a y a ) represents the coordinates of the endpoint; c represents the distance threshold between the unmanned vessel and the obstacle; w represents the number of obstacles encountered by the unmanned vessel; p represents the dynamic weighting coefficient of a single obstacle; f(n) represents the total cost function; g(n) represents the actual cost function from the starting node to the current node n; u represents the dynamic weighting function that weights the heuristic function; and h(n) represents the heuristic function from the current node n to the target node.

[0018] A further improvement of the unmanned vessel maritime path planning method of the present invention is that, when optimizing the search neighborhood in the A* algorithm, unnecessary search neighborhoods are removed during the process of traversing nodes in the A* algorithm.

[0019] A further improvement of the unmanned vessel maritime path planning method of the present invention lies in the following: the method of searching the neighborhood optimization is as follows: starting from the starting point, it is determined whether the next node is an obstacle node. If not, the node is added to the node set for path planning. If so, it is determined whether the next node is an obstacle, until the next node is not an obstacle node. The non-obstacle start and end nodes in this path are grouped together and added to the path planning set. The search neighborhood optimization steps are repeated until the node is the end point.

[0020] The application of the technical solution of the present invention has the following beneficial effects:

[0021] This invention relates to an unmanned surface vessel (USV) maritime path planning system and method. By smoothing the optimized path using an improved A* algorithm and the Bézier curve method, the number of vertices in the final path can be reduced, and the path length can be shortened. This solves the technical problem of numerous vertices and uneven path smoothing in existing unidirectional path planning methods for USVs. This invention uses a multi-scale map method to process map data, improving the computational efficiency of the A* algorithm, reducing computation time, and accelerating path planning. Optimizing the search neighborhood further enhances the computational efficiency and running speed of the A* algorithm, and using a bidirectional path search method accelerates the path search speed.

[0022] In addition to the objectives, features, and advantages described above, the present invention has other objectives, features, and advantages. The invention will now be described in further detail with reference to the figures. Attached Figure Description

[0023] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0024] Figure 1 This is a schematic diagram of the structure of the unmanned vessel maritime path planning system of the present invention;

[0025] Figure 2 This is a flowchart of the multi-scale map method for the unmanned vessel maritime path planning method of the present invention;

[0026] Figure 3 This is a flowchart of the unmanned vessel maritime path planning method of the present invention;

[0027] Figure 4 This is a path map planned by the improved A* algorithm of the unmanned vessel maritime path planning method of this invention;

[0028] Figure 5 It is the path diagram planned by the existing A* algorithm.

[0029] Among them, 1. Unmanned vessel; 2. Controller; 3. LiDAR; 4. Propulsion device; 5. Encoder; 6. Industrial control computer; 7. Data transfer machine; 8. Industrial camera; 9. Industrial lens; 10. Fixed bracket; 11. Inertial navigation system; 12. Beidou data receiving module; 13. Speed ​​log. Detailed Implementation

[0030] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0031] See Figures 1-5 As shown, an unmanned surface vessel (USV) maritime path planning method includes a vision module, a lidar 3, a BeiDou data receiving module 12, an inertial navigation system 11, a speedometer 13, a data exchange 7, an industrial control computer 6, an encoder 5, a propulsion device 4, and a controller 2. The vision module, lidar 3, BeiDou data receiving module 12, inertial navigation system 11, speedometer 13, data exchange 7, industrial control computer 6, encoder 5, propulsion device 4, and controller 2 are all installed on the USV 1. The industrial control computer 6 is connected to the BeiDou data receiving module 12, inertial navigation system 11, vision module, lidar 3, and data exchange 7. The speedometer 13 is connected to the data exchange 7. The encoder 5 is connected to the propulsion device 4. The controller 2 is connected to the encoder 5. The vision module is installed at the front of the USV 1, the propulsion device 4 is installed at the rear of the USV 1, and the inertial navigation system 11 is installed at the center of the USV 1.

[0032] Specifically, the vision module consists of two industrial cameras 8, located on opposite sides of the front of the unmanned vessel 1. Each camera is mounted on the unmanned vessel 1 via a mounting bracket 10. Each camera has an industrial lens 9 for focusing and imaging, connected to the camera via a threaded connection. The camera acquires images and depth information within its current field of view and is directly connected to the network port of an industrial control computer 6. The connection method can be either a wired connection or a wireless communication connection. The industrial control computer 6 is a dedicated vision computer used to run image processing algorithms, improving program processing efficiency. Four industrial control computers 6 are installed at intervals at the rear of the unmanned vessel 1.

[0033] Two controllers 2 are installed at intervals in the middle of the unmanned vessel 1. Controller 2 acquires data from encoder 5, which in turn acquires the rotational speed of propulsion device 4. Encoder 5 is connected to a high-speed pulse counter in controller 2. Controller 2 performs feedback control of the rotational speed of propulsion device 4. Propulsion device 4 can be a propeller from existing technology. Multiple propulsion devices 4 are installed at intervals at the rear of the hull. LiDAR 3 is used to detect depth information around the hull and is directly connected to the network port of industrial control computer 6. Inertial navigation system 11 is used to detect the pose of unmanned vessel 1 in various directions in real time. Beidou data receiving module 12 can locate the position of unmanned vessel 1 in real time, obtaining its real-time position in the world coordinate system.

[0034] The industrial control computer 6 is used to process all the data from the lidar 3. The industrial control computer 6 and the speedometer 13 are directly connected to the network port of the data switch 7. The data switch 7 and the industrial control computer 6 are connected by a network cable. All the processed sensor data and the speedometer 13 data are transmitted to the industrial control computer 6. The industrial control computer 6 uses the vision and lidar 3 data to complete the construction of a local map and uses a path planning algorithm to complete the path planning of the unmanned vessel 1 on the map.

[0035] An industrial camera 8, an industrial lens 9, and a lidar 3 are mounted and fixed on a fixed bracket 10. These components acquire depth information around the ship's hull, which is used for local map construction and obstacle detection, forming a vision and lidar detection system. The original phrase "by writing a camera calibration program in the host computer software system" is modified to "using the Camera Calibrator tool in MATLAB software (commercial mathematical software) to calibrate the camera," determining and saving the intrinsic and extrinsic parameters of the industrial camera 8. The lidar surface calibration program module in the system is used to calibrate the laser scanning surface projected by the lidar, and the parameters of the fitted lidar surface are saved.

[0036] In the host computer software settings, the world coordinate system of the vision and lidar detection system is specified through the calibration board, so that the values ​​extracted and calculated each time are under the same world coordinate system. In the system, the zero-position calibration program module is used to calibrate the stable state of the unmanned vessel 1, determine the detection benchmark, and reduce detection deviation.

[0037] The operating speed of the propulsion device 4 is detected in real time by a rotary encoder 5 connected to the controller 2.

[0038] The travel distance and speed information of the unmanned vessel 1 are obtained through the log 13. The travel distance information can be used to determine the specific travel route of the unmanned vessel 1 on the map, and the speed information can be used as speed control feedback for the unmanned vessel 1.

[0039] The inertial navigation system 11, connected to the industrial control computer 6, can use the angle, speed, and attitude information data of the hull in six directions to perform autonomous positioning of the unmanned vessel 1 using the Monte Carlo particle filtering algorithm (which uses a set of particles to represent probabilities and can be used on any form of state-space model), thereby realizing autonomous control of the unmanned vessel 1's motion direction.

[0040] The Beidou data receiving module 12, which is connected to the industrial control computer 6, locates the unmanned vessel 1 in real time, accurately marks the position of the unmanned vessel 1 on the map, and thus accurately obtains the movement trajectory of the unmanned vessel 1 on the map.

[0041] This invention also provides a path planning method using the unmanned vessel maritime path planning system described above, comprising the following steps: Step 1, obtaining an initial path, specifically: using a multi-scale map method to reduce the map data, and using an improved A* algorithm to first complete the initial path planning on the small-scale map data to obtain the initial path; the improved A* algorithm includes dynamically weighting the heuristic function in the A* algorithm and optimizing the search neighborhood in the A* algorithm;

[0042] Step 2: Obtain the optimized path. Specifically, the improved A* algorithm is used to optimize the large-scale map data to obtain the optimized path. During the optimization process, the neighborhood search optimization method is used to remove redundant nodes in the map, and the Bézier curve method is used for smoothing to reduce the number of vertices in the final path and shorten the path length.

[0043] Step 3: Obtain the final path. Specifically, the unmanned vessel 1 travels along the optimized path obtained in Step 2. During the journey, obstacles are identified using the vision module and lidar 3. The Monte Carlo particle filter algorithm is used to determine the orientation of the unmanned vessel 1. The Beidou data receiving module 12 is used to determine the position of the unmanned vessel 1. The encoder 5 monitors the rotation speed of the propulsion device 4. The controller 2 controls the rotation speed of the propulsion device 4 based on the data fed back by the encoder 5. The travel distance and speed information of the unmanned vessel 1 are obtained through the speedometer 13. The DWA algorithm (Dynamic Window Approach) is used for local path planning to dynamically avoid obstacles. The angle, speed, and attitude information of the unmanned vessel 1 are updated in real time based on the data from the inertial navigation system 11 to obtain the final path of the unmanned vessel 1's sea navigation.

[0044] The Direct Path Avoidance (DWA) algorithm is used for local path planning and dynamic obstacle avoidance in mobile robots. It primarily samples multiple sets of velocities in the velocity space and simulates the trajectories of these velocities over a certain time period. An evaluation function is designed to evaluate the simulated trajectories, and the optimal trajectory corresponding to the simulated trajectory is selected to drive the robot's motion. Considering the limitations of velocity and acceleration, the DWA algorithm only considers collision-free trajectories; therefore, the sampled velocities form a dynamic window, within which the velocities are the actual speeds the robot can achieve.

[0045] Preferably, both steps one and two use a bidirectional search method to search for the path. The bidirectional search method searches from the starting point to the ending point, and simultaneously searches from the ending point to the starting point. If either search method completes the path planning first, the path planning ends.

[0046] like Figure 2 As shown, the preferred multi-scale mapping method involves using the original raster map data as raster map data at scale M1. Based on the raster map data at scale M1, every 16 graticules are merged into one raster to form a raster map data at scale M2. Based on the raster map data at scale M2, every 16 graticules are further merged into one raster to form a raster map data at scale M3. Based on the raster map data at scale M3, every 16 graticules are further merged into one raster to form a raster map data at scale M4. Based on the raster map data at scale M4, every 16 graticules are further merged into one raster to form a raster map data at scale M5. Based on the raster map data at scale M5, every 16 graticules are further merged into one raster to form a raster map data at scale M6.

[0047] Using the previously obtained raster map data as the source of the next raster map data, we can obtain raster map data with a maximum size of M6. The obtained raster map data of different scales {M1, M2}, {M3, M4}, {M5, M6} can be divided into small, medium, and large scale map data.

[0048] Figure 2 The flowchart for the multi-scale mapping method is as follows: First, construct a global raster map, establish a multi-scale map set, select a suitable large-scale map, and use the improved A* algorithm to plan the final path in the large-scale map. Map the final path to the corresponding position in the small-scale map, and determine whether there are any impassable points in the final path. If so, save the first and last points of the impassable path as the start and end points as a combination, and then obtain multiple combinations. Use the start and end points in the combination to plan the path again using the improved A* algorithm on the small-scale map to obtain a new path set U. Then, attach the paths in set U to the corresponding positions in the final path plan, perform node optimization, and then end. If not, proceed directly to node optimization.

[0049] The steps to improve the A* algorithm are as follows:

[0050] (1) Dynamically weight the heuristic function in the A* algorithm. When the unmanned vessel 1 is far from the destination or there are many obstacles, the A* algorithm needs to traverse a large number of nodes to complete path planning. However, this path planning speed is relatively slow. Therefore, the weight of the heuristic function should be increased to speed up the algorithm's search and find a path to the destination. When the unmanned vessel 1 is close to the destination or there are few obstacles, the algorithm traverses relatively few nodes, allowing it to find the shortest path first. In this case, the weight of the heuristic function should be reduced. The formula for calculating the total cost function of the improved A* algorithm is as follows:

[0051]

[0052] Where (x1, y1) are the coordinates of the current node n; (x o y o (x) represents the coordinates of the starting point; a y a ) represents the coordinates of the endpoint; c represents the distance threshold between the unmanned vessel 1 and the obstacle; w represents the number of obstacles encountered by the unmanned vessel 1; p represents the dynamic weighting coefficient of a single obstacle; f(n) represents the total cost function; g(n) represents the actual cost function from the starting node to the current node n; u represents the dynamic weighting function that weights the heuristic function; and h(n) represents the heuristic function from the current node n to the target node.

[0053] This paper describes how to write the A* algorithm and its improved version using Python. A comparison of the total cost function between the improved and original A* algorithms is also presented. Figure 4 and Figure 5 As shown.

[0054] Both the improved and original A* algorithms use a node cost of 63.6, but the improved algorithm takes 3546.7 ms, while the original algorithm takes 4083.4 ms. In summary, this demonstrates that the improved A* algorithm performs better in pathfinding compared to the original algorithm.

[0055] (2) Optimize the search neighborhood in the A* algorithm. During the process of the unmanned vessel 1 from the starting point to the end point, there will be a large number of unnecessary search nodes. Therefore, during the process of the A* algorithm traversing the nodes, it is necessary to remove some unnecessary search neighborhoods, which can greatly increase the speed of the A* algorithm search path and increase the computational load of the A* algorithm.

[0056] (3) The path planned by the A* algorithm is smoothed. Since there are often a lot of inflection points and sharp turns in the path planned by the A* algorithm, the unmanned ship 1 often turns suddenly. Therefore, the Bézier curve algorithm is used to smooth the final path, which reduces the number of inflection points in the final path and shortens the path length.

[0057] (4) An improvement to the A* algorithm is adopted by using a bidirectional search method, that is, searching from the starting point to the ending point, and simultaneously searching from the ending point to the starting point. The path planning ends as soon as either of the two search methods completes the path planning first. Therefore, the bidirectional path search method can complete path planning faster than the traditional unidirectional path search method.

[0058] In summary, by combining all the advantages of the improved A* algorithm with the original A* algorithm in path planning, the results are as follows: Figure 4 and Figure 5 As shown, the improved A* algorithm directly improves the efficiency of path search by 77.4% compared to the original A* algorithm, and the path planned by the improved A* algorithm is smoother.

[0059] The specific planning steps are as follows:

[0060] A global raster map is constructed using published electronic nautical chart data in a world coordinate system.

[0061] The constructed map data was processed using a multi-scale mapping method to obtain a multi-scale map dataset {M1, M2, M3, M4, M5, M6}.

[0062] An optimal travel path for the unmanned vessel 1 is planned using the improved A* algorithm on grid map data at a scale of M6.

[0063] The path planned on a large-scale map of scale M6 is mapped layer by layer to small-scale map data of scale M1. During each mapping layer, a neighborhood search optimization method is used: starting from the starting point, each node is checked to see if it is an obstacle. If it is not an obstacle, it is added to the path planning node set. If it is an obstacle, the process continues until the next non-obstacle node is reached. The starting and ending nodes of this non-obstacle segment are grouped together and added to the path planning set. This process is repeated until the endpoint is reached. Similarly, a search is performed from the endpoint back to the starting point.

[0064] Using multiple combinations of start and end point nodes from the node set for path planning, an improved A* algorithm is applied to plan paths between these multiple sets of start and end points. The planned optimal paths are then placed into a set U of path points. A specific example of the improved A* algorithm for multi-scale mapping is as follows: Figure 2 As shown.

[0065] The final path is smoothed using the Bézier curve algorithm.

[0066] During the movement of the unmanned surface vessel 1, data from the industrial camera 8 and the lidar 3 are used to determine whether there are dynamic obstacles around the unmanned surface vessel 1. If there are dynamic obstacles, the vessel stops and waits. If the waiting time exceeds the limit, the DWA algorithm is used for local path planning. The improved A* algorithm is fused with the DWA algorithm to complete the path planning of the unmanned surface vessel 1. Figure 3 As shown.

[0067] The Monte Carlo particle filter algorithm is used to process the data measured by the inertial navigation system 11 to determine the motion direction of the unmanned vessel 1.

[0068] The Beidou data receiving module 12 is used to locate the unmanned vessel 1 and determine its trajectory.

[0069] The angle, speed, and attitude information of the unmanned vessel 1 are updated in real time based on the data from the inertial navigation system 11.

[0070] Determine whether unmanned vessel 1 has reached the target point. If it has not reached the target point, return to the path planning during the movement of unmanned vessel 1 and start executing the subsequent steps until unmanned vessel 1 reaches the target point.

[0071] Bézier curves are mathematical curves widely used in 2D graphics applications. A Bézier curve consists of line segments and nodes; nodes are draggable pivot points, and line segments act like stretchable rubber bands. This characteristic allows vector graphics software to accurately draw the curve.

[0072] The Monte Carlo particle filter (PF) algorithm is based on Monte Carlo methods. It uses a set of particles to represent probabilities and can be applied to any form of state-space model. Its core idea is to express the distribution of states by randomly selecting state particles from the posterior probability; it is a type of ordinal importance sampling method.

[0073] Figure 3 The flowchart for path planning is as follows: First, a global grid map is constructed, and multi-scale maps are established. A path is planned using the improved A* algorithm on the large-scale map. The final path plan from the large-scale map is mapped to the small-scale map. It is determined whether the mapped path passes through obstacles on the small-scale map. If so, the starting and ending points of the path passing through obstacles are used to plan the path again using the improved A* algorithm on the small-scale map. The planned path is then stitched to the corresponding positions of the mapped path, and the path is optimized using the Bézier curve algorithm. If not, the path is directly optimized using the Bézier curve algorithm. Next, the surrounding obstacles are scanned based on sensor data to determine whether there are dynamic obstacles on the path of unmanned vessel 1. If not, the process continues directly on unmanned vessel 1. If yes, stop and wait, and determine if the waiting time has expired. If no, return to the stop and wait state. If yes, determine if the dynamic obstacle has left. If no, use the DWA algorithm to plan a local path to bypass the obstacle, and the unmanned vessel 1 continues to travel. If yes, continue directly to the unmanned vessel 1 and determine if there is an unknown static obstacle on the global path. If yes, return to the large-scale map and use the improved A-satellite algorithm to plan the path. If no, update the angle, velocity, and attitude information of the unmanned vessel 1 in real time based on the inertial navigation system data. Update the angle information using the Monte Carlo particle filter algorithm and update the position information using Beidou positioning. Determine if the final target point has been reached. If no, return to determine if there is an unknown static obstacle on the global path. If yes, end.

[0074] This invention relates to an unmanned surface vessel (USV) maritime path planning system and method. By using an improved A* algorithm and the Bézier curve method to smooth the final path, the number of vertices in the final path is reduced, and the path length is shortened. This solves the technical problem of numerous vertices and uneven path smoothing in the existing unidirectional path planning method for USVs. This invention uses a multi-scale map method to process map data, improving the computational efficiency of the A* algorithm, reducing computation time, and accelerating path planning. Optimizing the search neighborhood further enhances the computational efficiency and running speed of the A* algorithm. Using the dynamic weighting formula of the heuristic function in the A* algorithm accelerates path planning. Optimizing the search neighborhood further enhances the computational efficiency and running speed of the A* algorithm. Using a bidirectional path search method accelerates path search. Smoothing the final path using the Bézier curve method reduces the number of vertices and shortens the path length.

[0075] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An unmanned ship sea path planning system, characterized by, The unmanned ship includes a vision module, a laser radar, a Beidou data receiving module, an inertial navigation system, an odograph, a data switch, an industrial computer, an encoder, a propulsion device, and a controller, all of which are installed on the unmanned ship.

2. An unmanned ship sea path planning method, characterized in that, The unmanned ship sea path planning system of claim 1 is used for path planning, including the following steps: Step one: obtaining an initial path, specifically: using a multi-scale map method to reduce map data, using an improved A-star algorithm to complete initial path planning on small-scale map data to obtain an initial path; the improved A-star algorithm includes dynamically weighting the heuristic function in the A-star algorithm and optimizing the search neighborhood in the A-star algorithm; Step two: obtaining an optimized path, specifically: using an improved A-star algorithm to perform optimization processing on large-scale map data to obtain an optimized path, wherein: the search neighborhood optimization method is used to remove redundant nodes in the large-scale map during the optimization processing, and the Bezier curve method is used for smoothing processing; Step three: obtaining a final path, specifically: the unmanned ship travels according to the optimized path obtained in step two, in the process of traveling, the vision module and the laser radar are used to judge obstacles, the Monte Carlo particle filtering algorithm is used to complete directional positioning of the unmanned ship, the Beidou data receiving module is used to complete position positioning of the unmanned ship, the encoder is used to monitor the rotation speed of the propulsion device, the controller controls the rotation speed of the propulsion device according to the data fed back by the encoder, the odograph is used to obtain the distance and speed information of the unmanned ship, and the DWA algorithm is used for local path planning processing to avoid obstacles, the inertial navigation system data is used to update the angle, speed, and attitude information of the unmanned ship in real time, and the final path of the unmanned ship sea navigation is obtained.

3. The unmanned ship offshore path planning method of claim 2, wherein, In steps one and two, the bidirectional search method is used to complete path search, the bidirectional search method is to search from the starting point to the terminal point, and at the same time, search from the terminal point to the starting point, and any one of the two search methods is used to complete path planning, and then the path planning is ended.

4. The unmanned ship offshore path planning method of claim 2, wherein, The multi-scale map method is to take the original raster map data as the raster map data with a scale of M1, combine every 16 grids into one grid based on the raster map data with a scale of M1 to form a raster map data with a scale of M2, continue to combine every 16 grids into one grid based on the raster map data with a scale of M2 to form a raster map data with a scale of M3, continue to combine every 16 grids into one grid based on the raster map data with a scale of M3 to form a raster map data with a scale of M4, continue to combine every 16 grids into one grid based on the raster map data with a scale of M4 to form a raster map data with a scale of M5, and continue to combine every 16 grids into one grid based on the raster map data with a scale of M5 to form a raster map data with a scale of M6; Continue to combine every 16 grids into one grid based on the raster map data with a scale of M4 to form a raster map data with a scale of M5; Continue to combine every 16 grids into one grid based on the raster map data with a scale of M5 to form a raster map data with a scale of M6; Take the raster map data obtained in the previous step as the source of the raster map data in the next step to obtain the raster map data with the maximum scale of M6, and divide the raster map data with different scales of {M1, M2}, {M3, M4} and {M5, M6} into small, medium and large scale map data.

5. The unmanned ship maritime path planning method of claim 2, wherein, When the heuristic function in the A-star algorithm is dynamically weighted, the weight of the heuristic function is increased when the A-star algorithm traverses a large number of nodes, the search speed of the algorithm is accelerated, a path to the destination is found, and the total cost function of the improved A-star algorithm is calculated according to the following formula: wherein (x1, y1) is the coordinate position of the current node n; (x o , o y ) is the coordinate position of the starting point; (x a , a y ) is the coordinate position of the end point; c is the distance threshold between the unmanned ship and the obstacle object; w is the number of obstacle objects encountered by the unmanned ship; p is the dynamic weighting coefficient of a single obstacle object; f(n) is the total cost function, g(n) is the actual cost function from the starting node to the current node n; u is the dynamic weighting function of the weight weighting of the heuristic function; h(n) is the heuristic function from the current node n to the target node.

6. The unmanned ship offshore path planning method of claim 5, wherein, When the search neighborhood in the A-star algorithm is optimized, unnecessary search neighborhoods are removed in the process of traversing nodes in the A-star algorithm.

7. The unmanned ship offshore path planning method of claim 2, wherein, The method of search neighborhood optimization is as follows: starting from the starting point, it is judged whether the next node is an obstacle node, if not, the node is put into the node set of path planning, if yes, it is judged whether the next node is an obstacle, until the next node is a non-obstacle node, the starting node and the ending node of the non-obstacle in this path are grouped into a group, and are put into the path planning set, and the steps of search neighborhood optimization are repeated until the node is the end point.

Images