Restricted water area three-dimensional route planning method, system, equipment and medium

Abstract

The invention relates to the field of underwater route planning, and provides a limited water area three-dimensional route planning method, system and device and a medium, and the method comprises the steps: obtaining an underwater topographic map of a target water area, and determining a path end point; determining a water area size, obtaining an elliptical transformation matrix according to the water area size, and determining a transformation matrix; determining a stretching coefficient, calculating a stretching height through the stretching coefficient so as to determine a random point, and performing elliptic transformation on the random point through the elliptic transformation matrix and the transformation matrix to obtain a transformed random point; generating an offset optimization cylinder according to the cylinder radius, when an obstacle exists, changing the cylinder radius and updating the offset optimization cylinder, determining a growth direction in the offset optimization cylinder according to a path endpoint, and generating an initial path according to a path generation algorithm; and constraining the initial path according to the top safety limit constraint, the obstacle safety constraint and the vehicle angle constraint to obtain a target navigation path of the underwater vehicle. According to the method, a relatively smooth target path can be quickly obtained.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater route planning, and in particular to a three-dimensional route planning method, system, device and medium for restricted waters. Background Art

[0002] Polar scientific research is of great significance for global climate change research, maintaining the integrity of the polar ecosystem, exploring the origin of life, etc. Underwater platforms have played an increasingly important role in recent polar scientific expeditions, especially in the survey of sub-ice terrain, ice sheet monitoring, ecosystem investigation, climate change research, etc. However, in recent years, the phenomena of underwater platforms being lost, trapped, and damaged by collision in polar scientific expeditions have occurred from time to time, causing heavy losses to polar scientific expeditions. One of the reasons for the above accidents is the lack of real-time performance of the underwater three-dimensional path planning algorithm for underwater platforms. On the one hand, different from open waters, when an underwater platform sails in a top-restricted water area, it will be affected by strong non-linear hydrodynamic forces and environmental uncertainties, and its obstacle avoidance response time is relatively short, which puts higher requirements on the real-time performance of the underwater platform path planning. On the other hand, the large non-uniformity of the three-dimensional space distribution of the top-restricted environment reduces the calculation efficiency and path quality of the path planning algorithm, which puts higher requirements on the design of the path planning space for underwater platforms in the top-restricted environment. There is relatively little research on the three-dimensional path planning of top-restricted waters in the public information, and it is urgent to carry out research on the real-time three-dimensional path planning of underwater platforms in top-restricted waters. Summary of the Invention

[0003] The present invention aims to solve at least one of the technical problems existing in the related art. For this purpose, the present invention provides a three-dimensional route planning method, system, device and medium for restricted waters, which realizes a relatively reasonable planning of the underwater route under the condition of limited top height.

[0004] The present invention provides a three-dimensional route planning method for restricted waters, including: S1: Determine the target water area, obtain the underwater topographic map of the target water area, and determine the path endpoints in the target water area; S2: Determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; S3: Determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path endpoints, determine the random points according to the stretching height, and perform elliptical transformation on the random points through the elliptical transformation matrix and the transformation matrix to obtain the transformed random points; S4: Select a path generation algorithm and the radius of the cylinder. Generate an offset-optimized cylinder based on the radius of the cylinder, and determine whether there are obstacles inside the offset-optimized cylinder. When there are obstacles, change the radius of the cylinder and update the offset-optimized cylinder until there are no obstacles inside the offset-optimized cylinder. Determine the growth direction in the offset-optimized cylinder according to the path endpoints, so that the path generation algorithm obtains an initial path based on the transformed random points and the growth direction. S5: Obtain the top environmental model through machine learning and perform top safety boundary constraints. Obtain obstacle safety constraints through the underwater topographic map. Obtain the coordinates of the random tree nodes. Obtain the angle constraints of the vehicle through the coordinates of the random tree nodes. Constrain the initial path according to the top safety boundary constraints, the obstacle safety constraints, and the angle constraints of the vehicle to obtain the target path for the underwater vehicle to navigate.

[0005] According to the three-dimensional route planning method for restricted waters provided by the present invention, step S1 specifically includes: S11: Determine the target water area, conduct a topographic survey on the target water area, and obtain the underwater topographic map of the target water area. S12: Determine the path end point and the path start point of the underwater vehicle in the underwater topographic map, and use the path end point and the path start point as the path endpoints.

