Path planning method for underwater vehicle to pass through narrow water channel

Abstract

The invention discloses a path planning method for an underwater vehicle to pass through a narrow water channel, which can solve the problem of low path planning efficiency when the underwater vehicle operates through the narrow water channel. Firstly, an underwater vehicle operation space is selected, a marine environment feature model is established, and a final marine environment model is obtained; and defining the narrow water channel area through which the underwater vehicle passes by using the final marine environment model, and obtaining a grid chart of the task area. And based on the grid map of the task area, performing expansion processing and data simplification processing on the obstacles in the navigation obstructing area. And acquiring a global path by adopting eight-direction two-way A * search based on the processed grid map of the narrow water channel. And based on the grid map, carrying out real-time dynamic collision avoidance on the global path by adopting a DWA algorithm to carry out local path planning. Based on a DWA algorithm, an APF algorithm is adopted to carry out auxiliary real-time dynamic collision avoidance so as to carry out local path planning. And two actual sailing routes are obtained by adjusting the sailing direction and the safety coefficient in the DWA algorithm.

Description

Technical Field

[0001] The present invention relates to the field of underwater vehicle path planning under the influence of marine environmental elements, and in particular to a path planning method for an underwater vehicle passing through a narrow waterway. Background Art

[0002] In recent years, with the rapid development of underwater vehicle technology, its application in tasks such as narrow waterway navigation has gradually increased. Internationally, waterways with a width of less than 2 nautical miles are considered narrow waterways. However, traditional path planning methods face challenges in complex marine environments, including the identification and processing of obstacles in electronic nautical charts, and the mismatch between the planned path and the movement law of the underwater vehicle itself.

[0003] To address these challenges, an advanced underwater vehicle path planning system has emerged. This system accurately models obstacles in electronic nautical charts and combines them with efficient path planning algorithms to ensure that underwater vehicles can reasonably plan their navigation routes in narrow waterways based on specific conditions, thereby improving safety, efficiency and compliance.

[0004] With the development of underwater vehicles, advanced path planning systems have been widely used in military reconnaissance, marine resource surveys, environmental monitoring and other fields. Especially in tasks such as exploring marine resources, underwater vehicles need to be able to resume navigation autonomously and smoothly pass through narrow waterways to avoid static obstacles while cruising. The traditional method requires surface vessels to go out to sea with underwater vehicles. When the underwater vehicle completes the mission, it needs to plan a better path to return to the surface vessel through communication equipment. Therefore, accurate environmental modeling and efficient path planning algorithms can significantly improve the operating efficiency and safety of underwater vehicles in complex marine environments and reduce economic costs.

[0005] However, there are still problems in the current operation. When the underwater vehicle autonomously passes through a narrow waterway, the planned route does not conform to the kinematic characteristics of its own equipment and is inconsistent with the actual situation. Therefore, further improving the path planning algorithm to ensure that the planned path matches the motion law of the underwater vehicle has become one of the important directions of current technological development. Summary of the invention

[0006] In view of this, the present invention provides a path planning method for an underwater vehicle passing through a narrow waterway, which can solve the problem of low path planning efficiency when the underwater vehicle is operating through a narrow waterway.

[0007] In order to achieve the above object, the technical solution of the present invention is: comprising:

[0008] Step 1: Select the underwater vehicle operation space, establish the ocean environment characteristic model, and obtain the final ocean environment model; use the final ocean environment model to define the narrow waterway area through which the underwater vehicle passes, and obtain a raster chart of the mission area.

[0009] Step 2: Based on the grid map of the mission area, the obstacles in the obstruction area are expanded and the data is simplified.

[0010] Step 3: Based on the processed grid map of the narrow waterway, an eight-direction bidirectional A* search is used to obtain the global path.

[0011] Step 4: Based on the grid map and the global path, the DWA algorithm is used to perform real-time dynamic collision avoidance and local path planning.

[0012] Step 5: Based on the DWA algorithm, the APF algorithm is used to assist in real-time dynamic collision avoidance and local path planning.

[0013] Step 6: Obtain two actual navigation routes by adjusting the heading and safety factor in the DWA algorithm.

[0014] Furthermore, in the present invention, in step 1, the specific method for establishing the marine environment characteristic model is:

[0015] Step 11, in the S-57 standard electronic nautical chart, according to the upper left and lower right latitude and longitude coordinates of the given narrow waterway operation, extract the island and reef area to be patrolled to obtain the operation area chart, and construct the operation area into a Cartesian rectangular coordinate system;

[0016] Step 12: extract the obstruction area from the nautical chart of the island and reef area in the narrow waterway operation area to be passed, project the obstruction area into a rectangular coordinate system using Mercator projection transformation, and obtain the original environmental characteristic model of the narrow waterway operation area, which includes data such as land, islands and reefs, shipwreck accident sites, and lighthouses in the nautical chart data;

[0017] Step 13: convert the obstruction area in the original environmental feature model of the narrow waterway operation area into a geometric convex hull structure, expand the convex hull structure, obtain the safe distance between the unmanned boat and the obstruction area, obtain the safe patrol area, and convert the safe patrol area into a binary grid map to obtain the converted environmental feature model of the operation area;

[0018] Furthermore, in the present invention, in step 2, the specific method of performing expansion processing and data simplification processing on obstacles in the navigation area is:

[0019] The obstacle information in the obstruction area derived from the S-57 electronic chart is not a regular shape. After the derived topology map is rasterized, some unit grids still contain some obstacles. In order to improve the safety and efficiency of the planned path, it is necessary to expand the obstacles in the rasterized mission area. Each grid will be treated as an infeasible area regardless of whether it is fully filled. If the position of the obstacle in the grid map is (x i ,y i ), the method of expanding the neighborhood in eight directions is as follows:

