A route planning method, device, equipment and medium for underwater vehicle

Abstract

The present application relates to the technical field of underwater vehicles, and discloses a route planning method, device, equipment and medium for underwater vehicles, wherein the route planning method comprises: first, determining the target access sequence of multiple target passing places of the underwater vehicle in the feasible area; second, allocating path points between the starting point of the underwater vehicle and the multiple target passing places according to the target access sequence, and obtaining multiple path points arranged in order; then, for the multiple path points arranged in order, synchronously performing parallel path planning between every two adjacent path points, and obtaining a route sub-path between every two adjacent path points; finally, merging the sub-paths between every two adjacent path points to obtain the global planning route of the underwater vehicle. By planning the route sub-paths in parallel, the efficiency of the global route planning of the underwater vehicle is improved.

Description

Technical Field

[0001] The present application relates to the technical field of underwater vehicles, and in particular to a route planning method, device, equipment and medium for underwater vehicles. Background Art

[0002] An underwater vehicle is an autonomous or remotely controlled vehicle that can operate in an underwater environment. It is mainly used for detecting, studying, monitoring and exploring the ocean and other water bodies. Among them, autonomous underwater vehicles have the ability of automatic navigation, and path planning is the core component of its automatic navigation function. In a multi-target environment, path planning needs to take into account both global and local optimization to ensure that the vehicle can reach each target point efficiently and smoothly. In order to better adapt to the dynamic and complex underwater environment, path planning algorithms based on intelligent optimization have gradually been introduced, but the efficiency of path planning in related technologies still needs to be improved. Summary of the invention

[0003] The present application provides a route planning method, device, equipment and medium for an underwater vehicle, which solves the technical problem that the efficiency of path planning in the related technology needs to be improved, and achieves the technical effect of quickly planning a path for an underwater vehicle in a multi-target environment.

[0004] In order to achieve the above objectives, the main technical solutions adopted in this application include:

[0005] In a first aspect, an embodiment of the present application provides a route planning method for an underwater vehicle, the method comprising:

[0006] Determine a plurality of target passing locations within a feasible area of ​​the underwater vehicle; wherein the plurality of target passing locations correspond to a target access sequence, and the target access sequence is used to describe global path structure information of a global planned route of the underwater vehicle;

[0007] Allocating path points between the starting point of the underwater vehicle and the multiple target passing locations according to the target access sequence to obtain multiple path points arranged in an orderly manner;

[0008] For the plurality of path points arranged in order, synchronously performing parallel path planning between every two adjacent path points to obtain a route sub-path between every two adjacent path points;

[0009] The global planned route of the underwater vehicle is obtained by merging the sub-paths between every two adjacent path points.

[0010] In a second aspect, an embodiment of the present application provides a route planning device for an underwater vehicle, comprising:

[0011] A target location determination module, used to determine a plurality of target passing locations within a feasible area of ​​the underwater vehicle; wherein the plurality of target passing locations correspond to a target access sequence, and the target access sequence is used to describe the global path structure information of the global planning route of the underwater vehicle;

[0012] A waypoint allocation module, used to allocate waypoints between the starting point of the underwater vehicle and the multiple target passing locations according to the target access sequence, so as to obtain a plurality of waypoints arranged in an orderly manner;

[0013] A parallel path planning module is used to synchronously perform parallel path planning between every two adjacent path points for the plurality of path points arranged in order, so as to obtain a route sub-path between every two adjacent path points;

[0014] The path merging module is used to merge the sub-paths between every two adjacent path points to obtain the global planned route of the underwater vehicle.

[0015] In a third aspect, an embodiment of the present application provides a computer device, comprising a memory and a processor, wherein the memory and the processor are communicatively connected to each other, computer instructions are stored in the memory, and the processor executes the method described in any of the above embodiments by executing the computer instructions.

[0016] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which computer instructions are stored, and the computer instructions are used to enable a computer to execute the method described in any of the above embodiments.

[0017] In the embodiment of the present application, first, the target access sequence of multiple target passing points of the underwater vehicle in the feasible area is determined; second, the path points are allocated between the starting point of the underwater vehicle and the multiple target passing points according to the target access sequence to obtain multiple path points arranged in order; then, for the multiple path points arranged in order, parallel path planning is synchronously performed between every two adjacent path points to obtain the route sub-path between every two adjacent path points; finally, the sub-path between every two adjacent path points is merged to obtain the global planning route of the underwater vehicle. By planning the route sub-path in parallel, the efficiency of the global route planning of the underwater vehicle is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the specific implementation methods of the present application or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0019] Figure 1 A schematic diagram of a route planning module for an underwater vehicle provided in an embodiment of this specification;

[0020] Figure 2 A schematic diagram of a grid environment map provided in an embodiment of this specification;

[0021] Figure 3 A schematic diagram of an electronic nautical chart and a pre-processed chart provided in an embodiment of this specification;

[0022] Figure 4 A schematic diagram of a rasterized nautical chart provided in an embodiment of this specification;

[0023] Figure 5 A schematic diagram of a grid environment map provided in an embodiment of this specification;

[0024] Figure 6 A schematic diagram of nautical chart information stored in a matrix form provided in an embodiment of this specification;

[0025] FIG7 is a schematic diagram of a dense grid division and obstacle calibration method provided in an embodiment of this specification;