[0006] According to the three-dimensional route planning method for restricted waters provided by the present invention, step S2 specifically includes: S21: Determine the water area size including the water area length and the water area width in the underwater topographic map, calculate the ellipse foci and the length of the major axis of the ellipse according to the water area size, and obtain the ellipse transformation matrix through the ellipse foci and the length of the major axis of the ellipse. S22: Determine the path endpoint vector according to the path endpoints, determine the included angle of the endpoint vectors through the path endpoint vector, and thus construct the transformation matrix using the included angle of the endpoint vectors.

[0007] According to the three-dimensional route planning method for restricted waters provided by the present invention, step S3 specifically includes: S31: Determine the stretching coefficient, where the value of the stretching coefficient is 1.2 to 1.5, calculate the height difference between the endpoints of the path, and calculate the initial stretching height through the stretching coefficient and the height difference between the endpoints. S32: Obtain the safety distance through the initial stretching height, adjust the initial stretching height according to the safety distance to obtain the stretching height, and determine the stretching ellipse according to the stretching height. S33: Select a random point according to the stretching height, determine the plane coordinates of the random point, perform elliptical transformation on the plane coordinates of the random point through the elliptical transformation matrix and the conversion matrix to obtain the transformed coordinates of the random point, and obtain the transformed random point located within the stretched ellipse according to the transformed coordinates of the random point.

[0008] According to the three-dimensional route planning method for restricted waters provided by the present invention, in step S4, when there is an obstacle, reduce the cylinder radius and generate a new offset optimized cylinder according to the reduced cylinder radius until there is no obstacle in the offset optimized cylinder, complete the update of the offset optimized cylinder, and use the direction from the path start point to the path end point in the offset optimized cylinder as the growth direction. The path generation algorithm performs random tree growth through the transformed random point and according to the growth direction to obtain the initial path.

[0009] According to the three-dimensional route planning method for restricted waters provided by the present invention, in step S5, obtain the top environmental data, clean, preprocess, and interpolate the top environmental data to obtain the top environmental input data, perform machine learning on the top environmental input data to obtain the top environmental model, and perform the top safety limit constraint through the top environmental model.

[0010] According to the three-dimensional route planning method for restricted waters provided by the present invention, in step S5, obtain the random tree node coordinates including the three-dimensional coordinates of the node and the three-dimensional coordinates of the adjacent node, and obtain the angle constraints of the vehicle including the pitch angle constraint and the yaw angle constraint through the random tree node coordinates.

[0011] The present invention also provides a three-dimensional route planning system for restricted waters, including: Underwater topographic map module: used to determine the target water area, obtain the underwater topographic map of the target water area, and determine the path endpoints in the target water area; Conversion matrix module: used to determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the conversion matrix; Elliptical transformation module: used to determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path endpoints, determine random points according to the stretching height, and perform elliptical transformation on the random points through the elliptical transformation matrix and the conversion matrix to obtain the transformed random points; Initial path module: used to select a path generation algorithm and a cylinder radius, generate an offset optimized cylinder according to the cylinder radius, and determine whether there is an obstacle in the offset optimized cylinder. When there is an obstacle, change the cylinder radius and update the offset optimized cylinder until there is no obstacle in the offset optimized cylinder. Determine the growth direction according to the path endpoints in the offset optimized cylinder, so that the path generation algorithm obtains the initial path according to the transformed random point and the growth direction; Target path module: It is used to obtain the top environment model through machine learning and perform top safety limit constraints, obtain obstacle safety constraints through the underwater topographic map, obtain the coordinates of random tree nodes, obtain the vehicle angle constraints based on the coordinates of random tree nodes, and constrain the initial path according to the top safety limit constraints, obstacle safety constraints and vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

[0012] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, the steps of the three-dimensional route planning method for restricted waters as described in any one of the above are implemented.

[0013] The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the three-dimensional route planning method for restricted waters as described in any one of the above are implemented.

[0014] One or more of the above technical solutions in the embodiments of the present invention have at least one of the following technical effects: The three-dimensional route planning method, system, device and medium for restricted waters provided by the present invention make the distribution of random points more conform to the direction of the line connecting the path endpoints in the underwater environment restricted at the top by calculating the stretching height and performing elliptical transformation, thereby improving the efficiency of subsequent path planning; using the offset optimization cylinder makes the growth direction of the path generation algorithm also more conform to the direction of the line connecting the path endpoints, and further obtains the top environment model to constrain the initial path, so that the method can adapt to the underwater vehicle route planning task in the underwater environment restricted at the top and generate a suitable target path for it more efficiently.

