Improved a* underwater vehicle path planning method fusing marine environment

Abstract

This invention belongs to the field of underwater vehicle path planning technology, and relates to an improved A* underwater vehicle path planning method that integrates marine environment. The method includes: acquiring marine environmental parameters, selecting sound propagation characteristics, seabed topography, and seawater transparency as environmental factors; employing the Analytic Hierarchy Process (AHP) for weighting, designing a cost function based on physical influences and weighted fusion to construct a navigation cost model; using navigation cost and Euclidean distance together as node evaluation costs to improve the A* algorithm cost function; using the improved A* algorithm for path search, combining kinematics and marine environmental constraints for path smoothing optimization to obtain the final planned path; simultaneously eliminating dangerous and impassable nodes based on navigation cost thresholds to construct a navigable space, with the final planned path and navigable space jointly serving as the output. This invention achieves comprehensive evaluation of navigation costs and smooth path optimization under multiple environmental constraints, improving path safety and feasibility.

Description

Technical Field

[0001] This invention belongs to the field of underwater vehicle path planning technology, specifically relating to an improved A* underwater vehicle path planning method that integrates the marine environment. Background Technology

[0002] When underwater vehicles perform underwater exploration and operations, path planning is a core technical aspect that ensures their navigation safety and mission effectiveness. The navigation safety and performance of underwater vehicles are significantly affected by marine environmental factors: seawater temperature, salinity, and structure affect buoyancy and drag; seabed topography and sediment conditions restrict the safety of near-bottom navigation; and seawater transparency and acoustic propagation environment affect the vehicle's detection capabilities and stealth.

[0003] In existing path planning techniques, the traditional A* algorithm primarily focuses on obstacle avoidance and path shortest distance optimization, with limited consideration of the aforementioned marine environmental characteristics, making it difficult to comprehensively assess navigation costs under multiple environmental constraints. Furthermore, existing methods generally lack a systematic introduction of kinematic constraints on underwater vehicles, such as minimum turning radius, rate of change of heading angle, and pitch angle limits, resulting in poor trackability of the planned path during actual navigation and frequent occurrences of unexecutable trajectories. This is a shortcoming of existing technologies.

[0004] In view of this, it is very necessary to provide an improved path planning method for A* underwater vehicles that integrates the marine environment to solve the above-mentioned defects in the prior art. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of existing underwater path planning technologies, such as limited consideration of marine environmental characteristics and lack of systematic introduction of kinematic constraints for underwater vehicles, by providing an improved A* underwater vehicle path planning method and system that integrates the marine environment, thereby solving the aforementioned technical problems.

[0006] To achieve the above objectives, the present invention provides the following technical solution: An improved path planning method for A* underwater vehicles that incorporates the marine environment includes the following steps: Step S1: Obtain marine environmental parameters of the underwater vehicle's operating area, and select the main influencing factors affecting the underwater vehicle's activities as environmental factors; design corresponding cost functions based on the physical impact of each environmental factor on the underwater vehicle's activities, use the analytic hierarchy process (AHP) to assign weights to each environmental factor, construct a judgment matrix and normalize it to obtain the weight coefficients of each environmental factor; use the weight coefficients of each environmental factor as the coefficients of the corresponding cost functions, and fuse them to form a navigation cost model; Step S2: The calculated value of the navigation cost model and the Euclidean distance are used together as components of the path node evaluation cost; based on the navigation cost model and the path node evaluation cost, the cost function of the A* algorithm is improved. Step S3: The improved A* algorithm is used to expand nodes and search paths to obtain the initial planned path; Step S4: Combining the kinematic constraints of the underwater vehicle and the constraints of the marine environment, the initial planned path is optimized to obtain the final underwater planned path; Step S5: Based on a preset navigation cost threshold, dangerous and impassable nodes are removed from all nodes in the operational sea area to obtain feasible nodes. These feasible nodes form a locally continuous navigable space. The final underwater planned path and the navigable space together constitute the path planning output of the method.

[0007] Preferably, step S1 specifically includes: S11, Obtain marine environmental parameters of the sea area where the underwater vehicle is operating. The marine environmental parameters include: hydrological parameters, seabed topography parameters, seabed sediment parameters, ocean transparency parameters, and acoustic propagation characteristics parameters. Based on the criteria of the direct physical impact of various marine environmental parameters on the navigation safety and mission effectiveness of underwater vehicles, the main influencing factors that physically affect the activities of underwater vehicles are selected as environmental factors, including: sound propagation characteristics factor, seabed topography factor, and seawater transparency factor. S12, Design the corresponding cost function based on the physical impact of each environmental factor on the activities of underwater vehicles; S13, use the AHP (Analog-Hybrid Analysis) method to assign weights to each environmental factor: based on the experts' assessment of the relative importance of each environmental factor, use the 1-9 scale method to compare each environmental factor pairwise and assign values, fill the corresponding positions in the judgment matrix with the assigned values, and the judgment matrix is ​​completed when it is filled. Calculate the largest eigenvalue of the judgment matrix and its corresponding eigenvector. Each element in the eigenvector corresponds to the initial weight of each environmental factor. Normalize the eigenvector to obtain the weight coefficient of each environmental factor. S14: The weight coefficients are used as the coefficients of the corresponding cost functions. The cost functions are weighted and fused to form a navigation cost model. The navigation cost model calculation value of a single path node is the navigation cost value of that node.

[0008] This step can achieve the following technical effects: By collecting multi-dimensional marine environmental parameters such as hydrology, seabed topography, seabed sediment, ocean transparency, and acoustic propagation environment, comprehensive basic data is provided for subsequent navigation cost modeling. The AHP (Analytic Hierarchy Process) method is used to assign weights to multiple types of marine environmental factors and normalize them, realizing the same quantification of multiple environmental constraints. The cost function is designed based on the physical influence mechanism, so that the navigation cost model can truly reflect the actual impact of the marine environment on the activities of underwater vehicles.