[0026] FIG8 is a schematic diagram of a sparse grid division and obstacle calibration method provided in an embodiment of this specification;

[0027] Fig. 9 A flow chart of path planning for an underwater vehicle provided in an embodiment of this specification;

[0028] Fig.10 A flow chart of path planning for an underwater vehicle provided in an embodiment of this specification;

[0029] Fig.11 A flow chart of a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0030] Fig.12 A flow chart of an ant colony algorithm in a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0031] Fig.13 The path planning method for an underwater vehicle provided in the embodiment of this specification Algorithm flow chart;

[0032] Fig.14 A flow chart of a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0033] Fig.15 A flow chart of a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0034] Fig.16 A flow chart of a particle swarm algorithm in a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0035] Fig.17 A flow chart of a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0036] Fig.18 A flow chart of a path planning method for an underwater vehicle provided in an embodiment of this specification;

[0037] Fig.19 A schematic diagram of a path planning device for an underwater vehicle provided in an embodiment of this specification;

[0038] Fig. 20 A schematic diagram of the structure of a computer device provided in an embodiment of this specification. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application.

[0040] When planning a path for an underwater vehicle, it is sometimes necessary to plan a path that can access multiple target locations. This requires taking into account global multi-target point search, local detail optimization, and smooth path generation. The time spent on overall path planning directly affects the execution effect of the task. In an underwater environment, long-term path planning may make it impossible for the vehicle to respond to environmental changes in a timely manner, resulting in unexpected consequences.

[0041] Therefore, this application provides an example of a route planning method for an underwater vehicle, please refer to Figure 1The route planning method is implemented by a global path planning module 100. The global path planning module 100 includes a map loading function 101, a target passing point detection function 102, a path search function 103, and a median filter function 104. Among them, the map loading function 101 includes a sparse grid map loading function 105 and a dense grid map loading function 106, and the path search function 103 includes a target passing point search function 107, a path point search function 108, and a sub-path search function 109.

[0042] The functions of the global path planning module 100 include:

[0043] 1) Receiving and parsing target passing point instructions: The target passing point instructions can be provided by inputting the coordinates of the starting point location and the target passing point location. The global path planning module 100 can receive the input target passing point instructions through the target passing point detection function 102, and parse them. Further, the validity of the starting point and the target passing point is verified, including checking whether the coordinates are within the valid range, whether the target passing point exists in the feasible area, etc., to provide basic data for subsequent path planning.

[0044] 2) Target waypoint sequence formulation: The global path planning module 100 can comprehensively evaluate the path length and smoothness through the target waypoint search function 107, thereby determining the optimal visit sequence of multiple target waypoints and determining the overall navigation route structure.

[0045] 3) Route planning and selection: First, the global path planning module 100 can search for path points in the grid map through the path point search function 108 based on the determined sequence of visiting multiple target passing points. When searching, factors such as obstacles and terrain features in the underwater environment need to be considered. Path points are allocated between the starting point of the underwater vehicle and multiple target passing points according to the target access sequence to obtain multiple path points arranged in an orderly manner. Then, the global path planning module 100 performs refined sub-path planning between adjacent path points through the sub-path search function 109. When planning, the obstacle distribution needs to be fully considered. The sub-path between two adjacent points in the reference path is quickly generated by parallel computing. Further, all sub-paths are merged to form a global planning route.

[0046] 4) Path smoothing: Since the generated global planning route may have discontinuous or jagged features, which will lead to poor stability during navigation, the global path planning module 100 can smooth the path through the median filter function 104 to make the path more continuous and smooth, reduce sharp turns and unnecessary path fluctuations, thereby improving navigation stability and performance.

[0047] The process of using the grid method to model the environment is described as an example. Figure 2 As shown, the grid method divides the movement area of ​​the underwater vehicle into a series of square grid units of equal size, and represents obstacles and feasible areas as different grids. The passable state of the grid is represented by binary values, where the grid where the obstacle is located (black area) is set to 1, and the passable grid (white area) is set to 0. The passable neighborhood of each grid is composed of its 8 adjacent grids. The underwater vehicle can choose the next direction of movement from these feasible grids according to specific rules. The distance between adjacent grids is calculated by Euclidean distance, that is, the distance between the midpoints of the grid, and the possible values ​​are 1 or 1.41. The specific steps for environmental modeling using the grid method are:

[0048] 1) Pre-process the electronic chart. You can use Photoshop to convert the electronic chart into black and white pictures, and expand the obstacles (such as Figure 3 As shown); Use the imread function in Matlab software to read the image, and then use the imresize function to resize the image to 100x100;

[0049] 2) Convert the binary image into a logical matrix, where 1 represents land and 0 represents ocean. The processed rasterized sea map is as follows: Figure 4 shown.

[0050] 3) After the grid is segmented, a two-dimensional array is created to store the grid map information. Each grid has a unique corresponding serial number, and the order is as follows: Figure 5 shown.

[0051] 4) Establish an adjacency matrix, such as Figure 6 As shown in the figure, each row of the adjacency matrix stores the access relationship between the corresponding sequence grid and all grids: the relationship values ​​between the obstacle grid and other grids are all 0; the relationship value between the non-obstacle grid and the adjacent feasible grid is 1 or 1.41, and the relationship value with the non-adjacent grid is 0; the relationship value between all grids and themselves is 0.