[0015] Additional aspects and advantages of the present invention will be given in part in the following description, become apparent in part from the following description, or be understood through the practice of the present invention. Description of the Drawings

[0016] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0017] Figure 1 It is a schematic flowchart of the three-dimensional route planning method for restricted waters provided by the present invention.

[0018] Figure 2 It is a schematic diagram of the elliptical area and stretching height of the three-dimensional route planning method for restricted waters provided by the present invention.

[0019] Figure 3(a) is a schematic diagram of the pitch angle of the three-dimensional route planning method in restricted waters provided by the present invention.

[0020] Figure 3(b) is a schematic diagram of the turning angle of the three-dimensional route planning method in restricted waters provided by the present invention.

[0021] Figure 4 is a schematic diagram of the target path of the three-dimensional route planning method in restricted waters provided by the present invention.

[0022] Figure 5 is a schematic diagram of the structure of the three-dimensional route planning system in restricted waters provided by the present invention.

[0023] Figure 6 is a schematic diagram of the structure of the three-dimensional route planning device in restricted waters provided by the present invention.

[0024] Reference numerals: 100, underwater topographic map module; 200, transformation matrix module; 300, elliptical transformation module; 400, initial path module; 500, target path module; 810, processor; 820, communication interface; 830, memory; 840, communication bus. Detailed implementation manners

[0025] In order to make the objectives, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present invention without making creative efforts shall fall within the protection scope of the present invention. The following embodiments are used to illustrate the present invention, but cannot be used to limit the scope of the present invention.

[0026] In the description of the embodiments of the present invention, it should be noted that the terms "first", "second", and "third" are only used for descriptive purposes and cannot be construed as indicating or implying relative importance.

[0027] In the description of this specification, the descriptions referring to terms such as "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the embodiments of the present invention. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in a suitable manner in any one or more embodiments or examples. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0028] The following will describe the specific implementation manners of the present invention in conjunction with Figures 1 to 6 Describe the specific implementation manners of the present invention: Figure 1 is a schematic flowchart of a three-dimensional route planning method for restricted waters provided by the present invention.

[0029] First, determine the target waters, obtain the underwater topographic map of the target waters, and determine the path endpoints in the target waters; subsequently, determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; then calculate the stretching height through the stretching coefficient, determine random points according to the stretching height and perform elliptical transformation to obtain transformed random points; then determine the growth direction according to the path endpoints in the bias optimization cylinder, and the path generation algorithm obtains the initial path according to the growth direction; finally, constrain the initial path according to the top safety constraint, obstacle safety constraint, and vehicle angle constraint to obtain the target path.

[0030] The present invention provides a three-dimensional route planning method for restricted waters, including: S1: Determine the target waters, obtain the underwater topographic map of the target waters, and determine the path endpoints in the target waters; Furthermore, the purpose of this stage is to obtain the underwater topographic map of the target waters so as to determine the path endpoints therein. Specifically, step S1 specifically includes: S11: Determine the target waters, conduct topographic surveys on the target waters to obtain the underwater topographic map of the target waters; S12: Determine the path end point and path start point of the underwater vehicle in the underwater topographic map, and use the path end point and the path start point as the path endpoints.

[0031] For the above steps, the specific implementation schemes in this embodiment are as follows: First, it is necessary to determine the target water area, that is, the water area where the underwater vehicle conducts activities. Then, the underwater terrain of the target water area is detected by sonar to obtain an underwater topographic map that can reflect the distribution state of the underwater topographic features of the target water area. Here, the target water area is a water area with limited top space, such as the area under the ice sheet in the polar ocean. Then, according to the position of the underwater vehicle and the detection purpose, the path end point and the path start point of the underwater vehicle are determined in the underwater topographic map, and the path end point and the path start point are used as the path end points.