[0009] Preferably, the cost function in step S12 includes: the sound propagation loss and detection probability function corresponding to the sound propagation characteristic factor, the collision risk cost function corresponding to the seabed topography factor, and the optical detection concealment cost function corresponding to the seawater transparency factor. The sound propagation loss and detection probability function are coupled, and the influence of the sound propagation loss distribution on navigation stealth is comprehensively characterized by the influence of depth changes on acoustic stealth under the action of sound propagation characteristics. The specific expression is as follows: , in, D The ocean depth at which the current path node is located. This represents the maximum ocean depth at the corresponding planar location of the current path node. P The probability of being detected by external acoustic detection equipment. P The larger the value, the more favorable the sound propagation conditions are at that location, the higher the risk of exposure for the detection vehicle, and the greater the corresponding acoustic value of the node. The collision risk cost function, coupled with the influence of seabed parameters on acoustic detection effectiveness, comprehensively characterizes the combined effect of topographic relief and seabed type on navigation safety and detection risk. The specific expression is as follows: , in, M Let the collision risk cost function be... D The ocean depth at which the current path node is located. i Slope grade type, N i For the first i The number of slope grades Specifically, the study area is divided into a square grid of 5×5 sampling points, and then... i The spatial distribution probability of a slope grade within its corresponding grid cell is calculated using the following expression: , in, N This represents the total number of slope grades. The optical detection concealment cost function characterizes the impact of water body optical properties on the exposure risk of underwater vehicles, and its specific expression is as follows: , in, The cost of optical detection stealth, i.e., apparent contrast. H SDD The transparency depth of this sea area, The diffuse reflectance at an infinite depth of water at the current transparency depth. D The ocean depth at which the current path node is located. D max This represents the maximum ocean depth at the current path node's corresponding planar location.

[0010] This step can achieve the following technical effects: By designing cost functions for sound propagation characteristics, seabed topography, and seawater transparency, the specific physical impact of each environmental parameter on underwater vehicles—including sound propagation loss and detection probability, collision risk, stealth, and sonar detection range—is quantified. This gives the navigation cost model a clear physical meaning and avoids the crude approach of simply treating environmental factors as obstacles.

[0011] Preferably, in step S14, the weighting coefficients can be adjusted by processing with a preset mathematical function after normalization.

[0012] This step can achieve the following technical effects: Based on the weight coefficients obtained by AHP normalization, further transformations are performed through preset mathematical functions. This allows for fine adjustment of the weight distribution while maintaining the relative order of each environmental factor, adapting to different sea area characteristics or mission preferences. This adjustment does not require reorganizing expert scoring, reducing the repetitive cost of weight calibration, enhancing the navigation cost model's ability to quickly adapt to different application scenarios, and improving the flexibility and engineering practicality of model parameter adjustment.

[0013] Preferably, step S2 specifically includes: S21. Based on the longitude, latitude, and depth of the starting point of the path, and the longitude, latitude, and depth of the ending point of the path, construct a starting node and an ending node respectively; each starting node and ending node contains the following data fields: longitude, latitude, depth, cost incurred, estimated cost, and evaluation cost; wherein, the cost incurred is the sum of the travel costs from the starting node to the current node of the planned route, the estimated cost is the Euclidean distance from the current node to the ending node, and the evaluation cost is the sum of the cost incurred and the estimated cost; S22, construct an open table and a closed table for path planning; the open table is a priority pre-selected path node queue, storing nodes to be evaluated marked as "open", and all nodes in the table are arranged in ascending order of evaluation cost; the closed table is an unordered set, storing nodes that have been expanded and marked as "close". S23 uses the node evaluation cost, which consists of the travel cost and the Euclidean distance, as the cost function of the improved A* algorithm.

[0014] This step can achieve the following technical effects: By incorporating both the cost of navigation and Euclidean distance into the node evaluation cost, the A* algorithm achieves dual control over path node selection, considering both path distance and navigation cost. Expanding nodes are optimized based on comprehensive cost, achieving a balance between safety priority and shortest path. The division of labor between open and closed tables ensures the orderliness and efficiency of the path search process. The node data structure includes three-dimensional information of longitude, latitude, and depth, making it suitable for underwater three-dimensional spatial path planning.

[0015] Preferably, step S3 specifically includes: S31, mark the starting node as "open" and calculate its evaluation cost, and mark the adjacent nodes around the starting node as "open" as well. S32, Select new nodes in the navigable space that are not marked as "open" or "close" as nodes to be expanded, and mark them as "close"; S33, Perform dynamic update processing on the node: Traverse all neighboring nodes of the currently processed node. If a node has been marked as "open", calculate the evaluation cost of the node and select the node with the smallest evaluation cost from all "open" nodes to mark it as "close". If a node has been marked as "close", remark the node as "open" and calculate the evaluation cost of the node for re-evaluation. S34. When the end node is reached, the calculation stops. All nodes marked as "close" are arranged in descending order of evaluation cost to obtain the optimal path node sequence.

[0016] This step can achieve the following technical effects: By marking and dynamically updating the "open" and "close" states of nodes, the efficiency and global optimality of path search are ensured; nodes marked as "close" are allowed to be remarked as "open" and participate in re-evaluation, avoiding the omission of the globally optimal path due to local optimal selection; all "close" nodes are backtracked and sorted to ensure that the generated path node sequence is optimal in the sense of comprehensive cost.

[0017] Preferably, step S3 further includes: According to a preset scaling ratio, the navigation cost value of each path node in the optimal path node sequence is scaled to construct a navigation cost value range for each path node; taking the optimal path node sequence as the path axis, non-path nodes perpendicular to the path axis and located within the navigable space are obtained; the navigation cost value of the non-path nodes is compared with the navigation cost value range, and non-path nodes that meet the range limit are used as navigable candidate nodes to expand the safe navigation area around the optimal path. The optimal path node sequence is used as the initial planned path.