[0020]

[0021] Judge each grid of the expanded obstacle:

[0022] 1) If the grid contains some obstacles before, it will be regarded as a navigation obstruction area, otherwise it will become a navigable area.

[0023] 2) Determine whether the obstacle reaches the map boundary after expansion. If it reaches the boundary, there are four situations. Just expand it to the boundary. The specific method is as follows:

[0024]

[0025] x max ,y max is the maximum value of the horizontal coordinate and the maximum value of the vertical coordinate.

[0026] After the obstacle is expanded, the data of each obstacle is simplified and only the polygon outline is taken as the obstacle point.

[0027] Furthermore, in the present invention, in step 3, the specific method of using eight-direction bidirectional A* search to obtain the global path is:

[0028] Step 31, establish two open lists and two close lists, the open list is used to store the node data that has been generated in the forward and reverse directions but has not been traversed and is waiting to be checked, and the close list is used to record the node data that has been visited in the forward and reverse directions and does not need to be checked again.

[0029] Step 32, put the start node and the target node in two open lists as the current node, and expand all its reachable and unmarked child nodes. Since there is only one node to be searched at this time, that is, the evaluation values ​​of the start node and the target node must be the optimal values, the two nodes are added to the two close lists respectively.

[0030] Step 33, calculate the evaluation values ​​of the child nodes in the open list, arrange them according to the size of the evaluation values, find the node with the smallest evaluation value, mark it and add it to the corresponding close list.

[0031] Step 34, if the search condition is met, it means that the path is found and the search stops; otherwise, the node is further expanded and step 43 is repeated; or the open list is empty, indicating that there is no path.

[0032] Step 35, start traversing the father nodes from the meeting node of the two close lists and plan the final path.

[0033] Furthermore, in the present invention, in step 4, the method for local path planning based on the grid map and the global path using the DWA algorithm for real-time dynamic collision avoidance is:

[0034] Based on the global route searched by bidirectional A*, some waypoints are selected as waypoints for local path planning, and the DWA algorithm is used for dynamic collision avoidance. The DWA algorithm requires speed sampling, trajectory prediction, trajectory evaluation, and trajectory selection.

[0035] Step 41, set the speed boundary limit. According to the hardware conditions and environmental restrictions of the mobile robot, the speed of the mobile robot has boundary limits. At this time, the sampleable speed space V m for

[0036] V m ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}

[0037] Where v min ,v min are the minimum and maximum linear velocities of the underwater vehicle, ω min ,ω max are the minimum angular velocity and maximum angular velocity of the underwater vehicle respectively.

[0038] Step 42, set the acceleration limit. Due to the limitation of the driving motor of the mobile robot, the linear acceleration and angular acceleration of the mobile robot have boundary limits. Assuming that the maximum acceleration and deceleration are the same, the velocity space V that can be sampled when considering acceleration is d , whose expression is:

[0039]

[0040] Where v c ,ω care the linear velocity and angular velocity of the mobile robot at the current moment, a v_max ,a ω_max are the maximum linear acceleration and maximum angular acceleration of the mobile robot respectively.

[0041] Step 43, set environmental obstacle restrictions, and local planning also needs dynamic real-time obstacle avoidance. Considering the obstacles around the mobile robot, the constraints for the mobile robot not to collide with the surrounding obstacles at a certain moment are:

[0042]

[0043] Where dist(v,ω) represents the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed. In the absence of obstacles, dist(v,ω) will be a large constant value. When the robot's running sampling speed is within the range of formula (3), it can achieve safe deceleration with the constraint of maximum deceleration until it avoids the obstacle.

[0044] Combining the above three types of speed restrictions, the final mobile robot speed sampling space is the intersection of the three speed spaces, namely

[0045] V s =V m ∩V d ∩V a

[0046] Step 44, determine the speed sampling space V s After that, the DWA algorithm samples uniformly in this space with a certain sampling interval (resolution). In the velocity space, the resolution is set for the linear velocity and angular velocity respectively, using E w 、E v Represents the sampling resolution, then the number of sampling speed groups n can be determined, the formula is as follows:

[0047] n=[(v high -v low ) / E v ]*[(ω high -ω low ) / E ω ]

[0048] Where v high ,v low ,ω high ,ω low are the upper and lower limits of the velocity space.

[0049] Step 45, after sampling a set of (v, ω), the trajectory is predicted through the kinematic model of the underwater vehicle. Based on the task of passing through a narrow waterway, two-dimensional plane collision avoidance is adopted. Therefore, the differential drive robot motion model is adopted, and the trajectory prediction formula is as follows:

[0050] x k =x k-1 +v*cos(θ k-1 )*△t

[0051] y k =y k-1 +v*sin(θ k-1 )*△t

[0052] θ k =θ k-1 +ω*△t

[0053] In the formula, (x, y, θ) represents the position of the robot, x, y are the current position, θ is the direction angle, k represents the sampling time, and △t is the sampling interval.

[0054] Step 46, after determining the speed range of the underwater vehicle constraint, some speed simulation trajectories are feasible, but there are still trajectories that do not meet the standard, which requires evaluating and selecting the optimal trajectories obtained by sampling. The optimal trajectory is selected by comparing the scores through standard evaluation trajectories, and then the speed corresponding to the optimal trajectory is selected as the driving speed. The evaluation function for evaluating each trajectory is as follows:

[0055] G(v,ω)=α(σ*heading(v,ω))+β(σ*dist(v,ω))+γ(σ*velocity(v,ω))

[0056] Where heading(v,ω) is the azimuth evaluation function, which is used to evaluate the error △θ between the angle between the trajectory end position direction and the target point line generated at the current sampling speed. Since we want the larger the evaluation function, the better, Π-△θ is used in the evaluation. Its expression is as follows:

[0057]

[0058] heading=Π-△θ

[0059] Where θ goal is the direction angle between the current point and the target point, θ self The direction of movement of the current position.

[0060] dist(v,ω) is a distance evaluation function, which indicates the closest distance between the corresponding simulated trajectory and the obstacle at the current speed; if there is no obstacle or the closest distance is greater than the set threshold, its value is set to a larger constant value. Its expression is as follows:

[0061] dist=min(abs((x i ,y i )-obs)))

[0062] Where obs is the location of the obstacle.

[0063] velocity(v,ω) is the velocity evaluation function. v represents the current velocity and can be directly expressed by the current linear velocity. The larger it is, the faster the speed on the planned trajectory is and the higher the evaluation score is. Its expression is as follows:

[0064] velocity=vn*a*dt

[0065] Where v is the current velocity, n is the number of iterations required to reduce the velocity to 0 or below, a is the acceleration, and dt is the simulation sampling interval.

[0066] α, β, and γ are all coefficients of the evaluation function. Since the local path planning process requires the collection of multiple sensors, the collected information cannot be continuous, which will also make the evaluation differ greatly, so normalization processing (smoothing processing) can be performed, where σ represents normalization, and the normalization process is as follows:

[0067]

[0068] Among them, i represents the i-th simulation trajectory, and n is the total number of all sampled trajectories under the constraints.

[0069] Furthermore, in the present invention, in step 5, based on the DWA algorithm, the APF algorithm is used to assist in real-time dynamic collision avoidance and perform local path planning as follows:

[0070] The underwater vehicle uses the DWA algorithm to pass through narrow waterways. The path planning is realized by searching for feasible speed and steering in the dynamic window around the current position of the underwater vehicle. Sometimes it falls into an infinite loop due to the dynamic nature of the environment, the accumulation of tracking errors, and falling into the local minimum point. Therefore, the APF algorithm is used to assist DWA in local path planning.

[0071] Furthermore, in the present invention, in step six, the specific method of obtaining two actual navigation routes by adjusting the heading and safety factor in the DWA algorithm is as follows:

[0072] When the DWA algorithm is used for path planning in step 46, there are three parameters α, β, and γ. By adjusting α and β, two routes with the lowest energy consumption and the highest safety factor are obtained.

[0073] Beneficial effects:

[0074] The unmanned boat island and reef patrol path planning method based on marine environmental elements proposed in this application ensures the accuracy and efficiency of path planning by converting complex marine environmental element parameters into data that can be processed by the unmanned boat; by incorporating corner constraints and obstruction threats into the cost range of path planning, the safety of the planned path is improved, the patrol efficiency is optimized, and the necessary conditions are provided for the unmanned boat to autonomously plan paths according to marine environmental characteristics (such as ocean currents, shoals and other obstruction areas) and to effectively respond to complex marine environments. This application not only improves the efficiency and safety of island and reef patrols, but also lays a solid foundation for the future application of unmanned boats in the fields of marine environmental monitoring, scientific research, and marine resource protection. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 A flowchart of this application;

[0076] Figure 2 Schematic diagram of expansion processing.

[0077] Figure 3 The schematic diagram of the ocean terrain environment grid corresponding to S-57 and the schematic diagram after data simplification processing;

[0078] Figure 4 Schematic diagram of an underwater vehicle using the DWA+APF algorithm to pass through a narrow waterway. DETAILED DESCRIPTION

[0079] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0080] Reference Figure 1 Specifically describe this embodiment, described in this embodiment:

[0081] Step 1: Select the underwater vehicle operation space, establish the ocean environment characteristic model, and obtain the final ocean environment model; use the final ocean environment model to define the narrow waterway area through which the underwater vehicle passes, and obtain a raster chart of the mission area.

[0082] Step 2: Based on the grid map of the mission area, the obstacles in the obstruction area are expanded and the data is simplified.

[0083] Step 3: Based on the processed grid map of the narrow waterway, an eight-direction bidirectional A* search is used to obtain the global path.

[0084] Step 4: Based on the grid map and the global path, the DWA algorithm is used to perform real-time dynamic collision avoidance and local path planning.

[0085] Step 5: Based on the DWA algorithm, the APF algorithm is used to assist in real-time dynamic collision avoidance and local path planning.

[0086] Step 6: Obtain two actual navigation routes by adjusting the heading and safety factor in the DWA algorithm.

[0087] In this embodiment, the routes finally obtained through the narrow waterway include two routes with the shortest journey and the highest safety, so when it is actually applied, it is only necessary to select the route according to actual needs.

[0088] Furthermore, in the present invention, in step 1, the specific method for establishing the marine environment characteristic model is:

[0089] Step 11, in the S-57 standard electronic nautical chart, according to the upper left and lower right latitude and longitude coordinates of the given narrow waterway operation, extract the island and reef area to be patrolled to obtain the operation area chart, and construct the operation area into a Cartesian rectangular coordinate system;

[0090] Step 12: extract the obstruction area from the nautical chart of the island and reef area in the narrow waterway operation area to be passed, and project the obstruction area into a rectangular coordinate system using Mercator projection transformation to obtain the original environmental characteristic model of the narrow waterway operation area;