[0032] S2: Determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; Furthermore, the purpose of this stage is to determine the water area size, so as to obtain the elliptical transformation matrix and determine the transformation matrix. Specifically, step S2 specifically includes: S21: Determine the water area size including the water area length and the water area width in the underwater topographic map, calculate the elliptical foci and the elliptical major axis length according to the water area size, and obtain the elliptical transformation matrix through the elliptical foci and the elliptical major axis length; S22: Determine the path end point vector according to the path end points, determine the included angle of the end point vectors through the path end point vector, and thus construct the transformation matrix using the included angle of the end point vectors.

[0033] For the above steps, the specific implementation methods in this embodiment are as follows: First, it is necessary to determine the water area size including the water area length a and the water area width b in the underwater topographic map. Subsequently, the elliptical foci and the elliptical major axis length can be calculated according to the water area size: where is the first adjustment coefficient determined according to experience, is the second adjustment coefficient determined according to experience. Subsequently, the elliptical transformation matrix L can be obtained through the elliptical foci and the elliptical major axis length: where diag{} represents the diagonal matrix of the content in the brackets.

[0034] Then, determine the path end point vector according to the path end points. Here, the end point vector is the vector pointing from the path start point to the path end point. Determine the included angle θ of the end point vectors through the path end point vector. Here, the included angle of the end point vectors is the included angle between the end point vector and the direction of the water area length in the horizontal direction. Subsequently, the transformation matrix C can be constructed: S3: Determine the stretching coefficient, calculate the stretching height based on the stretching coefficient and the path endpoints, determine random points according to the stretching height, and perform elliptical transformation on the random points through the elliptical transformation matrix and the transformation matrix to obtain transformed random points; Further, the purpose of this stage is to determine the stretching coefficient and calculate the stretching height, so as to determine random points and perform elliptical transformation on the random points through the elliptical transformation matrix and the transformation matrix to obtain transformed random points. Specifically, step S3 specifically includes: S31: Determine the stretching coefficient, where the value of the stretching coefficient is 1.2 - 1.5, calculate the endpoint height difference of the path endpoints, and calculate the initial stretching height through the stretching coefficient and the endpoint height difference; S32: Obtain the safety distance through the initial stretching height, adjust the initial stretching height according to the safety distance to obtain the stretching height, and determine the stretching ellipse according to the stretching height; S33: Select random points according to the stretching height, determine the plane coordinates of the random points of the random points, perform elliptical transformation on the plane coordinates of the random points through the elliptical transformation matrix and the transformation matrix to obtain random point transformation coordinates, and obtain the transformed random points located within the stretching ellipse according to the random point transformation coordinates.

[0035] For the above steps, the specific implementation methods in this embodiment are as follows: First, it is necessary to determine the stretching coefficient according to experience , whose value range is 1.2 - 1.5. Subsequently, calculate the endpoint height difference between the height of the path start point and the height of the path end point, so as to calculate the initial stretching height : Among them, is the height of the path start point, is the height of the path end point. Subsequently, make half of the initial stretching height be located at the center of the connection line between the path start point and the path end point, and then the height at the top of the initial stretching height can be used as the top height , and obtain the lowest height of the obstacles at the top of the water area from the underwater topographic map as the obstacle height , and calculate the difference between the two to obtain the safety distance : Then, adjust the initial stretching height according to the safety distance, that is, when the safety distance > 0, it is necessary to subtract the safety distance from the initial stretching height to obtain the stretching height , this can avoid stretching the height above the obstacle at the top of the water area. Otherwise, the initial stretching height is directly used as the stretching height. Subsequently, the stretching ellipse is determined according to the stretching height. The stretching ellipse is an ellipse whose projection on the horizontal plane is an ellipse, and the center of the ellipse is located at the center of the line connecting the path start point and the path end point. Its foci and major axis length are respectively the ellipse foci and ellipse major axis length calculated previously, and the height is the height of the stretching ellipse area. The ellipse area also needs to ensure that the height where half of the stretching height is located is the same as the height of the center of the line connecting the path start point and the path end point. The ellipse area and the stretching height are as Figure 2 shown.