[0018] This step can achieve the following technical effects: By scaling the navigation cost of each node in the optimal path node sequence to construct a numerical range, alternative nodes that meet the safety conditions are selected around the optimal path, reserving adjustment space for subsequent smoothing under kinematic constraints, and avoiding the single optimal path from becoming too rigid and causing out-of-bounds after smoothing.

[0019] Preferably, the underwater vehicle kinematic constraints and marine environment constraints in step S4 specifically include: The kinematic constraints include: minimum turning radius constraint, used to limit the path curvature to not exceed the upper limit of the vehicle's maneuverability; heading angle variation constraint, used to limit the amount of heading angle variation between adjacent path segments to within the allowable range; path slope range constraint, used to limit the longitudinal slope of the path to not exceed the vehicle's maximum climb angle and maximum dive angle; and depth undulation constraint, used to limit the rate of change of depth in the vertical direction of the path to remain gentle. The marine environmental constraint is that the smoothed path must remain entirely within navigable space.

[0020] This step can achieve the following technical effects: By introducing multi-dimensional kinematic constraints such as minimum turning radius, heading angle change, path slope, and depth undulation, the planned path is ensured to conform to the actual maneuverability of the underwater vehicle, thereby improving the trackability and feasibility of the path. The marine environment constraints confine the entire path within the navigable space, ensuring that the optimized path always meets environmental safety requirements.

[0021] Preferably, step S4 specifically includes: S41 sets the kinematic constraints and marine environmental constraints for underwater vehicles; S42, smooth the optimal path node sequence in the initial planned path; the smoothing process uses non-uniform B-spline interpolation to fit the optimal path node sequence and generate a smooth path curve; During the smoothing process, the kinematic constraints are used as constraints to control the adjustment of the node positions, so that the generated smooth path curve meets the maneuverability requirements of the underwater vehicle. S43 uses Gaussian filtering to filter the smooth path curve, eliminating local jitter and sharp corners in the path, and eliminating inflection points and stuttering points while keeping the overall direction of the path unchanged. S44, Based on the marine environment constraints, the path after smoothing and filtering is checked point by point: it is determined whether each node on the path meets the set kinematic constraints and marine environment constraints, and the points that meet the conditions are used as the constituent nodes of the final underwater planning path. S45 generates the final underwater planning path that satisfies kinematic constraints and marine environmental constraints.

[0022] This step can achieve the following technical effects: A combined smoothing strategy of non-uniform B-spline interpolation and Gaussian filtering is adopted to eliminate inflection points and bottlenecks while maintaining the overall path direction, generating a smooth trajectory with continuous curvature. By verifying the smooth path throughout and confining it within the navigable space, the optimized path is ensured to always meet marine environmental safety constraints. The introduction of kinematic constraints also filters nodes located to the side or rear of the path nodes during the node selection stage, improving planning efficiency and avoiding unnecessary trajectory turns. The final output path is both safe and executable, and can be directly used for heading control and navigation command generation of underwater vehicles.

[0023] Preferably, in step S5, dangerous and impassable nodes are removed from all nodes in the operational sea area based on a preset navigation cost threshold to obtain feasible nodes. These feasible nodes form a locally continuous navigable space, specifically including: S51, statistically analyze the distribution of navigation cost among all nodes in the operational sea area, and set a lower and upper threshold for navigation cost. S52, mark nodes whose navigation cost value is below the lower threshold as dangerous nodes, and mark nodes whose navigation cost value is above the upper threshold as impassable nodes; S53, remove dangerous nodes and impassable nodes, and retain nodes whose navigation cost is within the range of the lower and upper thresholds as feasible nodes, and the feasible nodes form a locally continuous navigable space.

[0024] This step can achieve the following technical effects: By setting upper and lower limits for navigation costs, the safety classification and screening of nodes in the operational sea area can be achieved, eliminating dangerous and impassable nodes, and constructing a continuous navigable space from the remaining feasible nodes. This space, together with the final planned path, serves as the output, providing a safe physical boundary for the vehicle's activities, and also providing a reference for subsequent local replanning or human intervention.

[0025] Furthermore, this invention also provides an improved A* underwater vehicle path planning system that integrates the marine environment, comprising: The navigation cost modeling module contains: Marine environmental parameters of the operating area of ​​underwater vehicles are obtained, and the main influencing factors affecting the activities of underwater vehicles are selected as environmental factors. Based on the physical impact of each environmental factor on the activities of underwater vehicles, corresponding cost functions are designed. The Analytic Hierarchy Process (AHP) is used to assign weights to each environmental factor, construct a judgment matrix, and normalize it to obtain the weight coefficients of each environmental factor. The weight coefficients of each environmental factor are used as the coefficients of the corresponding cost functions and fused to form a navigation cost model. The A* cost function improvement module contains: The cost calculated by the navigation cost model and the Euclidean distance are used together as components of the path node evaluation cost; based on the navigation cost model and the path node evaluation cost, the cost function of the A* algorithm is improved. The path search module contains: An improved A* algorithm is used for node expansion and path search to obtain the initial planned path; The path smoothing optimization module contains: By combining the kinematic constraints of the underwater vehicle and the constraints of the marine environment, the initial planned path is optimized to obtain the final underwater planned path; The navigable space construction module contains: Dangerous and impassable nodes are removed from all nodes in the operational sea area based on a preset navigation cost threshold, resulting in feasible nodes. These feasible nodes form a locally continuous navigable space. The final underwater planned path and the navigable space together constitute the path planning output of the method.