[0091] Step 13: convert the obstruction area in the original environmental feature model of the narrow waterway operation area into a geometric convex hull structure, expand the convex hull structure, obtain the safe distance between the unmanned boat and the obstruction area, obtain the safe patrol area, and convert the safe patrol area into a binary grid map to obtain the converted environmental feature model of the operation area;

[0092] Furthermore, in the present invention, in step 2, the specific method of performing expansion processing and data simplification processing on obstacles in the navigation area is:

[0093] The obstacle information in the obstruction area derived from the S-57 electronic chart is not a regular graphic. After the derived topology map is rasterized, some unit grids still contain some obstacles. In order to improve the safety and efficiency of the planned path, it is necessary to expand the obstacles in the rasterized mission area. Each grid will be treated as an infeasible area regardless of whether it is fully filled. Each grid of the expanded obstacle will be judged. If the grid previously contained some obstacles, it will be treated as an obstruction area, otherwise it will become a navigable area.

[0094] After the obstacle is expanded, the data of each obstacle is simplified and only the polygonal outline is taken as the obstacle point.

[0095] Furthermore, in the present invention, in step 3, the specific method of using eight-direction bidirectional A* search to obtain the global path is:

[0096] Step 31, establish two open lists and two close lists, the open list is used to store the node data that has been generated in the forward and reverse directions but has not been traversed and is waiting to be checked, and the close list is used to record the node data that has been visited in the forward and reverse directions and does not need to be checked again.

[0097] Step 32, put the start node and the target node in two open lists as the current node, and expand all its reachable and unmarked child nodes. Since there is only one node to be searched at this time, that is, the evaluation values ​​of the start node and the target node must be the optimal values, the two nodes are added to the two close lists respectively.

[0098] Step 33, calculate the evaluation values ​​of the child nodes in the open list, arrange them according to the size of the evaluation values, find the node with the smallest evaluation value, mark it and add it to the corresponding close list.

[0099] Step 34, if the search condition is met, it means that the path is found and the search stops; otherwise, the node is further expanded and step 43 is repeated; or the open list is empty, indicating that there is no path.

[0100] Step 35, start traversing the father nodes from the meeting node of the two close lists and plan the final path.

[0101] Furthermore, in the present invention, in step 4, the method for local path planning based on the grid map and the global path using the DWA algorithm for real-time dynamic collision avoidance is:

[0102] Based on the global route searched by bidirectional A*, some waypoints are selected as waypoints for local path planning, and the DWA algorithm is used for dynamic collision avoidance. The DWA algorithm requires speed sampling, trajectory prediction, trajectory evaluation, and trajectory selection.

[0103] Step 41, set the speed boundary limit. According to the hardware conditions and environmental restrictions of the mobile robot, the speed of the mobile robot has boundary limits. At this time, the sampleable speed space V m for

[0104] V m ={(v,ω)|v∈[v min ,v max ],ω∈[ωmin ,ω max ]}

[0105] Where v min ,v min are the minimum and maximum linear velocities of the mobile robot, ω min ,ω max are the minimum angular velocity and maximum angular velocity of the mobile robot respectively.

[0106] Step 42, set the acceleration limit. Due to the limitation of the driving motor of the mobile robot, the linear acceleration and angular acceleration of the mobile robot have boundary limits. Assuming that the maximum acceleration and deceleration are the same, the velocity space V that can be sampled when considering acceleration is d , whose expression is:

[0107]

[0108] Where v c ,ω c are the linear velocity and angular velocity of the mobile robot at the current moment, a v_max ,a ω_max are the maximum linear acceleration and maximum angular acceleration of the mobile robot respectively.

[0109] Step 43, set environmental obstacle restrictions, and local planning also needs dynamic real-time obstacle avoidance. Considering the obstacles around the mobile robot, the constraints for the mobile robot not to collide with the surrounding obstacles at a certain moment are:

[0110]

[0111] Where dist(v,ω) represents the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed. In the absence of obstacles, dist(v,ω) will be a large constant value. When the robot's running sampling speed is within the range of formula (3), it can achieve safe deceleration with the constraint of maximum deceleration until it avoids the obstacle.

[0112] Combining the above three types of speed restrictions, the final mobile robot speed sampling space is the intersection of the three speed spaces, namely

[0113] V s =V m ∩V d ∩V a

[0114] Step 44, determine the speed sampling space V s After that, the DWA algorithm samples uniformly in this space with a certain sampling interval (resolution). In the velocity space, the resolution is set for the linear velocity and angular velocity respectively, using E w、E v Represents the sampling resolution, then the number of sampling speed groups n can be determined, the formula is as follows:

[0115] n=[(v high -v low ) / E v ]×[(ω high -ω low ) / E ω ]

[0116] Where v high ,v low ,ω high ,ω low are the upper and lower limits of the velocity space.

[0117] Step 45, after sampling a set of (v, ω), the trajectory is predicted through the kinematic model of the underwater vehicle. Based on the task of passing through a narrow waterway, two-dimensional plane collision avoidance is adopted. Therefore, the differential drive robot motion model is adopted, and the trajectory prediction formula is as follows:

[0118] x k =x k-1 +v×cos(θ k-1 )×△t

[0119] y k =y k-1 +v×sin(θ k-1 )×△t

[0120] θ k =θ k-1 +ω×△t

[0121] In the formula, (x, y, θ) represents the posture of the robot, x, y are the current position, θ is the direction angle, k represents the sampling time, and △t is the sampling interval.