[0036] In a general algorithm, the range for taking random points is generally a circular area centered at the center of the line connecting the path start point and the path end point and with a diameter greater than the line connecting the path start point and the path end point. Therefore, random points are first taken in the circular area, and the height of the random points is located within the stretching height. Subsequently, the coordinates of the projections of these random points on the plane are used as the random point plane coordinates , and the random point plane coordinates are ellipse-transformed through the ellipse transformation matrix and the transformation matrix so that the random point plane coordinates are within the horizontal projection of the ellipse area: Among them, is the horizontal projection coordinate of the path start point, is the horizontal projection coordinate of the path end point, is the horizontal projection coordinate of the transformed random point, is the ellipse center coordinate. Since the ellipse transformation does not change the height, the height coordinate of the random point is kept unchanged and combined with the horizontal projection coordinate of the transformed random point to obtain the random point transformation coordinate, and the transformed random point is obtained according to the random point transformation coordinate. In this way, the transformed random point is within the stretching ellipse, which can make the growth direction of the random tree more concentrated when the algorithm obtains the initial path, so as to obtain the initial path faster and reduce the delay caused by the operation.

[0037] S4: Select the path generation algorithm and the cylinder radius, generate the offset-optimized cylinder according to the cylinder radius, and determine whether there is an obstacle in the offset-optimized cylinder. When there is an obstacle, change the cylinder radius and update the offset-optimized cylinder until there is no obstacle in the offset-optimized cylinder. Determine the growth direction according to the path endpoints in the offset-optimized cylinder, so that the path generation algorithm obtains the initial path according to the transformed random points and the growth direction; Further, the purpose of this stage is to generate an offset-optimized cylinder based on the cylinder radius and update the offset-optimized cylinder, so as to determine the growth direction according to the path endpoints and obtain the initial path. Specifically, in step S4, when there is an obstacle, the cylinder radius is reduced and a new offset-optimized cylinder is generated according to the reduced cylinder radius until there is no obstacle in the offset-optimized cylinder, completing the update of the offset-optimized cylinder. And in the offset-optimized cylinder, the direction from the path start point to the path end point is used as the growth direction. The path generation algorithm performs random tree growth by transforming random points and according to the growth direction to obtain the initial path.

[0038] For the above steps, the specific implementation in this embodiment is as follows: First, select a path generation algorithm. Here, the path generation algorithm selects the algorithm. In addition, select the cylinder radius R according to experience, so as to generate an offset-optimized cylinder with the path start point as the center, the cylinder radius as the radius, and the height as the stretching height, and determine whether there is an obstacle in the offset-optimized cylinder. When there is an obstacle, reduce the cylinder radius and keep other parameters unchanged to generate a new offset-optimized cylinder according to the reduced cylinder radius, and repeat this process until there is no obstacle in the offset-optimized cylinder, thus completing the update of the offset-optimized cylinder.

[0039] In the offset-optimized cylinder, the direction from the path start point to the path end point is used as the growth direction, and the path generation algorithm is made to use the transformed random points to perform random tree growth along the growth direction until reaching the path end point, thereby obtaining the initial path.

[0040] S5: Obtain the top environment model through machine learning and perform top safety limit constraints, obtain obstacle safety constraints through the underwater topographic map, obtain the random tree node coordinates, obtain the vehicle angle constraints through the random tree node coordinates, and constrain the initial path according to the top safety limit constraints, obstacle safety constraints, and vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

[0041] Further, the purpose of this stage is to obtain the top safety limit constraints, obstacle safety constraints, and vehicle angle constraints, so as to constrain the initial path and obtain the target path. Specifically, in step S5, obtain the top environment data, clean, preprocess, and interpolate the top environment data to obtain the top environment input data, perform machine learning on the top environment input data to obtain the top environment model, and perform the top safety limit constraints through the top environment model.

[0042] In step S5, obtain the coordinates of the random tree nodes including the three-dimensional coordinates of the nodes and the three-dimensional coordinates of the adjacent nodes, and obtain the angle constraints of the underwater vehicle including the pitch angle constraint and the yaw angle constraint through the coordinates of the random tree nodes.

[0043] For the above steps, the specific implementation in this embodiment is as follows: First, it is necessary to obtain the top environmental data of the top obstacles from the underwater topographic map, that is, various data related to the top obstacles, and clean, preprocess, and interpolate the top environmental data, so as to remove unreasonable and incomplete data and complete the data, and obtain the top environmental input data. Then, use the top environmental input data for machine learning to obtain a relatively perfect top environmental model. Then, determine the safety distance according to experience ， Perform top safety limit constraints through the top environmental model, that is, it is necessary to ensure that the distance between any point on the target path and any point in the top environmental model is greater than the safety distance.