[0026] The beneficial effects of this invention are as follows: To address the issue of insufficient consideration of marine environmental characteristics in existing A* algorithms, this invention employs the Analytic Hierarchy Process (AHP) to assign weights to multiple parameters, including hydrology, topography, seabed sediment, transparency, and acoustic propagation environment, thereby achieving unified quantification of multiple environmental constraints. A cost function is designed based on the physical influence mechanism of each parameter, and a navigation cost model is constructed using AHP weights as coefficients. This allows path search to incorporate navigation cost control while considering path length, achieving a balance between "safety priority + shortest path". To address the problem of poor path tracing due to the lack of a kinematic constraint system in existing methods, this invention introduces multi-dimensional kinematic constraints such as minimum turning radius, heading angle variation, path slope, and depth undulation. Combined with marine environmental constraints, non-uniform B-spline interpolation combined with Gaussian filtering is used to smooth the path, confining the smooth path entirely within the navigable space. This generates a final path with continuous curvature that meets the maneuverability requirements of the vehicle and environmental safety requirements, thereby improving the trajectory smoothness and the feasibility of actual navigation. Furthermore, this invention constructs navigable space by setting a navigation cost threshold, which narrows the node search range and improves planning efficiency; it also expands the safe navigation area by filtering candidate nodes through cost intervals, thus avoiding overly rigid paths.

[0027] Therefore, it is evident that the present invention has outstanding substantive features and significant progress compared with the prior art, and the beneficial effects of its implementation are also obvious. Attached Figure Description

[0028] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0029] Figure 1 This is a flowchart of an improved A* underwater vehicle path planning method that incorporates the marine environment, provided by the present invention. Figure 2 This is a schematic diagram of the path planning plane provided by the present invention.

[0030] Figure 3 The distribution diagram of extended nodes in the path planning provided by this invention. Detailed Implementation

[0031] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. The following embodiments are explanations of the present invention, but the present invention is not limited to the following implementation methods.

[0032] Example 1: like Figure 1 As shown in the figure, this embodiment provides an improved path planning method for A* underwater vehicles that incorporates the marine environment, including the following steps: Step S1: Obtain marine environmental parameters of the underwater vehicle's operating area, and select the main influencing factors affecting the underwater vehicle's activities as environmental factors; design corresponding cost functions based on the physical impact of each environmental factor on the underwater vehicle's activities, use the analytic hierarchy process (AHP) to assign weights to each environmental factor, construct a judgment matrix and normalize it to obtain the weight coefficients of each environmental factor; use the weight coefficients of each environmental factor as the coefficients of the corresponding cost functions, and fuse them to form a navigation cost model; Step S1 specifically includes: S11, Obtain marine environmental parameters of the sea area where the underwater vehicle is operating. The marine environmental parameters include: hydrological parameters, seabed topography parameters, seabed sediment parameters, ocean transparency parameters, and acoustic propagation characteristics parameters. Using the direct physical impact of various marine environmental parameters on the navigation safety and mission effectiveness of underwater vehicles as the criterion, the main influencing factors that physically affect the activities of underwater vehicles were selected as environmental factors, specifically including: Sound propagation characteristic parameters directly affect sonar detection range, underwater communication quality, and navigation stealth through sound velocity profile and propagation loss distribution. Their vertical structural changes also restrict the vehicle's ability to perceive the surrounding situation, so they were selected as sound propagation characteristic factors. Seabed topography parameters directly define the safety boundary for near-bottom navigation. The amplitude of topographic undulation and slope characteristics determine the probability of collision risk and affect the vehicle's attitude control accuracy and safe seabed clearance margin. Therefore, seabed topography factors were selected. Ocean transparency parameters directly determine the identification distance and detection probability of underwater vehicles by optical detection methods, especially affecting the exposure risk and tactical stealth of vehicles when navigating in shallow water or near the water surface. Therefore, it was selected as the seawater transparency factor. Hydrological parameters include the vertical distribution structure of ocean temperature, salinity, and depth. Although they have an objective impact on seawater density field and buoyancy drag characteristics, their effect on vehicle activities is indirect. Moreover, their acoustic effects are already coupled to the cost function of sound propagation characteristic factors through sound speed profiles, so they are not considered as environmental factors alone. Although seabed sediment parameters affect the characteristics of sound wave reflection and absorption, their impact on the risk to be detected can be coupled and characterized through the cost function of seabed topography factors, so they are not considered as environmental factors alone. The environmental factors include: sound propagation characteristics factor, seabed topography factor, and seawater transparency factor. S12, Design corresponding cost functions based on the physical impact of various environmental factors on the activities of underwater vehicles; the cost functions specifically include: the acoustic propagation loss and detection probability function corresponding to the acoustic propagation characteristic factors. P Collision risk cost function corresponding to seabed topography factors MThe optical detection concealment cost function corresponding to the seawater transparency factor ; The sound propagation loss and detection probability function are coupled, and the influence of the sound propagation loss distribution on navigation stealth is comprehensively characterized by the influence of depth changes on acoustic stealth under the action of sound propagation characteristics. The specific expression is as follows: , in, D The ocean depth at which the current path node is located. This represents the maximum ocean depth at the corresponding planar location of the current path node. P The probability of being detected by external acoustic detection equipment. P The larger the value, the more favorable the sound propagation conditions are at that location, the higher the risk of exposure for the detection vehicle, and the greater the corresponding acoustic value of the node. The collision risk cost function, coupled with the influence of seabed parameters on acoustic detection effectiveness, comprehensively characterizes the combined effect of topographic relief and seabed type on navigation safety and detection risk. The specific expression is as follows: , in, M Let the collision risk cost function be... D The ocean depth at which the current path node is located. i Slope grade type, N i For the first i The number of slope grades Specifically, the study area is divided into a square grid of 5×5 sampling points, and then... i The spatial distribution probability of a slope grade within its corresponding grid cell is calculated using the following expression: , in, N This represents the total number of slope grades. The optical detection concealment cost function characterizes the impact of water body optical properties on the exposure risk of underwater vehicles, and its specific expression is as follows: , in, The cost of optical detection stealth, i.e., apparent contrast. H SDD The transparency depth of this sea area, The diffuse reflectance at an infinite depth of water at the current transparency depth. D The ocean depth at which the current path node is located. D max This represents the maximum ocean depth at the current path node's corresponding planar location. S13. The AHP (Analytical Hierarchical Method) is used to assign weights to each environmental factor: Based on the experts' assessment of the relative importance of each environmental factor, the 1-9 scale method is used to compare each environmental factor pairwise and assign values. For example, if the experts believe that the sound propagation characteristics factor is equally important as the seabed topography factor, then the value is assigned to 1; if they believe that the sound propagation characteristics factor is more important than the seabed topography factor, then the value is assigned to 5. The judgment matrix is ​​completed when the judgment matrix is ​​filled. The judgment matrix A is an n-order square matrix, where n is the number of environmental factors. The expression for the judgment matrix is ​​as follows: , in, Let represent the importance scale value of the i-th environmental factor relative to the j-th environmental factor, and satisfy ... , ; Calculate the largest eigenvalue of judgment matrix A and its corresponding eigenvectors Each element in the feature vector corresponds to the initial weight of each environmental factor. The feature vector is normalized to obtain the weight coefficient of each environmental factor. S14: Using the weight coefficients as coefficients of the corresponding cost functions, the cost functions are weighted and fused to form a navigation cost model. The navigation cost model calculation value for a single path node is the navigation cost value of that node. The expression for the navigation cost model is: , in, The weighting coefficients for the sound propagation characteristic factors. These are the weighting coefficients for seabed topography factors. This represents the weighting coefficient for the seawater transparency factor.