[0122] Step 46, after determining the speed range of the underwater vehicle constraint, some speed simulation trajectories are feasible, but there are still trajectories that do not meet the standard, which requires evaluating and selecting the optimal trajectories obtained by sampling. The optimal trajectory is selected by comparing the scores through standard evaluation trajectories, and then the speed corresponding to the optimal trajectory is selected as the driving speed. The evaluation function for evaluating each trajectory is as follows:

[0123] G(v,ω)=α(σ×heading(v,ω))+β(σ×dist(v,ω))+γ(σ×velocity(v,ω))

[0124] Where heading(v,ω) is the azimuth evaluation function, which is used to evaluate the error △θ between the angle between the trajectory end position direction and the target point line generated at the current sampling speed. Since we want the larger the evaluation function, the better, Π-△θ is used in the evaluation. Its expression is as follows:

[0125]

[0126] heading=Π-△θ

[0127] Where θ goal is the direction angle between the current point and the target point, θ self The direction of movement of the current position.

[0128] dist(v,ω) is a distance evaluation function, which indicates the closest distance between the corresponding simulated trajectory and the obstacle at the current speed; if there is no obstacle or the closest distance is greater than the set threshold, its value is set to a larger constant value. Its expression is as follows:

[0129] dist=min(abs((x i ,y i )-obs)))

[0130] Where obs is the location of the obstacle.

[0131] velocity(v,ω) is the velocity evaluation function. v represents the current velocity and can be directly expressed by the current linear velocity. The larger it is, the faster the speed on the planned trajectory is and the higher the evaluation score is. Its expression is as follows:

[0132] velocity=vn×a×dt

[0133] Where v is the current velocity, n is the number of iterations required to reduce the velocity to 0 or below, a is the acceleration, and dt is the simulation sampling interval.

[0134] α, β, and γ are all coefficients of the evaluation function. Since the local path planning process requires the collection of multiple sensors, the collected information cannot be continuous, which will also make the evaluation differ greatly, so normalization processing (smoothing processing) can be performed, where σ represents normalization, and the normalization process is as follows:

[0135]

[0136] Among them, i represents the i-th simulation trajectory, and n is the total number of all sampled trajectories under the constraints.

[0137] Furthermore, in the present invention, in step 5, based on the DWA algorithm, the APF algorithm is used to assist in real-time dynamic collision avoidance and perform local path planning as follows:

[0138] Based on the underwater vehicle using the DWA algorithm to pass through narrow waterways, it is based on the current position of the underwater vehicle.

[0139] Searching for feasible speed and steering in the surrounding dynamic window to achieve path planning may sometimes lead to an infinite loop due to the dynamic nature of the environment, accumulation of tracking errors, and falling into the local minimum point. Therefore, the APF algorithm is used to assist DWA in local path planning.

[0140] Furthermore, in the present invention, in step six, the specific method of obtaining two actual navigation routes by adjusting the heading and safety factor in the DWA algorithm is as follows:

[0141] When the DWA algorithm is used for path planning in step 46, there are three parameters α, β, and γ, by adjusting α and β.

[0142] Get two routes with the lowest energy consumption and the highest safety factor.

[0143] Example:

[0144] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0145] Combination Figures 1 to 3 In the present invention, firstly, a marine environment characteristic model must be established. In the specific context of underwater vehicles operating through narrow waterways, the S-57 standard electronic chart data is mainly used to construct a marine environment characteristic model of the operating area. First, the marine environment characteristic data in the electronic chart of the designated island and reef patrol area is extracted, and the original data is deeply analyzed to determine the spatial information of the obstruction area (such as land, shoals, islands and reefs, optical cables, etc.) in the operating area, so as to establish the original environmental characteristic model of the operating area. On this basis, the original environmental model is converted into a geometric convex hull structure, and the safe navigation distance of the unmanned boat is comprehensively considered. By rasterizing, expanding, and simplifying the data of the area, a relatively safe marine environment characteristic model is obtained, which significantly improves the calculation efficiency of the path planning algorithm. The actual underwater vehicle operating area is defined. The global path planning of the underwater vehicle requires starting from the preparation point and avoiding static obstacles to reach the target point. The actual operating area is a two-dimensional plane containing the preparation point and the target island and reef.

[0146] The process of constructing the marine environment characteristic model of the underwater vehicle operation area using the S-57 standard electronic chart is as follows:

[0147] 2a) According to the designated upper left longitude and latitude coordinates and lower right longitude and latitude coordinates of the working area, the working area is constructed into a Cartesian rectangular coordinate system O-XY, where the origin is the lower left longitude and latitude point O of the working area, the latitude direction of the origin is the positive direction of the X axis, and the longitude direction is the Y axis direction;

[0148] 2b) Read the original file data of S-57 standard electronic chart and extract the spatial information of the obstruction area. The obstruction area includes a series of obstructions such as land, islands and reefs, and lighthouses. The above objects constitute the obstruction area. The Mercator projection transformation is used to project the obstruction area to the rectangular coordinate system O-XY, thereby obtaining the original environmental characteristic model of the underwater vehicle operation area;

[0149] 2c) Based on the coordinate system constructed in step 2a), after converting the obstruction area into a geometric convex hull structure, the obstruction area is puffed to ensure that the underwater vehicle maintains a safe distance from the obstruction area, and finally the operation area is converted into a binary raster map.

[0150] Combination Figure 2 , Figure 3 These are the processes of expanding the obstacles in the obstruction area and simplifying the data, and the specific implementation methods are as follows:

[0151] 3a) Based on the binary grid map constructed in step 2c), the center coordinate point of each obstacle is obtained.