[0044] Next, formulate obstacle safety constraints, that is, for other obstacles except the top obstacles, expand their sizes by 1.5 to 2 times, and ensure that the target path does not contact the expanded obstacles, so as to complete the formulation of obstacle safety constraints.

[0045] Finally, obtain the three-dimensional coordinates of the nodes including the path points on the initial path and the three-dimensional coordinates of the adjacent nodes of the random tree nodes, and then the pitch angle constraint can be formulated: where is the pitch angle of the underwater vehicle, is the x-axis direction coordinate of the path point in the left-handed space rectangular coordinate system, is the y-axis direction coordinate of the path point in the left-handed space rectangular coordinate system, is the z-axis direction coordinate of the path point in the left-handed space rectangular coordinate system, is the x-axis direction coordinate of the adjacent node of the path point in the left-handed space rectangular coordinate system, is the y-axis direction coordinate of the adjacent node of the path point in the left-handed space rectangular coordinate system, is the z-axis direction coordinate of the adjacent node of the path point in the left-handed space rectangular coordinate system. Here, the adjacent node is the other path point closest to the path point, is the pitch angle limit value of the underwater vehicle, is the reciprocal of the tangent function. The pitch angle is shown in Fig. 3(a), where the coordinate system is the left-handed space coordinate system.

[0046] It is also necessary to use the node plane coordinates obtained from the three-dimensional coordinates of the nodes , the adjacent node planar coordinates obtained from the three-dimensional coordinates of adjacent nodes and the planar coordinates of the parent node of the adjacent node of the path point obtained from the initial path Formulate the turning angle constraint: Among them, is the first intermediate vector, is the second intermediate vector, is the x-axis direction coordinate of the parent node of the adjacent node of the path point in the left-handed space rectangular coordinate system, is the y-axis direction coordinate of the parent node of the adjacent node of the path point in the left-handed space rectangular coordinate system, β is the turning angle of the underwater vehicle, is the maximum turning angle of the underwater vehicle, is the inverse cosine function. The turning angle is shown in Fig. 3(b). Adjust the initial path to make it meet the constraints of the top safety limit constraint, the obstacle safety constraint and the vehicle angle constraint, and the target path for the underwater vehicle to navigate can be obtained, and the underwater vehicle navigates according to the target path. The target path is as shown in Figure 4 , where the distance unit is meters. The time taken by the method provided by the present invention to obtain the target path is about 92% less than that of the original algorithm. For the underwater top-constrained navigation simulation environment with a size of 4000m * 4000m * 100m, the time taken by the present invention to obtain the target path does not exceed 2s.

[0047] The present invention provides a method for three-dimensional route planning of an underwater vehicle in a restricted water area, enabling the underwater vehicle to quickly generate a relatively smooth and less fluctuating target path and navigate according to the target path.

[0048] The following describes the restricted water area three-dimensional route planning device provided by the present invention. The restricted water area three-dimensional route planning device described below can be mutually referred to the restricted water area three-dimensional route planning method described above.

[0049] Figure 5 Illustrates the structural schematic diagram of the restricted water area three-dimensional route planning system, as shown in Figure 5 , for executing the restricted water area three-dimensional route planning method as described above, including: Underwater topographic map module 100: used to determine the target water area, obtain the underwater topographic map of the target water area, and determine the path endpoints in the target water area; Transformation matrix module 200: used to determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; Ellipse transformation module 300: used to determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path endpoints, determine random points according to the stretching height, and perform ellipse transformation on the random points through the ellipse transformation matrix and the transformation matrix to obtain transformed random points; Initial path module 400: used to select a path generation algorithm and a cylinder radius, generate an offset-optimized cylinder according to the cylinder radius, and determine whether there are obstacles in the offset-optimized cylinder. When there are obstacles, change the cylinder radius and update the offset-optimized cylinder until there are no obstacles in the offset-optimized cylinder. Determine the growth direction in the offset-optimized cylinder according to the path endpoints, so that the path generation algorithm obtains an initial path according to the transformed random points and the growth direction; Target path module 500: used to obtain the top environment model through machine learning and perform top safety limit constraints, obtain obstacle safety constraints through the underwater topographic map, obtain the coordinates of random tree nodes, obtain the vehicle angle constraints through the coordinates of random tree nodes, and perform constraints on the initial path according to the top safety limit constraints, obstacle safety constraints and vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