[0033] This step can achieve the following technical effects: By collecting multi-dimensional marine environmental parameters such as hydrology, seabed topography, seabed sediment, ocean transparency, and acoustic propagation environment, comprehensive basic data is provided for subsequent navigation cost modeling. The Analytic Hierarchy Process (AHP) is used to assign weights to multiple marine environmental factors and normalize them, achieving the same quantification of multiple environmental constraints. By designing cost functions for acoustic propagation characteristics, seabed topography, and seawater transparency, the specific physical impact of each environmental parameter on underwater vehicles—including acoustic propagation loss and detection probability, collision risk, stealth, and sonar detection range—is quantified, giving the navigation cost model a clear physical meaning and avoiding the crude approach of simply treating environmental factors as obstacles.

[0034] In step S14, the weighting coefficients can be adjusted by a preset mathematical function after normalization.

[0035] This step can achieve the following technical effects: Based on the weight coefficients obtained by AHP normalization, further transformations are performed through preset mathematical functions. This allows for fine adjustment of the weight distribution while maintaining the relative order of each environmental factor, adapting to different sea area characteristics or mission preferences. This adjustment does not require reorganizing expert scoring, reducing the repetitive cost of weight calibration, enhancing the navigation cost model's ability to quickly adapt to different application scenarios, and improving the flexibility and engineering practicality of model parameter adjustment.

[0036] Step S2: The calculated value of the navigation cost model and the Euclidean distance are used together as components of the path node evaluation cost; based on the navigation cost model and the path node evaluation cost, the cost function of the A* algorithm is improved. Step S2 specifically includes: S21. Based on the longitude, latitude, and depth of the starting point of the path, and the longitude, latitude, and depth of the ending point of the path, construct a starting node and an ending node respectively; the starting node and the ending node each contain the following data fields: longitude, latitude, depth, cost incurred, estimated cost, and evaluation cost; For any node n: its cost g(n) is the sum of the travel costs from the starting node to the current node n along the planned route: k is the total number of nodes traversed from the starting node to the current node n; the estimated cost h(n) is the Euclidean distance from the current node n to the ending node. The evaluation cost f(n) mentioned therein is the sum of the cost incurred and the estimated cost: ; S22, construct an open table and a closed table for path planning; the open table is a priority pre-selected path node queue, storing nodes to be evaluated marked as "open", and all nodes in the table are arranged in ascending order of evaluation cost; the closed table is an unordered set, storing nodes that have been expanded and marked as "close". S23 uses the node evaluation cost f(n), which is composed of the travel cost and the Euclidean distance, as the cost function of the improved A* algorithm.

[0037] This step can achieve the following technical effects: By incorporating both the cost of navigation and Euclidean distance into the node evaluation cost, the A* algorithm achieves dual control over path node selection, considering both path distance and navigation cost. Expanding nodes are optimized based on comprehensive cost, achieving a balance between safety priority and shortest path. The division of labor between open and closed tables ensures the orderliness and efficiency of the path search process. The node data structure includes three-dimensional information of longitude, latitude, and depth, making it suitable for underwater three-dimensional spatial path planning.

[0038] Step S3: The improved A* algorithm is used to expand nodes and search paths to obtain the initial planned path; Step S3 specifically includes: S31, mark the starting node as "open" and calculate its evaluation cost, and also mark the adjacent nodes around the starting node as "open"; at this time, the path has not been planned, and the evaluation cost at the starting point is only the estimated cost to reach the surrounding nodes. S32, select new nodes within the navigable space that are not marked as "open" or "close" as nodes to be expanded, and mark them as "close". Considering that the core objective of the planned path is forward movement, the allowed expansion nodes are located in the path forward of the current node, such as... Figure 2 As shown; S33, Perform dynamic update processing on the node: Traverse all neighboring nodes of the currently processed node. If a node has been marked as "open", calculate the evaluation cost of the node and select the node with the smallest evaluation cost from all "open" nodes to mark it as "close". If a node has been marked as "close", remark the node as "open" and calculate the evaluation cost of the node for re-evaluation. S34, when processing the endpoint node, stop the calculation, arrange all nodes marked as "close" in descending order of evaluation cost to obtain the optimal path node sequence, such as... Figure 3 As shown, the black squares represent underwater obstacles, and the nodes pointed to by the solid black arrows are path nodes generated after path planning using the improved A* algorithm.