[0052] It is understandable that in a grid map, the next moving position of the vehicle will only be in a grid whose relation value with the current grid is not 0, that is, the underwater vehicle can only move to adjacent non-obstacle grids, and the step length of each movement is fixed to 1.

[0053] It should be noted that the path planning algorithm in this example can use grid division methods with different resolutions. When the number of grids divided is large, that is, the resolution is high, it is a dense grid. When the number of grids divided is small, that is, the resolution is low, it is a sparse grid. For dense grid division and obstacle calibration, please refer to Figure 7a and Figure 7bFor sparse grid division and obstacle calibration, please refer to Figure 8a , Figure 8b and Figure 8c .

[0054] In the above example, the raster method is used to model the route planning environment of the underwater vehicle, which can intuitively present the environmental information, and the data structure is simple, which is convenient for storage and processing. Compared with related modeling methods, the resulting raster map model is more efficient and easy to implement.

[0055] For the route planning process in the above example, please refer to Fig. 9 , mainly includes the following links:

[0056] 1) Map processing: including map data loading and map modeling. Map data loading: obtain and load the processed data of electronic charts from the human-computer interaction interface, including environmental information and obstacle distribution, to provide basic environmental data for path planning. Map modeling: raster model the environment based on the loaded chart data, use raster maps to digitally represent static obstacles in the environment, generate sparse and dense raster models, and provide support for path planning at different algorithm levels.

[0057] 2) Verification of passing points: Receive the starting point location and target passing point location information entered by the user through the interface, which can be longitude and latitude, and verify the validity of the longitude and latitude information to ensure that the starting point location and the passing target location are within the feasible area.

[0058] 3) Path search, divided into three main steps:

[0059] Determine the target access sequence: In the barrier-free grid map, the access sequence of multiple target points is optimized through the target passing point search function 107 to determine the global path structure.

[0060] Planning path points: In a sparse grid map with obstacles, the path point search function 108 is called to allocate path points and search for a global path from the starting point to the target point.

[0061] Subpath planning: In a dense grid map with obstacles, a subpath search function 109 is used to parallelly and finely plan the subpath between every two path points to ensure that the path is safe and avoids obstacles. All subpaths are combined to obtain the global planning path of the underwater vehicle.

[0062] 4) Path evaluation and smoothness optimization: There can be multiple global planning routes obtained in the path search phase. Users can evaluate and select the generated multiple global planning routes, mainly considering the path length, smoothness and safety, and then apply smoothing technology, such as median filtering, to smooth the selected path, optimize the continuity of the path, reduce sharp turns and unnecessary twists and turns, and make the path smoother. Alternatively, multiple paths can be smoothed first and then selected.

[0063] 5) Upload of global path planning results: Generate the final optimized global planning route, and upload the discrete path node data to the display and control unit of the underwater vehicle for navigation reference and execution of the vehicle.

[0064] For other examples, see Fig.10 The core algorithm of the global path planning module uses particle swarm algorithm at the top level, ant colony algorithm at the middle level, and Algorithm, the algorithm implementation process is as follows:

[0065] 1) Input the starting point of the underwater vehicle and multiple target passing locations, and perform analysis and validity judgment. The target passing locations need to be within the nautical chart range of the underwater vehicle.

[0066] 2) On an obstacle-free grid map, the target access order between multiple target passing points is determined by the particle swarm algorithm (PSO), and an access order sequence including the starting point and multiple target passing points is obtained; it should be noted that in the process of determining the target access order, the distance between the targets needs to be calculated. On an obstacle-free grid map, the straight-line distance between points can be directly calculated by the formula, which takes less time; on an obstacle-ridden grid map, it may be necessary to consider bypassing obstacles, and the distance calculation is more complicated and time-consuming.

[0067] 3) On the sparse grid map, the ant colony algorithm (ACO) is used to allocate path points between the starting point of the underwater vehicle and multiple target passing points according to the target access order, and these path points are inserted into the access order sequence to obtain multiple path points arranged in order. It should be noted that the sparse grid map takes into account the underwater terrain and obstacles. The ant colony algorithm can avoid these factors and search for the optimal path. Compared with the dense grid map, the number of grids is smaller, which can reduce the time consumption of path point allocation.

[0068] 4) On a dense grid map, for multiple path points arranged in order, synchronously pass between every two adjacent path points The algorithm performs parallel path planning to obtain the route sub-path between every two adjacent path points; it should be noted that the dense grid map takes into account the underwater terrain and obstacles. The algorithm can avoid these factors and search for the optimal path.

[0069] It should also be noted that in a scenario where a sparse grid can be converted to four dense grids, some relatively small obstacles are in a sparse grid of the sparse grid map, and the sparse grid is impassable, while on the dense grid map, the obstacle may occupy one or two dense grids. In this way, when planning a sub-path, three or two dense grids are passable, instead of all four being impassable. The dense grid makes a more detailed judgment on the passable status of the nautical chart, which is conducive to planning a better sub-path.

[0070] 5) The sub-paths between every two adjacent path points are merged to obtain the global planning route of the underwater vehicle.