[0152] 3b) Based on the coordinate points of the obstacle extracted in step 3a), the neighborhood is expanded in 8 directions. If there is a partial structure of convex polygons in the expanded grid, the grid is regarded as an obstacle, otherwise it is regarded as a navigable area.

[0153] 3c) Take the outline of the obstacle in 3b) as the new obstacle.

[0154] Combination Figure 4 The process of obtaining the global path based on the processed grid map of narrow waterways using eight-direction bidirectional A* search is as follows:

[0155] 4a) Create two open lists and two close lists. The open list is used to store the node data that has been generated in the forward and reverse directions but has not been traversed and is waiting to be checked. The close list is used to record the node data that has been visited in the forward and reverse directions and does not need to be checked again.

[0156] 4b) Put the start node and the target node in two open lists as the current node, and expand all its reachable and unmarked child nodes. Since there is only one node to be searched at this time, that is, the evaluation values ​​of the start node and the target node must be the optimal values, the two nodes are added to the two close lists respectively.

[0157] 4c) Calculate the evaluation values ​​of the child nodes in the open list, sort them according to the size of the evaluation values, find the node with the smallest evaluation value, mark it and add it to the corresponding close list.

[0158] 4d) If the search condition is met, it means the path is found and the search stops; otherwise, the node is expanded and step 4c) is repeated; or the open list is empty, indicating that there is no path.

[0159] 4e) Start traversing the parent nodes from the meeting node of the two close lists and plan the final path.

[0160] Combination Figure 4 The specific process of using the DWA algorithm for real-time dynamic collision avoidance and local path planning based on the grid map and the global path is as follows:

[0161] 5a) Set the speed boundary limit. According to the hardware conditions and environmental restrictions of the mobile robot, the speed of the mobile robot has boundary limits. At this time, the sampled speed space V m for

[0162] V m ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}(1)

[0163] where v min ,v min are the minimum and maximum linear velocities of the mobile robot, ω min ,ω max are the minimum angular velocity and maximum angular velocity of the mobile robot respectively.

[0164] 5b) Set acceleration limit. Due to the limitation of the driving motor of the mobile robot, the linear acceleration and angular acceleration of the mobile robot have boundary limits. Assuming that the maximum acceleration and deceleration are the same, the velocity space V that can be sampled when considering acceleration is d , whose expression is:

[0165]

[0166] where v c ,ω c are the linear velocity and angular velocity of the mobile robot at the current moment, a v_max ,a ω_max are the maximum linear acceleration and maximum angular acceleration of the mobile robot respectively.

[0167] 5c) Set environmental obstacle limits. Local planning also needs to have dynamic and real-time obstacle avoidance functions. Considering the obstacles around the mobile robot, the constraints for the mobile robot not to collide with the surrounding obstacles at a certain moment are:

[0168]

[0169] Where dist(v, ω) represents the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed. In the absence of obstacles, dist(v, ω) will be a large constant value. When the robot's running sampling speed is within the range of formula (3), it can achieve safe deceleration with the constraint of maximum deceleration until it avoids the obstacle.

[0170] Combining the above three types of speed restrictions, the final mobile robot speed sampling space is the intersection of the three speed spaces, namely

[0171] V s =V m ∩V d ∩V a (4)

[0172] 5d) Determine the velocity sampling space V s After that, the DWA algorithm samples uniformly in this space with a certain sampling interval (resolution). In the velocity space, the resolution is set for the linear velocity and angular velocity respectively, using E w 、E v Represents the sampling resolution, then the number of sampling speed groups n can be determined, the formula is as follows:

[0173] n=[(v high -v low ) / E v ]×[(ω high -ω low ) / E ω ](5)

[0174] where v high ,v low ,ω high ,ω low are the upper and lower limits of the velocity space.

[0175] 5e) After sampling a set of (v, ω), the trajectory is predicted through the kinematic model of the underwater vehicle. Based on the task of passing through a narrow waterway, two-dimensional plane collision avoidance is adopted. Therefore, the differential drive robot motion model is adopted, and the trajectory prediction formula is as follows:

[0176]

[0177] Among them, (x, y, θ) represents the posture of the robot, x, y is the current position, θ is the direction angle, k represents the sampling time, and △t is the sampling interval.

[0178] 5f) After determining the speed range of the underwater vehicle, some speed simulation trajectories are feasible, but there are still trajectories that do not meet the standards. This requires evaluating and selecting the best of the multiple groups of trajectories obtained by sampling. The best trajectory is selected by comparing the scores through standard evaluation trajectories, and then the speed corresponding to the best trajectory is selected as the driving speed. The evaluation function for evaluating each trajectory is as follows:

[0179] G(v,ω)=α(σ×heading(v,ω))+β(σ×dist(v,ω))+γ(σ×velocity(v,ω))(7)

[0180] Its expression is as follows:

[0181]

[0182] heading=Π-△θ

[0183] where θ goal is the direction angle between the current point and the target point, θ self The direction of movement of the current position.

[0184] dist(v,ω) is a distance evaluation function, which indicates the closest distance between the corresponding simulated trajectory and the obstacle at the current speed; if there is no obstacle or the closest distance is greater than the set threshold, its value is set to a larger constant value. Its expression is as follows:

[0185] dist=min(abs((x i ,y i )-obs)))

[0186] Where obs is the location of the obstacle.

[0187] velocity(v,ω) is the velocity evaluation function. v represents the current velocity and can be directly expressed by the current linear velocity. The larger it is, the faster the speed on the planned trajectory is and the higher the evaluation score is. Its expression is as follows:

[0188] velocity=vn×a×dt