[0039] This step can achieve the following technical effects: By marking and dynamically updating the "open" and "close" states of nodes, the efficiency and global optimality of path search are ensured; nodes marked as "close" are allowed to be remarked as "open" and participate in re-evaluation, avoiding the omission of the globally optimal path due to local optimal selection; all "close" nodes are backtracked and sorted to ensure that the generated path node sequence is optimal in the sense of comprehensive cost.

[0040] Step S3 further includes: According to a preset scaling ratio, the navigation cost value of each path node in the optimal path node sequence is scaled to construct a navigation cost value range for each path node; taking the optimal path node sequence as the path axis, non-path nodes perpendicular to the path axis and located within the navigable space are obtained; the navigation cost value of the non-path nodes is compared with the navigation cost value range, and non-path nodes that meet the range limit are used as navigable candidate nodes to expand the safe navigation area around the optimal path. The optimal path node sequence is used as the initial planned path.

[0041] This step can achieve the following technical effects: By scaling the navigation cost of each node in the optimal path node sequence to construct a numerical range, alternative nodes that meet the safety conditions are selected around the optimal path, reserving adjustment space for subsequent smoothing under kinematic constraints, and avoiding the single optimal path from becoming too rigid and causing out-of-bounds after smoothing.

[0042] Step S4: Combining the kinematic constraints of the underwater vehicle and the constraints of the marine environment, the initial planned path is optimized to obtain the final underwater planned path; The underwater vehicle kinematic constraints and marine environment constraints in step S4 specifically include: The kinematic constraints include: minimum turning radius constraint, used to limit the path curvature to not exceed the upper limit of the vehicle's maneuverability; heading angle variation constraint, used to limit the amount of heading angle variation between adjacent path segments to within the allowable range; path slope range constraint, used to limit the longitudinal slope of the path to not exceed the vehicle's maximum climb angle and maximum dive angle; and depth undulation constraint, used to limit the rate of change of depth in the vertical direction of the path to remain gentle. The marine environmental constraint is that the smoothed path must remain entirely within navigable space.

[0043] This step can achieve the following technical effects: By introducing multi-dimensional kinematic constraints such as minimum turning radius, heading angle change, path slope, and depth undulation, the planned path is ensured to conform to the actual maneuverability of the underwater vehicle, thereby improving the trackability and feasibility of the path. The marine environment constraints confine the entire path within the navigable space, ensuring that the optimized path always meets environmental safety requirements.

[0044] Step S4 specifically includes: S41 sets the kinematic constraints and marine environmental constraints for underwater vehicles; S42, smooth the optimal path node sequence in the initial planned path; the smoothing process uses non-uniform B-spline interpolation to fit the optimal path node sequence and generate a smooth path curve; During the smoothing process, the kinematic constraints are used as constraints to control the adjustment of the node positions, so that the generated smooth path curve meets the maneuverability requirements of the underwater vehicle. S43 uses Gaussian filtering to filter the smooth path curve, eliminating local jitter and sharp corners in the path, and eliminating inflection points and stuttering points while keeping the overall direction of the path unchanged. S44, Based on the marine environment constraints, the path after smoothing and filtering is checked point by point: it is determined whether each node on the path meets the set kinematic constraints and marine environment constraints, and the points that meet the conditions are used as the constituent nodes of the final underwater planning path. S45 generates the final underwater planning path that satisfies kinematic constraints and marine environmental constraints.

[0045] This step can achieve the following technical effects: A combined smoothing strategy of non-uniform B-spline interpolation and Gaussian filtering is adopted to eliminate inflection points and bottlenecks while maintaining the overall path direction, generating a smooth trajectory with continuous curvature. By verifying the smooth path throughout and confining it within the navigable space, the optimized path is ensured to always meet marine environmental safety constraints. The introduction of kinematic constraints also filters nodes located to the side or rear of the path nodes during the node selection stage, improving planning efficiency and avoiding unnecessary trajectory turns. The final output path is both safe and executable, and can be directly used for heading control and navigation command generation of underwater vehicles.

[0046] Step S5: Based on a preset navigation cost threshold, dangerous and impassable nodes are removed from all nodes in the operational sea area to obtain feasible nodes. These feasible nodes form a locally continuous navigable space. The final underwater planned path and the navigable space together constitute the path planning output of the method. In step S5, dangerous and impassable nodes are removed from all nodes in the operational sea area based on a preset navigation cost threshold, resulting in feasible nodes. These feasible nodes form a locally continuous navigable space, specifically including: S51, statistically analyze the distribution of navigation cost among all nodes in the operational sea area, and set a lower and upper threshold for navigation cost; in this embodiment, the lower threshold is taken as the 5th percentile, and the upper threshold is taken as the 90th percentile; the lower and upper thresholds can be manually adjusted according to the vehicle's maneuverability, mission safety level, and sea area environmental characteristics. S52, mark nodes whose navigation cost value is below the lower threshold as dangerous nodes, and mark nodes whose navigation cost value is above the upper threshold as impassable nodes; S53, remove dangerous nodes and impassable nodes, and retain nodes whose navigation cost is within the range of the lower and upper thresholds as feasible nodes, and the feasible nodes form a locally continuous navigable space.

[0047] This step can achieve the following technical effects: By setting upper and lower limits for navigation costs, the safety classification and screening of nodes in the operational sea area can be achieved, eliminating dangerous and impassable nodes, and constructing a continuous navigable space from the remaining feasible nodes. This space, together with the final planned path, serves as the output, providing a safe physical boundary for the vehicle's activities, and also providing a reference for subsequent local replanning or human intervention.