[0071] In the above example, first, the target access order of multiple target passing points is determined in the barrier-free grid map by the particle swarm algorithm. Compared with the target access order of multiple target passing points in the sparse grid with obstacles, the time spent on determining the target access order is reduced by only considering the straight-line distance without considering obstacles. Secondly, the path points are allocated in the sparse grid map with obstacles by the ant colony algorithm to obtain the path points between the starting and ending points and the target passing points. Compared with the path point allocation in the dense grid map with obstacles, the sparse grid reduces the time spent on determining the path points. Then, The algorithm parallelly and finely plans the sub-paths between every two path points in the dense grid map with obstacles, splits the global area into multiple local areas, and simultaneously performs local path optimal planning for each local area, reducing the time consumption of global route planning. At the same time, when the obstacles are small, the dense grid map is used to improve the meticulousness of traffic status judgment. Finally, the sub-paths are merged to obtain the global planning route. This PSO-ACO-based The multi-layer path planning method realizes the complete process from path point analysis, path selection to path optimization, ensuring the security, smoothness and efficiency of global path planning.

[0072] According to an embodiment of the present application, an embodiment of a route planning method for an underwater vehicle is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0073] See also Fig.11 The route planning method for an underwater vehicle provided in an embodiment of the present application comprises the following steps:

[0074] S110: Determine a plurality of target passing locations within a feasible area of ​​the underwater vehicle.

[0075] Among them, a plurality of target passing locations correspond to a target access sequence, and the target access sequence is used to describe the global path structure information of the global planning route of the underwater vehicle.

[0076] The global path planning module of the underwater vehicle is loaded with a grid model of the electronic chart, which has the longitude and latitude information of each grid, and the passable area is marked as the feasible area. The target passing point is the specific location that the underwater vehicle needs to pass when performing the mission.

[0077] Specifically, before route planning, multiple places specified by the user can be obtained. During route planning, the order of visits between multiple places needs to be determined, such as using a particle swarm algorithm to determine the order of visits between multiple places. When the target visit order of multiple target places is determined, the global path structure information of the global planning route is obtained, that is, several places with visit order on the global planning route, including the starting point and the target place, and sometimes the end point.

[0078] S120, allocating path points between the starting point of the underwater vehicle and multiple target passing locations according to the target access sequence, to obtain multiple path points arranged in an orderly manner.

[0079] Specifically, the ant colony algorithm can be used to allocate path points between the starting point of the underwater vehicle and multiple target passing points according to the target access order, thereby obtaining multiple path points arranged in an orderly manner. Fig.12 ,The main process of the ant colony algorithm is as follows:

[0080] 1) Initialization parameters: Set the number of ants, number of iterations, pheromones, pheromone volatility, etc., and randomly place a certain number of ants at the starting position in the problem space. Among them, pheromones can be initialized according to the grid distance on the map and feasibility (such as free space or obstacles) Distribution, the initial pheromone value is set low to avoid ants falling into the local optimum too early.

[0081] 2) Path selection: Each ant starts from the starting point, selects the next path based on the pheromone concentration and path heuristic factor, and updates its position. Path selection probability The calculation formula is as follows:

[0082]

[0083] in, is the pheromone concentration of path i to j, are parameters that control the weights of pheromones and heuristic factors. is the heuristic factor, defined as follows:

[0084]

[0085] in, is the path distance, is the number of turns, is the adjustment factor, which is used to adjust the weights of the two. Used to encourage the selection of shorter paths and paths with fewer turns.

[0086] 3) Pheromone update: When all ants have completed path selection, the pheromones on the path are updated according to the quality of the path. Usually, paths with good quality will leave more pheromones.

[0087] The pheromone update formula is as follows:

[0088]

[0089] in, is the pheromone volatility coefficient, which is used to control the decay rate of pheromone. The definition is as follows:

[0090]

[0091] is the amount of pheromone released by the kth ant, is the path length of the ants, Q is the total amount of pheromones released by the ants, which is a constant.

[0092] 4) Global update: At the end of each iteration, the pheromones on all global paths are updated. By volatilization and attenuation, the concentration of pheromones is reduced and gradually tends to equilibrium, which helps to prevent the algorithm from falling into a local optimal solution too early.

[0093] 5) Termination judgment: When the specified number of iterations is reached or the stopping condition is met, the ant colony algorithm terminates and outputs the optimal solution currently found as the result.

[0094] S130. For a plurality of path points arranged in an orderly manner, synchronously perform parallel path planning between every two adjacent path points to obtain a route sub-path between every two adjacent path points.

[0095] In some cases, the number of multiple path points is recorded as N. If the path planning is performed at each two adjacent path points in sequence to obtain the global planned route, and it is assumed that the path planning time between each two adjacent path points is t, then the route planning time from the first path point to the Nth path point is (N-1) t. However, the requirements for the route planning time are getting higher and higher, such as requiring less than 1s. Therefore, in this embodiment, for multiple path points arranged in order, parallel path planning is performed between each two adjacent path points simultaneously, and the route planning time from the first path point to the Nth path point is shortened to t.

[0096] Specifically, it can be done by The algorithm performs parallel path planning between every two adjacent path points for multiple path points arranged in order. Fig.13 , The main process of the algorithm is as follows:

[0097] 1) Initialization: Setup The initial conditions of the algorithm include the starting point, the end point, and the feasible area and obstacles on the map. The starting point node is added to the "open list" to store the nodes to be explored; the "closed list" is initialized to empty to store the nodes that have been explored.