[0050] On the other hand, Figure 6 An example of a schematic physical structure of an electronic device is shown in Figure 6 As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830, and a communication bus 840. Among them, the processor 810, the communication interface 820, and the memory 830 complete mutual communication through the communication bus 840. The processor 810 can call the logical instructions in the memory 830 to execute the three-dimensional route planning method in restricted waters, and the method includes: S1: Determine the target water area, obtain the underwater topographic map of the target water area, and determine the path endpoints in the target water area; S2: Determine the water area size in the underwater topographic map, obtain the ellipse transformation matrix according to the water area size, and determine the transformation matrix; S3: Determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path endpoints, determine random points according to the stretching height, and perform ellipse transformation on the random points through the ellipse transformation matrix and the transformation matrix to obtain transformed random points; S4: Select a path generation algorithm and a cylinder radius, generate an offset-optimized cylinder according to the cylinder radius, and determine whether there are obstacles in the offset-optimized cylinder. When there are obstacles, change the cylinder radius and update the offset-optimized cylinder until there are no obstacles in the offset-optimized cylinder. Determine the growth direction in the offset-optimized cylinder according to the path endpoints, so that the path generation algorithm obtains an initial path according to the transformed random points and the growth direction; S5: Obtain the top environment model through machine learning and perform top safety limit constraints. Obtain obstacle safety constraints from the underwater topographic map, acquire the coordinates of the random tree nodes, obtain the vehicle angle constraints based on the coordinates of the random tree nodes, and constrain the initial path according to the top safety limit constraints, obstacle safety constraints, and vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

[0051] In addition, when the logical instructions in the above-mentioned memory 830 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0052] On the other hand, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it is implemented to execute the restricted water area three-dimensional route planning method provided by the above-mentioned various methods. The method includes: S1: Determine the target water area, obtain the underwater topographic map of the target water area, and determine the path endpoints in the target water area; S2: Determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; S3: Determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path endpoints, determine the random points according to the stretching height, and perform elliptical transformation on the random points through the elliptical transformation matrix and the transformation matrix to obtain the transformed random points; S4: Select a path generation algorithm and a cylinder radius, generate an offset optimization cylinder according to the cylinder radius, and determine whether there are obstacles in the offset optimization cylinder. When there are obstacles, change the cylinder radius and update the offset optimization cylinder until there are no obstacles in the offset optimization cylinder. Determine the growth direction in the offset optimization cylinder according to the path endpoints, so that the path generation algorithm obtains the initial path according to the transformed random points and the growth direction; S5: Obtain the top environment model through machine learning and perform top safety boundary constraints, obtain obstacle safety constraints through the underwater topographic map, acquire the coordinates of the random tree nodes, obtain the angle constraints of the vehicle through the coordinates of the random tree nodes, and constrain the initial path according to the top safety boundary constraints, obstacle safety constraints, and vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

[0053] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art can understand and implement it without creative efforts.

[0054] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus the necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0055] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments or equivalently replace some of the technical features. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A three-dimensional route planning method for restricted waters, characterized in that, Including: S1: Determine the target water area, obtain the underwater topographic map of the target water area, and determine the path endpoints in the target water area; S2: Determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; S3: Determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path endpoints, determine random points according to the stretching height, and perform elliptical transformation on the random points through the elliptical transformation matrix and the transformation matrix to obtain the transformed random points; S4: Select a path generation algorithm and a cylinder radius, generate an offset-optimized cylinder according to the cylinder radius, and determine whether there are obstacles in the offset-optimized cylinder. When there are obstacles, change the cylinder radius and update the offset-optimized cylinder until there are no obstacles in the offset-optimized cylinder. Determine the growth direction in the offset-optimized cylinder according to the path endpoints, so that the path generation algorithm obtains the initial path according to the transformed random points and the growth direction; S5: Obtain the top environment model through machine learning and perform top safety limit constraints, obtain obstacle safety constraints through the underwater topographic map, obtain the random tree node coordinates, obtain the vehicle angle constraints through the random tree node coordinates, and perform constraints on the initial path according to the top safety limit constraints, obstacle safety constraints, and vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

2. The three-dimensional route planning method for restricted waters according to claim 1, wherein Step S1 specifically includes: S11: Determine the target water area, conduct a topographic survey on the target water area, and obtain the underwater topographic map of the target water area; S12: Determine the path end point and the path start point of the underwater vehicle in the underwater topographic map, and use the path end point and the path start point as the path endpoints.