[0048] Example 2: This embodiment provides an improved A* underwater vehicle path planning system that integrates the marine environment, including: The navigation cost modeling module contains: Marine environmental parameters of the operating area of ​​underwater vehicles are obtained, and the main influencing factors affecting the activities of underwater vehicles are selected as environmental factors. Based on the physical impact of each environmental factor on the activities of underwater vehicles, corresponding cost functions are designed. The Analytic Hierarchy Process (AHP) is used to assign weights to each environmental factor, construct a judgment matrix, and normalize it to obtain the weight coefficients of each environmental factor. The weight coefficients of each environmental factor are used as the coefficients of the corresponding cost functions and fused to form a navigation cost model. The A* cost function improvement module contains: The cost calculated by the navigation cost model and the Euclidean distance are used together as components of the path node evaluation cost; based on the navigation cost model and the path node evaluation cost, the cost function of the A* algorithm is improved. The path search module contains: An improved A* algorithm is used for node expansion and path search to obtain the initial planned path; The path smoothing optimization module contains: By combining the kinematic constraints of the underwater vehicle and the constraints of the marine environment, the initial planned path is optimized to obtain the final underwater planned path; The navigable space construction module contains: Dangerous and impassable nodes are removed from all nodes in the operational sea area based on a preset navigation cost threshold, resulting in feasible nodes. These feasible nodes form a locally continuous navigable space. The final underwater planned path and the navigable space together constitute the path planning output of the method.

[0049] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. The methods disclosed in the embodiments are described simply because they correspond to the systems disclosed in the embodiments; relevant details can be found in the method section.

[0050] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0051] In the embodiments provided by this invention, it should be understood that the disclosed systems, methods, and approaches can be implemented in other ways. For example, the system embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between systems or units may be electrical, mechanical, or other forms.

[0052] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0053] In addition, the functional modules in the various embodiments of the present invention can be integrated into one processing unit, or each module can exist physically separately, or two or more modules can be integrated into one unit.

[0054] Similarly, in the various embodiments of the present invention, each processing unit can be integrated into a functional module, or each processing unit can exist physically, or two or more processing units can be integrated into a functional module.

[0055] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented directly by hardware, a software module executed by a processor, or a combination of both. The software module can be located in random access memory (RAM), main memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art.

[0056] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0057] The above-disclosed embodiments are merely preferred embodiments of the present invention, but the present invention is not limited thereto. Any non-creative variations that can be conceived by those skilled in the art, as well as any improvements and modifications made without departing from the principles of the present invention, should fall within the protection scope of the present invention.

Claims

1. An improved A* underwater vehicle path planning method that fuses the ocean environment, characterized by, Includes the following steps: Step S1: Obtain marine environmental parameters of the sea area where the underwater vehicle is operating, and screen the main influencing factors affecting the activities of the underwater vehicle as environmental factors. Based on the physical impact of various environmental factors on the activities of underwater vehicles, corresponding cost functions are designed. The Analytic Hierarchy Process (AHP) is used to assign weights to each environmental factor, construct a judgment matrix, and normalize it to obtain the weight coefficients of each environmental factor. The weight coefficients of each environmental factor are used as the coefficients of the corresponding cost functions and fused to form a navigation cost model. Step S2: The calculated value of the navigation cost model and the Euclidean distance are used together as components of the path node evaluation cost; based on the navigation cost model and the path node evaluation cost, the cost function of the A* algorithm is improved. Step S3: The improved A* algorithm is used to expand nodes and search paths to obtain the initial planned path; Step S4: Combining the kinematic constraints of the underwater vehicle and the constraints of the marine environment, the initial planned path is optimized to obtain the final underwater planned path; Step S5: Based on a preset navigation cost threshold, dangerous and impassable nodes are removed from all nodes in the operational sea area to obtain feasible nodes. These feasible nodes form a locally continuous navigable space. The final underwater planned path and the navigable space together constitute the path planning output of the method.

2. The improved path planning method for A* underwater vehicles integrating marine environment as described in claim 1, characterized in that, Step S1 specifically includes: S11, Obtain marine environmental parameters of the sea area where the underwater vehicle is operating. The marine environmental parameters include: hydrological parameters, seabed topography parameters, seabed sediment parameters, ocean transparency parameters, and acoustic propagation characteristics parameters. Based on the criteria of the direct physical impact of various marine environmental parameters on the navigation safety and mission effectiveness of underwater vehicles, the main influencing factors that physically affect the activities of underwater vehicles are selected as environmental factors, including: sound propagation characteristics factor, seabed topography factor, and seawater transparency factor. S12, Design the corresponding cost function based on the physical impact of each environmental factor on the activities of underwater vehicles; S13, use the AHP (Analog-Hybrid Analysis) method to assign weights to each environmental factor: based on the experts' assessment of the relative importance of each environmental factor, use the 1-9 scale method to compare each environmental factor pairwise and assign values, fill the corresponding positions in the judgment matrix with the assigned values, and the judgment matrix is ​​completed when it is filled. Calculate the largest eigenvalue of the judgment matrix and its corresponding eigenvector. Each element in the eigenvector corresponds to the initial weight of each environmental factor. Normalize the eigenvector to obtain the weight coefficient of each environmental factor. S14: The weight coefficients are used as the coefficients of the corresponding cost functions. The cost functions are weighted and fused to form a navigation cost model. The navigation cost model calculation value of a single path node is the navigation cost value of that node.