[0098] 2) Node expansion: Select the node with the lowest comprehensive cost (current optimal node) from the open list for expansion. The comprehensive cost is the sum of the actual cost and the estimated cost of the current node. Remove the node from the open list and add it to the closed list, indicating that the node has been processed.

[0099] 3) Neighbor node check: Check all neighboring nodes of the current node (up, down, left, right and diagonal directions). For each neighboring node, determine whether it is within the valid range of the map and make sure that the node is not an obstacle or is already in the closed list.

[0100] 4) Cost calculation and update: For each adjacent node that meets the conditions, calculate its cost and determine whether the node is already in the open list: If it is not in the open list, add it to the open list, set the current node as its parent node, and record the path cost. If it is already in the open list, check whether the cost of accessing the adjacent node through the current node is lower. If it is lower, update the parent node of the adjacent node to the current node and adjust its cost. The cost calculation formula for each node is:

[0101]

[0102] is the actual cost from the starting node to the current node n, Heuristic estimated cost from current node n to target node.

[0103] 5) Path reconstruction: When the end node is added to the closed list, it means that the optimal path from the starting point to the end point has been found. By tracing back to the parent node of each node, the complete path can be reconstructed from the end point to the starting point.

[0104] 6) Iterative judgment: Repeat the process of node expansion, adjacent node checking and cost updating until the optimal path is found or the open list is empty (indicating no solution).

[0105] 7) Return result: After the algorithm terminates, the optimal path found is output. If the open list is empty and the end point is not found, it means that there is no feasible path. The final path consists of a sequence of nodes from the start point to the end point.

[0106] S140: Merge the sub-paths between every two adjacent path points to obtain a global planned route for the underwater vehicle.

[0107] Specifically, the sub-paths between every two adjacent path points are merged, and the longitude and latitude information of all adjacent path points and the points on the sub-paths between them are merged together in sequence to form the global planning route information of the underwater vehicle.

[0108] In the above embodiment, first, the target access sequence of the underwater vehicle to multiple target passing points in the feasible area is determined; second, the path points are allocated between the starting point of the underwater vehicle and the multiple target passing points according to the target access sequence to obtain multiple path points arranged in order; then, for the multiple path points arranged in order, parallel path planning is synchronously performed between every two adjacent path points to obtain the route sub-path between every two adjacent path points; finally, the sub-path between every two adjacent path points is merged to obtain the global planning route of the underwater vehicle. By planning the route sub-paths in parallel, the efficiency of the global route planning of the underwater vehicle is improved.

[0109] In some embodiments, see Fig.14 , by merging the sub-paths between every two adjacent path points to obtain the global planning path of the underwater vehicle, the steps include:

[0110] S210, obtaining a reference planning path by merging sub-paths between every two adjacent path points;

[0111] S220: Perform median filtering on the path point sequence corresponding to the reference planned path to obtain a global planned route.

[0112] Specifically, through The algorithm plans the path between every two adjacent path points, obtains the sub-path between every two adjacent path points, and merges all the sub-paths to obtain the reference planning path. It is understandable that the reference planning path obtained may have discontinuous or jagged features, which will lead to poor stability during navigation. Therefore, it is necessary to smooth the reference planning path. There are many smoothing techniques, which can be median filtering technology.

[0113] The median filter is a nonlinear smoothing technology that effectively eliminates mutation points or sharp turns in the path and improves the smoothness of the path by performing window processing on the data. The steps are as follows:

[0114] 1) Path point sampling: Sample the reference planning path to form a path point sequence , which are the points that the vehicle will pass through in sequence.

[0115] 2) Apply median filtering: Apply median filtering to the path point sequence, use a sliding window to process each point, and replace the current point with the median value in the window, thereby eliminating noise and uneven points in the path:

[0116]

[0117]

[0118] Where k is the radius of the sliding window, which determines the smoothness of the filter.

[0119] 3) Update the smoothed path: The smoothed path is used as the final global path for the vehicle to execute in actual operation to ensure the smoothness and safety of the navigation process.

[0120] It is understandable that after a median filter is executed, the smoothness of the path needs to be evaluated to ensure that the navigation standard is met. If the evaluation result does not meet the requirements, the smoothness of the path can be improved by adjusting the filter parameters, adding sample points, resampling the path, etc.

[0121] In the above embodiment, the reference planning path is obtained by merging the sub-paths between every two adjacent path points, and the global planning route is obtained by performing median filtering on the path point sequence corresponding to the reference planning path. Median filtering can make the global planning route more continuous and smooth, reduce sharp turns and unnecessary path fluctuations, and thus improve the stability and performance of navigation.

[0122] In some embodiments, see Fig.15 , determining multiple target passing locations within the feasible area of ​​the underwater vehicle, the steps comprising:

[0123] S310, obtaining the starting point location information, the passing point location information and the ending point location information of the underwater vehicle;

[0124] S320, arranging multiple target access sequences based on the starting point location information, the passing point location information and the end point location information, and obtaining multiple target passing points arranged in the target access sequence.