[0189] Where v is the current velocity, n is the number of iterations required to reduce the velocity to 0 or below, a is the acceleration, and dt is the simulation sampling interval.

[0190] α, β, and γ are all coefficients of the evaluation function. Since the local path planning process requires the collection of multiple sensors, the collected information cannot be continuous, which will also make the evaluation differ greatly, so normalization processing (smoothing processing) can be performed, where σ represents normalization, and the normalization process is as follows:

[0191]

[0192] Among them, i represents the i-th simulation trajectory, and n is the total number of all sampled trajectories under the constraints.

[0193] The process of using the APF algorithm to assist in real-time dynamic collision avoidance and perform local path planning based on the DWA algorithm is as follows:

[0194] The underwater vehicle uses the DWA algorithm to pass through narrow waterways. The path planning is realized by searching for feasible speed and steering in the dynamic window around the current position of the underwater vehicle. Sometimes it falls into an infinite loop due to the dynamic nature of the environment, the accumulation of tracking errors, and falling into the local minimum point. Therefore, the APF algorithm is used to assist DWA in local path planning.

[0195] Combination Figure 4 The specific method of obtaining two actual navigation routes by adjusting the heading and safety factor in the DWA algorithm is as follows:

[0196] When the DWA algorithm is used for path planning in 5f), there are three parameters α, β, and γ. By adjusting α and β, two routes with the lowest energy consumption and the highest safety factor can be obtained.

[0197] At this point, for the path planning of underwater vehicles operating through narrow waterways, an ocean terrain model based on S-57 electronic nautical chart data is constructed. The accuracy and efficiency of path planning are ensured by using the DWA+APF algorithm, which improves the operating efficiency of underwater vehicles, reduces energy consumption, and improves adaptability in the marine environment.

[0198] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A path planning method for an underwater vehicle passing through a narrow waterway, characterized in that: The steps include: Step 1: Select the underwater vehicle operation space, establish the ocean environment characteristic model, and obtain the final ocean environment model; use the final ocean environment model to define the waterway area through which the underwater vehicle passes, and obtain a raster chart of the mission area; Step 2: Based on the grid map of the mission area, the obstacles in the obstruction area are expanded and the data is simplified; Step 3: Based on the processed grid map, an eight-direction bidirectional A* search is used to obtain the global path; Step 4: Based on the grid map and the global path, the DWA algorithm is used to perform real-time dynamic collision avoidance and local path planning; Step 5: Based on the DWA algorithm, the APF algorithm is used to assist in real-time dynamic collision avoidance and local path planning; Step 6: Obtain two actual navigation routes by adjusting the heading and safety factor in the DWA algorithm.

2. The path planning method for an underwater vehicle passing through a narrow waterway according to claim 1, characterized in that: In the step 1, the specific method of establishing the marine environment characteristic model is: Step 11, extracting the island and reef area to be patrolled according to the upper left and lower right latitude and longitude coordinates of the narrow waterway in the electronic nautical chart to obtain the nautical chart of the operation area, and constructing the operation area into a Cartesian rectangular coordinate system; Step 12: extract the obstruction area from the nautical chart of the island and reef area of ​​the narrow waterway operation to be passed, project the obstruction area to the rectangular coordinate system using Mercator projection transformation, and obtain the original environmental characteristic model of the narrow waterway operation area, which includes the land, islands and reefs, shipwreck accident points and lighthouse data in the nautical chart data; Step 13: convert the obstruction area in the original environmental feature model of the narrow waterway operation area into a geometric convex hull structure, expand the convex hull structure, obtain the safe distance between the unmanned boat and the obstruction area, obtain the safe patrol area, and convert the safe patrol area into a binary raster map to obtain the environmental feature model of the operation area after the conversion.

3. The path planning method for an underwater vehicle passing through a narrow waterway according to claim 1 or 2, characterized in that: In step 2, the obstacles in the obstruction area are expanded and the data is simplified as follows: The obstacle information in the obstruction area derived from the electronic chart is not a regular shape. After the derived topology map is rasterized, there are still some unit grids that contain some obstacles. The obstacles in the rasterized mission area are expanded; each grid is treated as an infeasible area regardless of whether it is fully filled. If the position of the obstacle in the grid map is (x i ,y i ), the method of expanding the neighborhood in eight directions is as follows: Judge each grid of the expanded obstacle: 1) If there are some obstacles before the grid, it will be regarded as a navigation obstruction area, otherwise it will become a navigable area; 2) Determine whether the obstacle reaches the map boundary after expansion; if it reaches the boundary, there are four situations, and it can be expanded to the boundary. The specific methods are as follows: where x max is the maximum value of the horizontal coordinate, y max is the maximum value of the vertical coordinate; After the obstacle is expanded, the data of each obstacle is simplified and only the polygon outline is taken as the obstacle point.

4. The path planning method for an underwater vehicle passing through a narrow waterway according to claim 3, characterized in that: In step 3, an eight-direction bidirectional A* search is used to obtain a global path based on the processed grid map. The specific method is as follows: Step 31, establish two open lists and two close lists, the open list is used to store the node data that has been generated in the forward and reverse directions but has not been traversed, i.e., waiting to be checked, and the close list is used to record the node data that has been visited in the forward and reverse directions, i.e., does not need to be checked again; Step 32, put the start node and the target node in two open lists as current nodes, and expand all its reachable and unmarked child nodes. Since there is only one node to be searched at this time, that is, the evaluation values ​​of the start node and the target node are definitely optimal values, the two nodes are added to the two close lists respectively; Step 33, calculate the evaluation value of the child nodes in the open list, sort them according to the evaluation value, find the node with the smallest evaluation value, mark it and add it to the corresponding close list; Step 34: If the search condition is met, it means that the path is found and the search stops; otherwise, the node is further expanded and step 34 is repeated; or if the open list is empty, it means that there is no path; Step 35, start traversing the father nodes from the meeting node of the two close lists and plan the final path.