3. The improved A* underwater vehicle path planning method integrating marine environment as described in claim 2, characterized in that, The cost functions in step S12 include: the sound propagation loss and detection probability function corresponding to the sound propagation characteristic factor, the collision risk cost function corresponding to the seabed topography factor, and the optical detection concealment cost function corresponding to the seawater transparency factor. The specific expression for the sound propagation loss and detection probability function is as follows: ， in, D The ocean depth at which the current path node is located. This represents the maximum ocean depth at the corresponding planar location of the current path node. P The probability of being detected by external acoustic detection equipment; The collision risk cost function is specifically expressed as follows: ， in, M Let the collision risk cost function be... D The ocean depth at which the current path node is located. i Slope grade type, N i For the first i The number of slope grades To rasterize the sampling points in the study area according to their planar location, the first... i The spatial distribution probability of a slope grade within its corresponding grid cell is calculated using the following expression: ， in, N This represents the total number of slope grades. The optical detection concealment cost function corresponding to the seawater transparency factor is specifically expressed as follows: , in, The cost of covert optical detection. H SDD The transparency depth of this sea area, The diffuse reflectance at an infinite depth of water at the current transparency depth. D The ocean depth at which the current path node is located. D max This represents the maximum ocean depth at the current path node's corresponding planar location.

4. The improved path planning method for A* underwater vehicles integrating marine environment as described in claim 2, characterized in that, In step S14, the weighting coefficients can be adjusted by a preset mathematical function after normalization.

5. The improved path planning method for A* underwater vehicles integrating marine environment as described in claim 1, characterized in that, Step S2 specifically includes: S21. Based on the longitude, latitude, and depth of the starting point of the path, and the longitude, latitude, and depth of the ending point of the path, construct a starting node and an ending node respectively; each starting node and ending node contains the following data fields: longitude, latitude, depth, cost incurred, estimated cost, and evaluation cost; wherein, the cost incurred is the sum of the travel costs from the starting node to the current node of the planned route, the estimated cost is the Euclidean distance from the current node to the ending node, and the evaluation cost is the sum of the cost incurred and the estimated cost; S22, construct an open table and a closed table for path planning; the open table is a priority pre-selected path node queue, storing nodes to be evaluated marked as "open", and all nodes in the table are arranged in ascending order of evaluation cost; the closed table is an unordered set, storing nodes that have been expanded and marked as "close". S23 uses the node evaluation cost, which consists of the travel cost and the Euclidean distance, as the cost function of the improved A* algorithm.

6. The improved A* underwater vehicle path planning method integrating marine environment as described in claim 1, characterized in that, Step S3 specifically includes: S31, mark the starting node as "open" and calculate its evaluation cost, and also mark the adjacent nodes around the starting node as "open". S32, Select new nodes in the navigable space that are not marked as "open" or "close" as nodes to be expanded, and mark them as "close"; S33, Perform dynamic update processing on the node: Traverse all neighboring nodes of the currently processed node. If a node has been marked as "open", calculate the evaluation cost of the node and select the node with the smallest evaluation cost among all "open" nodes and mark it as "close". If a node has been marked as "close", remark the node as "open" and calculate the evaluation cost of the node. S34. When the end node is reached, the calculation stops. All nodes marked as "close" are arranged in descending order of evaluation cost to obtain the optimal path node sequence.

7. The improved A* underwater vehicle path planning method integrating marine environment as described in claim 6, characterized in that, Step S3 further includes: According to a preset scaling ratio, the navigation cost value of each path node in the optimal path node sequence is scaled to construct a navigation cost value range for each path node; using the optimal path node sequence as the path axis, non-path nodes that are perpendicular to the path axis and located within the navigable space are obtained; the navigation cost value of the non-path nodes is compared with the navigation cost value range, and non-path nodes that meet the range limit are selected as navigable candidate nodes. The optimal path node sequence and the navigable alternative nodes together form the initial planned path.

8. The improved A* underwater vehicle path planning method integrating marine environment as described in claim 1, characterized in that, The underwater vehicle kinematic constraints and marine environment constraints in step S4 specifically include: The kinematic constraints include: minimum turning radius constraint, which limits the path curvature to no more than the upper limit of the vehicle's maneuverability; heading angle variation constraint, which limits the change in heading angle between adjacent path segments to within the allowable range; path slope range constraint, which limits the longitudinal slope of the path to no more than the vehicle's maximum climb angle and maximum dive angle; and depth undulation constraint, which limits the rate of change of depth in the vertical direction of the path to remain gentle. The marine environmental constraint is that the smoothed path must remain entirely within navigable space.

9. The improved path planning method for A* underwater vehicles integrating marine environment as described in claim 1, characterized in that, Step S4 specifically includes: S41 sets the kinematic constraints and marine environmental constraints for underwater vehicles; S42, smooth the optimal path node sequence in the initial planned path; the smoothing process uses non-uniform B-spline interpolation to fit the optimal path node sequence and generate a smooth path curve; During the smoothing process, the kinematic constraints are used as constraints to control the adjustment of node positions; S43 uses Gaussian filtering to filter the smooth path curve, eliminating local jitter and sharp corners in the path, and eliminating inflection points and stuttering points while keeping the overall direction of the path unchanged. S44, Based on the marine environment constraints, the path after smoothing and filtering is checked point by point: it is determined whether each node on the path meets the set kinematic constraints and marine environment constraints, and the points that meet the conditions are used as the constituent nodes of the final underwater planning path. S45 generates the final underwater planning path that satisfies kinematic constraints and marine environmental constraints.

10. The improved A* underwater vehicle path planning method integrating marine environment as described in claim 1, characterized in that, In step S5, dangerous and impassable nodes are removed from all nodes in the operational sea area based on a preset navigation cost threshold, resulting in feasible nodes. These feasible nodes form a locally continuous navigable space, specifically including: S51, statistically analyze the distribution of navigation cost among all nodes in the operational sea area, and set a lower and upper threshold for navigation cost. S52, mark nodes whose navigation cost value is below the lower threshold as dangerous nodes, and mark nodes whose navigation cost value is above the upper threshold as impassable nodes; S53, remove dangerous nodes and impassable nodes, and retain nodes whose navigation cost is within the range of the lower and upper thresholds as feasible nodes, and the feasible nodes form a locally continuous navigable space.

Images