[0125] Specifically, the starting point location information, the passing point location information and the end point location information may be input through the human-computer interaction interface of the underwater vehicle. The location information may be the longitude and latitude. After the global path planning module obtains this information, the particle swarm algorithm may be used to arrange the multi-target access sequence based on the starting point location information, the passing point location information and the end point location information. Fig.16 ,The main process of particle swarm algorithm is as follows:

[0126] 1) Initialization: Set the parameters of the PSO algorithm, including the number of particles, maximum number of iterations, inertia weight, learning factor, etc., and randomly initialize the position and speed of each particle. The position of each particle represents the order of visiting the target point, which is used for path planning optimization.

[0127] 2) Fitness evaluation: Considering the total length, smoothness, and obstacle avoidance performance of the path, the fitness value of each particle is calculated. The fitness function formula is:

[0128]

[0129] in, is the length between path point i and path point j, is a fixed value used to adjust the fitness function to prevent Close to zero leads to singular values, and based on experience, a constant between 0.2 and 1 can be selected. The quality of the current position of each particle is evaluated according to the fitness, and it is compared with the historical optimal position of the particle to update the individual optimal position of the particle.

[0130] 3) Group Optimum Update: Find the current global optimal position among all particles, that is, the best solution in the current group. Record this position as the current group optimal position to provide a reference for the next step.

[0131] 4) Speed ​​and position update: adjust the speed and position of each particle according to the individual optimal position of the particle and the optimal position of the group. Speed ​​update makes the particle move to the historical optimal position and the optimal position of the group, while position update adjusts the position of the particle according to the new speed, so that the group keeps approaching the optimal solution. The formula for updating the speed and position of the particle is:

[0132]

[0133] 5) Among them, is the particle speed, is the position of the particle, is the optimal position of an individual, is the optimal position of the group, Inertia weight, which affects the tendency of particles to maintain their current speed. , The learning factor adjusts the degree to which particles move toward the individual optimal and group optimal positions. , A random number (between 0 and 1) used to increase the randomness of the search.

[0134] 6) Iteration judgment: Check whether the algorithm has reached the preset maximum number of iterations or meets the stop condition. If not, return to the next round of iteration, re-evaluate the fitness and update the state of the particle.

[0135] 7) Termination judgment: When the specified number of iterations is reached or a solution that meets the conditions has been found, the PSO algorithm terminates. At this time, the optimal position of the current group is the optimal solution output by the algorithm.

[0136] 8) Return result: The PSO algorithm returns the optimal solution found, including the best target point visit sequence and the corresponding fitness value.

[0137] In the above embodiment, multiple target access sequences are arranged based on the acquired starting point location information, passing point location information and end point location information of the underwater vehicle to obtain multiple target passing points arranged in the target access sequence, providing a basis for subsequent path planning.

[0138] In some embodiments, multiple target access sequences are arranged based on the starting point location information, the location information, and the end point location information to obtain multiple target passing locations arranged in the target access sequence, including:

[0139] The starting point location information, the passing point location information and the end point location information are added to the barrier-free grid map of the underwater environment where the underwater vehicle is located, and multiple target access sequences are arranged based on the barrier-free grid map to obtain multiple target passing points arranged in the target access sequence.

[0140] Specifically, in the process of obtaining the multi-target access order based on the particle swarm algorithm, the distance between points needs to be calculated. In the barrier-free grid map, the Euclidean distance can be directly calculated using the distance formula. In the barrier grid map, the calculation is much more complicated and, accordingly, much more time-consuming.

[0141] In the above embodiment, the starting point location information, the passing point location information and the end point location information are added to the barrier-free grid map of the underwater environment where the underwater vehicle is located, and the multi-target access sequence is arranged based on the barrier-free grid map. By simplifying the calculation of the distance between targets on the barrier-free grid map, the time consumption of the target access sequence is effectively reduced.

[0142] In some embodiments, see Fig.17 , obtaining the starting point location information, the passing point location information and the end point location information of the underwater vehicle, the steps include:

[0143] S410, in response to a path point input instruction, determining an input starting point location, a plurality of input passing point locations, and an input end point location;

[0144] S420, performing validity check on the input starting point location, multiple input passing point locations and the input end point location to obtain the starting point location information, passing point location information and end point location information; wherein the validity check includes: verifying whether the passing point is within the valid range and whether the passing point is within the feasible area.

[0145] Specifically, when it is necessary to specify the location information for the underwater vehicle to perform a task, the user will be required to enter the path point information on the human-computer interaction interface, including the input of the starting point location, multiple input of the passing point locations and the input of the end point location. The user can input the latitude and longitude information of the starting point location, multiple passing point locations and the end point location. When the global path planning module receives these location information, it will perform validity verification. Since the global path planning module is loaded with the rasterized model of the electronic nautical chart, the model has the latitude and longitude information of each grid, and marks whether each grid has obstacles or is passable. Therefore, on the one hand, it can be verified whether the passing point is within the latitude and longitude range of the loaded rasterized model, and on the other hand, it can be verified whether the passing point is in the passable area of ​​the grid map.

[0146] In some embodiments, path points are allocated between a starting point of an underwater vehicle and a plurality of target passing points in a target access order to obtain a plurality of path points arranged in an orderly manner, and the steps include: in a first grid map of an underwater environment where the underwater vehicle is located, sparse path points are allocated between a starting point and a plurality of target passing points in a target access order to obtain a plurality of path points.