5. The method for planning a path for an underwater vehicle passing through a narrow waterway according to claim 4, characterized in that: In step 4, based on the grid map and the global path, the DWA algorithm is used to perform real-time dynamic collision avoidance and local path planning. The specific method is as follows: Step 41, set the speed boundary limit. According to the hardware conditions and environmental restrictions of the mobile robot, the speed of the mobile robot has boundary limits. At this time, the sampled speed space V m for: V m ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]} where v min ,v min are the minimum and maximum linear velocities of the mobile robot, ω min ,ω max are the minimum angular velocity and maximum angular velocity of the mobile robot respectively; Step 42, setting acceleration limit. Due to the limitation of the driving motor of the mobile robot, the linear acceleration and angular acceleration of the mobile robot have boundary limits. Assuming that the maximum acceleration and deceleration are the same, the velocity space V sampled when considering acceleration is d , its expression is: where v c ,ω c are the linear velocity and angular velocity of the mobile robot at the current moment, a v_max ,a ω_max are the maximum linear acceleration and maximum angular acceleration of the mobile robot respectively; Step 43, setting environmental obstacle restrictions, taking into account the obstacle factors around the mobile robot, the constraint conditions for the mobile robot not to collide with the surrounding obstacles are: Where dist(v,ω) represents the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed; When the robot's running sampling speed is within the range of formula (3), it can achieve safe deceleration with the constraint of maximum deceleration until it avoids the obstacle; Combining the above three types of speed restrictions, the final mobile robot speed sampling space is the intersection of the three speed spaces, namely: V s =V m ∩V d ∩V a Step 44, determine the speed sampling space V s Afterwards, the DWA algorithm samples uniformly in the space with a certain sampling interval; In velocity space, set the resolution for linear velocity and angular velocity respectively, using E w 、E v Represents the sampling resolution, then the number of sampling speed groups n can be determined, the formula is as follows: n=[(v high -v low ) / E v ]×[(ω high -oh low ) / E ω ] where v high ,v low ,ω high ,ω low are the upper and lower limits of the velocity space; Step 45, after sampling a set of (v, ω), the trajectory is predicted through the kinematic model of the underwater vehicle; based on the task of passing through a narrow waterway, two-dimensional plane collision avoidance is adopted, so the differential drive robot motion model is adopted, and the trajectory prediction formula is as follows: x k =x k-1 +v*cos(θ k-1 )*△t and k =and k-1 +v*sin(θ k-1 )*△t i k =θ k-1 +ω*△t Among them, (x, y, θ) represents the posture of the robot, x, y is the current position, θ is the direction angle, k represents the sampling time, and △t is the sampling interval. Step 46, after determining the speed range of the underwater vehicle constraint, some speed simulation trajectories are feasible, but there are still trajectories that do not meet the standard, which requires evaluating and selecting the optimal trajectories obtained by sampling. The optimal trajectory is selected by comparing the scores through standard evaluation trajectories, and then the speed corresponding to the optimal trajectory is selected as the driving speed. The evaluation function for evaluating each trajectory is as follows: G(v,ω)=α(σ×heading(v,ω))+β(σ×dist(v,ω))+γ(σ×velocity(v,ω)) Where heading(v,ω) is the azimuth evaluation function, which is used to evaluate the error △θ between the angle between the trajectory endpoint position direction and the target point connection line generated at the current sampling speed. Since we want the evaluation function to be larger, the better, Π-△θ is used in the evaluation, and its expression is as follows: heading=Π-△θ where θ goal is the direction angle between the current point and the target point, θ self is the direction of movement of the current position, dist(v,ω) is the distance evaluation function, which indicates the closest distance between the corresponding simulation trajectory and the obstacle at the current speed; if there is no obstacle or the closest distance is greater than the set threshold, its value is set to a larger constant value, and its expression is as follows: dist=min(abs((x i ,y i )-obs))) Where obs is the location of the obstacle; velocity(v,ω) is the velocity evaluation function, v represents the current velocity, and its expression is as follows: velocity=vn×a×dt Where v is the current velocity, n is the number of iterations required to reduce the velocity to 0 or below, a is the acceleration, and dt is the simulation sampling interval; α, β, and γ are coefficients of the evaluation function; The collected information of multiple sensors is normalized, where σ represents normalization. The normalization process is as follows: Among them, i represents the i-th simulation trajectory, and n is the total number of all sampled trajectories under the constraints.

6. The path planning method for an underwater vehicle passing through a narrow waterway according to claim 1 or 2, characterized in that: Step 5: Based on the DWA algorithm, the APF algorithm is used to assist in real-time dynamic collision avoidance and local path planning. The specific steps include: The underwater vehicle adopts the DWA algorithm to pass through narrow waterways. The path planning is realized by searching for feasible speed and steering in a dynamic window around the current position of the underwater vehicle. The APF algorithm is used to assist the DWA in local path planning.

7. The path planning method for an underwater vehicle passing through a narrow waterway according to claim 5, characterized in that: The specific steps to change some parameters in step 6 are as follows: When the DWA algorithm is used for path planning in step 46, there are three parameters α, β, and γ. By adjusting α and β, two routes with the lowest energy consumption and the highest safety factor are obtained.

Images