3. The three-dimensional route planning method for restricted waters according to claim 1, wherein Step S2 specifically includes: S21: Determine the water area size including the water area length and the water area width in the underwater topographic map, calculate the elliptical foci and the elliptical major axis length according to the water area size, and obtain the elliptical transformation matrix through the elliptical foci and the elliptical major axis length; S22: Determine the path endpoint vector according to the path endpoints, determine the included angle of the endpoint vectors through the path endpoint vector, and thus construct the transformation matrix using the included angle of the endpoint vectors.

4. The three-dimensional route planning method for restricted waters according to claim 1, characterized in that Step S3 specifically includes: S31: Determine the stretching coefficient, where the value of the stretching coefficient is 1.2 - 1.5, calculate the endpoint height difference of the path endpoints, and calculate the initial stretching height through the stretching coefficient and the endpoint height difference; S32: Obtain the safety distance through the initial stretching height, adjust the initial stretching height according to the safety distance to obtain the stretching height, and determine the stretching ellipse according to the stretching height; S33: Select random points according to the stretching height, determine the random point plane coordinates of the random points, perform elliptical transformation on the random point plane coordinates through the elliptical transformation matrix and the transformation matrix to obtain the random point transformation coordinates, and obtain the transformed random points located in the stretching ellipse according to the random point transformation coordinates.

5. The three-dimensional route planning method for restricted waters according to claim 1, characterized in that, In step S4, when there is an obstacle, reduce the radius of the cylinder and generate a new offset-optimized cylinder according to the reduced radius of the cylinder until there is no obstacle in the offset-optimized cylinder, complete the update of the offset-optimized cylinder, and use the direction from the path start point to the path end point in the offset-optimized cylinder as the growth direction. The path generation algorithm grows a random tree by transforming the random points and according to the growth direction to obtain the initial path.

6. The three-dimensional route planning method for restricted waters according to claim 1, wherein In step S5, obtain the top environment data, clean, preprocess, and interpolate the top environment data to obtain the top environment input data, perform machine learning on the top environment input data to obtain the top environment model, and perform the top safety limit constraint through the top environment model.

7. The three-dimensional route planning method for restricted waters according to claim 1, wherein In step S5, obtain the random tree node coordinates including the three-dimensional coordinates of the node and the three-dimensional coordinates of the adjacent nodes, and obtain the angle constraints of the vehicle including the pitch angle constraint and the yaw angle constraint through the random tree node coordinates.

8. A three-dimensional route planning system for restricted waters, which is used to execute the three-dimensional route planning method for restricted waters according to any one of claims 1 to 7, characterized in that, Comprising: Underwater topographic map module: used to determine the target water area, obtain the underwater topographic map of the target water area, and determine the path end points in the target water area; Transformation matrix module: used to determine the water area size in the underwater topographic map, obtain the elliptical transformation matrix according to the water area size, and determine the transformation matrix; Elliptical transformation module: used to determine the stretching coefficient, calculate the stretching height through the stretching coefficient and the path end points, determine the random points according to the stretching height, and perform elliptical transformation on the random points through the elliptical transformation matrix and the transformation matrix to obtain the transformed random points; Initial path module: used to select a path generation algorithm and a cylinder radius, generate an offset-optimized cylinder according to the cylinder radius, and determine whether there is an obstacle in the offset-optimized cylinder. When there is an obstacle, change the cylinder radius and update the offset-optimized cylinder until there is no obstacle in the offset-optimized cylinder. Determine the growth direction according to the path end points in the offset-optimized cylinder, so that the path generation algorithm obtains the initial path according to the transformed random points and the growth direction; Target path module: used to obtain the top environment model through machine learning and perform the top safety limit constraint, obtain the obstacle safety constraint through the underwater topographic map, obtain the random tree node coordinates, obtain the vehicle angle constraints through the random tree node coordinates, and perform constraints on the initial path according to the top safety limit constraint, the obstacle safety constraint, and the vehicle angle constraints to obtain the target path for the underwater vehicle to navigate.

9. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the restricted water area three-dimensional route planning method according to any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the restricted water area three-dimensional route planning method according to any one of claims 1 to 7.

Images