[0147] Synchronously performing parallel path planning between every two adjacent path points to obtain a route sub-path between every two adjacent path points, including: synchronously performing parallel fine path planning between every two adjacent path points in a second grid map of the underwater environment where the underwater vehicle is located to obtain a route sub-path; wherein the resolution of the first grid map is smaller than the resolution of the second grid map.

[0148] Specifically, the ant colony algorithm can be used to allocate sparse path points between the starting point and multiple target passing points in the first grid map of the underwater environment where the underwater vehicle is located according to the target access order to obtain multiple path points. The algorithm synchronously performs parallel fine path planning between every two adjacent path points in the second grid map of the underwater environment where the underwater vehicle is located to obtain the route sub-path.

[0149] In the above embodiment, multiple path points are obtained by performing sparse path point allocation on a relatively sparse first grid map; further, a route sub-path is obtained by synchronously performing parallel fine path planning between every two adjacent path points on a relatively dense second grid map. Compared with the high-resolution second grid map, the sparse first grid map has a smaller number of grids, which can reduce the time consumption of sparse path point allocation. The parallel fine path planning can also reduce the time consumption of sub-path planning, effectively improving the efficiency of global route planning.

[0150] In some embodiments, see Fig.18 , constructing the first grid map and the second grid map in the following manner, the steps comprising:

[0151] S510, obtaining nautical chart data;

[0152] S520, using a sparse grid division method to perform grid modeling on the underwater environment based on the nautical chart data to obtain a first grid map; wherein the first grid map includes information on obstacles in the underwater environment;

[0153] S530, using a dense grid division method to perform grid modeling on the underwater environment based on the nautical chart data to obtain a second grid map; wherein the second grid map includes information on obstacles in the underwater environment.

[0154] The nautical chart data may be a map of the ocean environment, generally a color picture.

[0155] For example, first, the electronic chart needs to be preprocessed. Photoshop software can be used to convert the electronic chart into a black and white picture, and the obstacles are puffed. Secondly, the image is read using the imread function in Matlab software, and then the image is resized to 25x25 using the imresize function. Thirdly, the binary image is converted into a logical matrix, where 1 represents land or obstacles and 0 represents the ocean. In this way, a relatively sparse first grid map is obtained.

[0156] For example, in the above example, if the imresize function is used to resize the image to 100x100, and then the binary image is converted into a logical matrix, where 1 represents land or obstacles and 0 represents the ocean, a relatively dense first grid map is obtained.

[0157] See also Fig.19 The route planning device 600 for an underwater vehicle provided in an embodiment of the present application includes:

[0158] The target location determination module 601 is used to determine a plurality of target passing locations within the feasible area of ​​the underwater vehicle; wherein the plurality of target passing locations correspond to a target access sequence, and the target access sequence is used to describe the global path structure information of the global planning route of the underwater vehicle;

[0159] A waypoint allocation module 602 is used to allocate waypoints between the starting point of the underwater vehicle and multiple target passing locations according to the target access sequence to obtain multiple waypoints arranged in an orderly manner;

[0160] The parallel path planning module 603 is used to synchronously perform parallel path planning between every two adjacent path points for the plurality of path points arranged in an orderly manner, so as to obtain a route sub-path between every two adjacent path points;

[0161] The path merging module 604 is used to merge the sub-paths between every two adjacent path points to obtain the global planned route of the underwater vehicle.

[0162] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0163] The route planning device of the underwater vehicle in this embodiment is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.

[0164] See also Fig. 20 , Fig. 20 is a schematic diagram of the structure of a computer device provided in an embodiment of the present application, such as Fig. 20As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components are connected to each other using different buses for communication, and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in or on the memory to display the graphical information of the GUI on an external input / output device (such as, a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Fig. 20 A processor 10 is taken as an example.

[0165] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

[0166] The memory 20 stores instructions executable by at least one processor 10, so that the at least one processor 10 executes the method shown in the above embodiment.

[0167] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created according to the use of the computer device, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely arranged relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0168] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid state drive; the memory 20 may also include a combination of the above types of memory.

[0169] The computer device also includes an input device 30 and an output device 40. The processor 10, the memory 20, the input device 30 and the output device 40 may be connected via a bus or other means. Fig. 20 The example of connecting through bus is taken in the following.

[0170] The input device 30 can receive input digital or character information, and generate key signal input related to the user settings and function control of the computer device, such as a touch screen, a keypad, a mouse, a track pad, a touch pad, an indicator bar, one or more mouse buttons, a trackball, a joystick, etc. The output device 40 may include a display device, an auxiliary lighting device (e.g., an LED) and a tactile feedback device (e.g., a vibration motor), etc. The above-mentioned display device includes but is not limited to a liquid crystal display, a light emitting diode, a display and a plasma display. In some optional embodiments, the display device may be a touch screen.

[0171] The embodiment of the present application also provides a computer-readable storage medium. The above method according to the embodiment of the present application can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0172] The embodiment of the present application provides a computer program product, which includes computer instructions, which are stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device executes the method of any embodiment of the present application.

[0173] Although the embodiments of the present application are described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present application, and such modifications and variations are all within the scope defined by the appended claims.

[0174] The systems, devices, modules or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0175] For the convenience of description, the above device is described in various units according to their functions. Of course, when implementing the present application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0176] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0177] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0178] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0179] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0180] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0181] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0182] The above is only an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

[0183] Although the embodiments of the present application have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present application, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A route planning method for an underwater vehicle, characterized in that: The method comprises: Determine multiple target waypoints within a feasible area of ​​the underwater vehicle; wherein the multiple target waypoints correspond to a target access sequence, and the target access sequence is used to describe the global path structure information of the global planned route of the underwater vehicle; the determination of multiple target waypoints within the feasible area of ​​the underwater vehicle includes: obtaining the starting point position information, the waypoint position information and the end point position information of the underwater vehicle; adding the starting point position information, the waypoint position information and the end point position information to an obstacle-free grid map of the underwater environment where the underwater vehicle is located, and arranging multiple target access sequences based on the obstacle-free grid map to obtain multiple target waypoints arranged according to the target access sequence; Allocating path points between the starting point of the underwater vehicle and the multiple target passing points according to the target access order to obtain a plurality of path points arranged in an orderly manner; allocating path points between the starting point of the underwater vehicle and the multiple target passing points according to the target access order to obtain a plurality of path points arranged in an orderly manner, comprising: in a first grid map of the underwater environment where the underwater vehicle is located, sparse path point allocation is performed between the starting point and the multiple target passing points according to the target access order to obtain the multiple path points; For the plurality of path points arranged in order, synchronously performing parallel path planning between every two adjacent path points to obtain a route sub-path between the two adjacent path points; the synchronously performing parallel path planning between every two adjacent path points to obtain a route sub-path between the two adjacent path points comprises: synchronously performing parallel fine path planning between every two adjacent path points in a second grid map of the underwater environment where the underwater vehicle is located to obtain the route sub-path; wherein the resolution of the first grid map is smaller than the resolution of the second grid map; The global planned route of the underwater vehicle is obtained by merging the sub-paths between every two adjacent path points.

2. The method according to claim 1, characterized in that The step of merging the subpaths between every two adjacent path points to obtain a global planned route for the underwater vehicle comprises: By merging the sub-paths between every two adjacent path points, a reference planning path is obtained; A median filter is performed on the path point sequence corresponding to the reference planned path to obtain the global planned route.

3. The method according to claim 1, characterized in that The obtaining of the starting point location information, the passing point location information and the ending point location information of the underwater vehicle comprises: In response to a waypoint input instruction, determining an input starting point location, a plurality of input waypoint locations, and an input end point location; The validity of the input starting point position, the multiple input passing point positions and the input end point position is checked to obtain the starting point position information, the passing point position information and the end point position information.

4. The method according to claim 3, characterized in that The validity check includes: verifying whether the passing points are within the valid range and whether the passing points are within the feasible area.

5. The method according to claim 1, characterized in that: The first grid map is constructed in the following way: Obtaining chart data; The underwater environment is rasterized and modeled based on the nautical chart data using a sparse grid partitioning method to obtain the first grid map.

6. The method according to claim 5, characterized in that The second grid map is constructed in the following way: The underwater environment is rasterized and modeled based on the nautical chart data using a dense grid division method to obtain the second grid map.

7. The method according to claim 6, characterized in that The first grid map includes information about obstacles in the underwater environment; the second grid map includes information about obstacles in the underwater environment.

8. A route planning device for an underwater vehicle, characterized in that: include: A target location determination module is used to determine multiple target passing locations within a feasible area of ​​the underwater vehicle; wherein the multiple target passing locations correspond to a target access sequence, and the target access sequence is used to describe the global path structure information of the global planned route of the underwater vehicle; wherein the determination of multiple target passing locations within the feasible area of ​​the underwater vehicle includes: obtaining the starting point position information, the passing point position information and the terminal position information of the underwater vehicle; adding the starting point position information, the passing point position information and the terminal position information to an obstacle-free grid map of the underwater environment where the underwater vehicle is located, and arranging multiple target access sequences based on the obstacle-free grid map to obtain multiple target passing locations arranged according to the target access sequence; A path point allocation module, configured to allocate path points between the starting point of the underwater vehicle and the plurality of target passing points according to the target access sequence to obtain a plurality of path points arranged in an orderly manner; wherein, the allocation of path points between the starting point of the underwater vehicle and the plurality of target passing points according to the target access sequence to obtain a plurality of path points arranged in an orderly manner comprises: in a first grid map of the underwater environment where the underwater vehicle is located, sparse path point allocation is performed between the starting point and the plurality of target passing points according to the target access sequence to obtain the plurality of path points; A parallel path planning module is used to synchronously perform parallel path planning between every two adjacent path points for the plurality of path points arranged in order, so as to obtain a route sub-path between the two adjacent path points; wherein the synchronously performing parallel path planning between every two adjacent path points, so as to obtain a route sub-path between the two adjacent path points, comprises: synchronously performing parallel fine path planning between every two adjacent path points in a second grid map of the underwater environment where the underwater vehicle is located, so as to obtain the route sub-path; wherein the resolution of the first grid map is smaller than the resolution of the second grid map; The path merging module is used to merge the sub-paths between every two adjacent path points to obtain the global planned route of the underwater vehicle.

9. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the method according to any one of claims 1 to 7 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the method according to any one of claims 1 to 7.

Images