An underwater robot path planning method considering ocean current influence
By fusing grid maps and octree maps and improving the A* algorithm, the problem of ocean current influence in underwater robot path planning was solved, achieving efficient and real-time path optimization and reducing energy consumption.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- HOHAI UNIV
- Filing Date
- 2023-06-16
- Publication Date
- 2026-07-24
AI Technical Summary
Existing technologies fail to effectively consider the influence of ocean currents in underwater robot path planning, leading to increased energy consumption and unreasonable path planning.
We employ a fusion modeling approach combining raster and octree maps to extend the traditional A* algorithm to 3D environments. We also design a cost function that incorporates ocean current influences and optimize the path planning algorithm to reduce energy consumption.
It achieves efficient path planning considering the influence of ocean currents, reduces the energy consumption of underwater robots, and improves the real-time performance and search efficiency of path planning.
Smart Images
Figure CN116734856B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of path planning, and more particularly to a path planning method for underwater robots that takes into account the influence of ocean currents. Background Technology
[0002] The underwater marine environment differs from other three-dimensional environments, exhibiting significant interference from ocean currents, making it far more complex. Therefore, AUV route planning requires not only consideration of the sailing distance but also adjustments to the route based on the prevalent ocean currents in the underwater environment to achieve energy conservation or hazard avoidance. Summary of the Invention
[0003] To address the aforementioned technical problems, this invention proposes a path planning method for underwater robots that considers the influence of ocean currents. This method is characterized by low complexity, high computational efficiency, and ease of implementation. To achieve the above objectives, this invention employs the following technical solution:
[0004] This invention discloses a path planning method for underwater robots that takes into account the influence of ocean currents, comprising the following steps:
[0005] S1: Integrate and model the raster map and octree map to construct terrain obstacles, integrate ocean current semantic map, add ocean current semantics, and implement map manager to model and manage ocean terrain obstacles and ocean current interference;
[0006] S2: Expand the perception range of the traditional A* algorithm to realize the three-dimensional search range of the traditional A* algorithm's single point, and extend the application scenarios of the traditional A* algorithm from two-dimensional environment to three-dimensional environment;
[0007] S3: Downsampling is performed to address the information-dense problem of 3D environment models, thereby improving the running efficiency of the A* algorithm in 3D environments;
[0008] S4: Establish a mathematical model of the relationship between ocean current direction, ocean current velocity and underwater robot navigation path direction, and quantify the additional energy consumption caused by ocean currents during underwater robot navigation.
[0009] S5: Design a cost function, improve the heuristic function of the traditional A* algorithm, improve the optimal direction selection of the traditional A* algorithm, and integrate the energy consumption factor and distance factor under the influence of ocean currents to plan the path with the optimal comprehensive cost.
[0010] Preferably, step S1 specifically includes:
[0011] S11: Change the traditional method of using grid points as neighbors to using equal distances as neighbors;
[0012] S12: A terrain generation function based on grid size is used to design and simulate terrain obstacles to generate different terrains; the terrain generation function based on grid size is shown in equation (1):
[0013]
[0014] Where X and Y are the rasterized X and Y data axes, respectively; Z is the vertical coordinate of the constructed terrain obstacle, which, together with the X and Y data axes, constitutes the complete terrain obstacle. i To construct auxiliary parameters for the height of the current node of the terrain obstacle, x i To construct auxiliary parameters for the top x-coordinate position of the current node of the terrain obstacle, y i To construct auxiliary parameters for the top ordinate position of the current node of the terrain obstacle, xs i To construct auxiliary parameters for the bottom x-coordinate position of the current node of the terrain obstacle, ys i This is an auxiliary parameter for constructing the bottom ordinate position of the current node when creating terrain obstacles.
[0015] S13: Adopts traditional raster map background, integrates octree map data processing methods, optimizes model data structure, integrates ocean current semantic map, adds ocean current semantics, improves ocean model construction, and fits the actual environment.
[0016] Preferably, step S4 specifically includes:
[0017] S41: Considering the characteristics of ocean currents, the relationship between the navigation direction and the ocean current direction vector at a certain point in the underwater robot's planned path is taken into account. The influence of ocean currents on the entire path is discretized into vector relationships at each point. A decomposition and refinement method is adopted to decompose and refine the energy consumption calculation of the entire path into the energy consumption calculation of individual nodes. Specifically, the angle between the navigation direction and the ocean current direction at a certain point in the planned path is used as an indicator of energy consumption or savings, and the magnitude of the ocean current vector is used as an indicator of the degree of energy consumption or savings. This realizes the mapping of the degree of energy consumption to the relationship between the angle between 0° and 180° and the vector magnitude.
[0018] S42: Construct a mapping function to numerically map the influence of ocean currents and the sailing angle, mapping the linear angle to the [0,1] interval. The mapping function is shown in Equation (2):
[0019]
[0020] Where θ is the angle between the sailing direction and the ocean current direction at a certain point on the planned path, and ang represents the specific value mapped by the angle, which ranges from 0 to 1.
[0021] The larger the ang value, the more significant the obstruction effect of the ocean current per unit speed on the current direction of travel;
[0022] S43: Decompose and refine the ocean current size and direction, consider the ocean current size at the current navigation path point and the angle between the ocean current and the navigation direction, and design the energy consumption cost function under the influence of the ocean current at a certain navigation path point as shown in equation (3):
[0023]
[0024] Where i represents the ocean current at the current node, |V i | represents the current ocean current velocity at the current node, θ i oc represents the angle between the current ocean current and the direction of travel. i The value represents the additional energy consumption caused by the ocean current at the i-th waypoint, oc i The larger the value, the greater the additional energy consumption at that waypoint.
[0025] The energy cost accumulated by the ocean currents along this path is shown in equation (4):
[0026]
[0027] Wherein, the value of oc represents the additional energy consumption caused by ocean currents accumulated along this path;
[0028] The larger the oc value, the greater the additional energy consumption of this path.
[0029] Preferably, step S5 specifically includes:
[0030] S51: The distance factor in the underwater robot's movement process is quantified using the Euclidean distance formula, as shown in equation (5):
[0031]
[0032] Where S(x) s ,y s ,z s ) and E(x e ,y e ,z e Let S and E be two points in three-dimensional space, and let E be the Euclidean distance d between them.
[0033] If the value of d is larger, it means that the distance from the current target node to the previous node is greater; if the value of d is smaller, it means that the distance from the current target node to the previous target node is closer; the direction with the smaller value of d is the optimal search direction of the algorithm; S52: Introduce the energy consumption cost function designed in S43, and calculate the energy consumption cost under the influence of ocean current at a certain node, as shown in equation (6):
[0034]
[0035] Where i represents the ocean current at the current node, |V i | represents the current ocean current velocity at the current node, θ i oc represents the angle between the current ocean current and the direction of travel. i The value represents the additional energy consumption caused by the ocean current at the i-th navigation path point;
[0036] If oc i The larger the value, the stronger the ocean current that the current target node needs to resist, and the more energy is required to resist the ocean current; if oc i The smaller the value, the weaker the ocean current intensity that the current target node needs to resist, and the less energy is required to resist the ocean current; in terms of oc i The direction with the smallest value is taken as the optimal search direction for the algorithm.
[0037] S53: By combining distance factors with energy consumption costs, a comprehensive cost function is constructed, improving the heuristic function of the traditional A* algorithm. Priority is given to searching for collision-free paths with shorter distances and less ocean current influence. The comprehensive cost function formula that comprehensively considers path distance and energy consumption costs is shown in Equation (7):
[0038]
[0039] Where S(x) s ,y s ,z s ) and E(x e ,y e ,z e Let and represent two points in three-dimensional space, where i represents the ocean current at the current node, and |V i | represents the current ocean current velocity at the current node, θ i This indicates the angle between the current ocean current and the direction of travel.
[0040] The comprehensive cost from the current node S to the next node E is calculated using equation (7). By comparing the cost function values of different nodes, the node with the smaller value is selected as the next moving node. E The path with the smallest numerical value is the optimal path.
[0041] S54: Use Bezier smoothing to smooth the optimal path planned in S52 to obtain an optimal path with smooth corners.
[0042] Compared with existing technologies, the advantages of this invention are as follows: The underwater robot path planning method disclosed in this invention, which considers the influence of ocean currents, does not require an increase in the computing power of the underwater robot hardware, thus preventing the problem of insufficient hardware computing power caused by excessive method complexity. In terms of algorithm, the algorithm has high path search efficiency, fast path planning speed, and strong real-time performance, making it more suitable for the characteristics of the underwater robot's working environment.
[0043] Furthermore, most current path planning methods, when used in underwater environments, do not consider the influence of ocean currents on the underwater robot's path travel. They still use the optimal distance between the starting point and the target point as the path search direction, causing the underwater robot to consume a large amount of energy to resist ocean current interference when traveling along this optimal path. The underwater robot path planning method disclosed in this invention, which considers the influence of ocean currents, designs a cost function that takes into account the influence of ocean currents, which can improve the optimal path search direction of the algorithm. Attached Figure Description
[0044] Figure 1 This is a schematic diagram of the underwater robot path planning process that takes into account the influence of ocean currents in this invention;
[0045] Figure 2 This is a schematic diagram of the simulated marine environment generated by the present invention;
[0046] Figure 3 This is a schematic diagram of the path planned by the present invention in a simulated marine environment, taking into account the influence of ocean currents. Detailed Implementation
[0047] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0048] like Figure 1 As shown, this invention discloses a path planning method for underwater robots that takes into account the influence of ocean currents, comprising the following steps:
[0049] S1: Integrate and model the raster map and octree map to construct terrain obstacles, integrate ocean current semantic map, add ocean current semantics, and implement map manager to model and manage ocean terrain obstacles and ocean current interference;
[0050] S2: Expand the perception range of the traditional A* algorithm to realize the three-dimensional search range of the traditional A* algorithm's single point, and extend the application scenarios of the traditional A* algorithm from two-dimensional environment to three-dimensional environment;
[0051] S3: Downsampling is performed to address the information-dense problem of 3D environment models, thereby improving the efficiency of the A* algorithm in 3D environments; S4: A mathematical model is established to describe the relationship between ocean current direction, ocean current velocity, and underwater robot navigation path direction, quantifying the additional energy consumption caused by ocean currents during underwater robot navigation.
[0052] S5: Design a cost function, improve the heuristic function of the traditional A* algorithm, improve the optimal direction selection of the traditional A* algorithm, and integrate the energy consumption factor and distance factor under the influence of ocean currents to plan the path with the optimal comprehensive cost.
[0053] Preferably, step S1 specifically includes:
[0054] S11: Change the traditional method of using grid points as neighbors to using equal distances as neighbors;
[0055] S12: A terrain generation function based on grid size is used to design and simulate terrain obstacles to generate different terrains; the terrain generation function based on grid size is shown in equation (1):
[0056]
[0057] Where X and Y are the rasterized X and Y data axes, respectively; Z is the vertical coordinate of the constructed terrain obstacle, which, together with the X and Y data axes, constitutes the complete terrain obstacle. i To construct auxiliary parameters for the height of the current node of the terrain obstacle, x i To construct auxiliary parameters for the top x-coordinate position of the current node of the terrain obstacle, y i To construct auxiliary parameters for the top ordinate position of the current node of the terrain obstacle, xs i To construct auxiliary parameters for the bottom x-coordinate position of the current node of the terrain obstacle, ys i This is used to construct auxiliary parameters for the bottom ordinate position of the current node in the terrain obstacle model. The constructed simulated marine environment model is as follows: Figure 2 As shown.
[0058] S13: Adopts traditional raster map background, integrates octree map data processing methods, optimizes model data structure, integrates ocean current semantic map, adds ocean current semantics, improves ocean model construction, and fits the actual environment.
[0059] Preferably, step S4 specifically includes:
[0060] S41: Discretize the ocean current influence along the entire path into vector relationships at each point. Use a decomposition and refinement method to decompose and refine the energy consumption calculation of the entire path into the energy consumption calculation of a single node. Specifically, the angle between the navigation direction and the ocean current direction at a certain point of the planned path is used as an indicator of energy consumption or savings, and the magnitude of the ocean current vector is used as an indicator of the degree of energy consumption or savings. This realizes the mapping of the degree of energy consumption to the relationship between the angle between 0° and 180° and the vector magnitude.
[0061] S42: Construct a mapping function to numerically map the influence of ocean currents and the sailing angle, mapping the linear angle to the [0,1] interval. The mapping function is shown in Equation (2):
[0062]
[0063] Where θ is the angle between the sailing direction and the ocean current direction at a certain point on the planned path, and ang represents the specific value mapped by the angle, which ranges from 0 to 1.
[0064] The larger the ang value, the more significant the obstruction effect of the ocean current per unit speed on the current direction of travel;
[0065] S43: Decompose and refine the ocean current size and direction, consider the ocean current size at the current navigation path point and the angle between the ocean current and the navigation direction, and design the energy consumption cost function under the influence of the ocean current at a certain navigation path point as shown in equation (3):
[0066]
[0067] Where i represents the ocean current at the current node, |V i | represents the current ocean current velocity at the current node, θ i oc represents the angle between the current ocean current and the direction of travel. i The value represents the additional energy consumption caused by the ocean current at the i-th waypoint, oc i The larger the value, the greater the additional energy consumption at that waypoint.
[0068] The energy cost accumulated by the ocean currents along this path is shown in equation (4):
[0069]
[0070] Wherein, the value of oc represents the additional energy consumption caused by ocean currents accumulated along this path;
[0071] The larger the oc value, the greater the additional energy consumption of this path.
[0072] Preferably, step S5 specifically includes:
[0073] S51: The distance factor in the underwater robot's movement process is quantified using the Euclidean distance formula, as shown in equation (5):
[0074]
[0075] Where S(x) s ,y s ,z s ) and E(x e ,y e ,z e Let S and E be two points in three-dimensional space, and let d be the Euclidean distance between them.
[0076] If the value of d is larger, it means that the distance from the current target node to the previous node is greater; if the value of d is smaller, it means that the distance from the current target node to the previous target node is closer; the direction with the smaller value of d is the optimal search direction of the algorithm; S52: Introduce the energy consumption cost function designed in S43, and calculate the energy consumption cost under the influence of ocean current at a certain node, as shown in equation (6):
[0077]
[0078] Where i represents the ocean current at the current node, |V i | represents the current ocean current velocity at the current node, θ i oc represents the angle between the current ocean current and the direction of travel. i The value represents the additional energy consumption caused by the ocean current at the i-th navigation path point;
[0079] If oc i The larger the value, the stronger the ocean current that the current target node needs to resist, and the more energy is required to resist the ocean current; if oc i The smaller the value, the weaker the ocean current intensity that the current target node needs to resist, and the less energy is required to resist the ocean current; in terms of oc i The direction with the smallest value is taken as the optimal search direction for the algorithm.
[0080] S53: By combining distance factors with energy consumption costs, a comprehensive cost function is constructed, improving the heuristic function of the traditional A* algorithm. Priority is given to searching for collision-free paths with shorter distances and less ocean current influence. The comprehensive cost function formula that comprehensively considers path distance and energy consumption costs is shown in Equation (7):
[0081]
[0082] Where S(x) s ,y s ,z s ) and E(x e ,ye ,z e Let and represent two points in three-dimensional space, where i represents the ocean current at the current node, and |V i | represents the current ocean current velocity at the current node, θ i This indicates the angle between the current ocean current and the direction of travel.
[0083] The comprehensive cost from the current node S to the next node E is calculated using equation (7). By comparing the cost function values of different nodes, the node with the smaller value is selected as the next moving node. E The path with the smallest numerical value is the optimal path.
[0084] S54: Using Bezier smoothing, the optimal path planned in S52 is smoothed to obtain an optimal path with smooth corners, such as... Figure 3 As shown.
[0085] The above description, in conjunction with specific preferred embodiments, provides a further detailed explanation of the present invention. It should not be construed that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, several equivalent substitutions or obvious modifications can be made without departing from the concept of the present invention, and all such modifications, achieving the same performance or purpose, should be considered within the scope of protection of the present invention.
Claims
1. A path planning method for underwater robots considering the influence of ocean currents, characterized in that... Includes the following steps: S1: Integrate and model the raster map and octree map to construct terrain obstacles, integrate ocean current semantic map, add ocean current semantics, and implement map manager to model and manage ocean terrain obstacles and ocean current interference; S2: Expand the perception range of the traditional A* algorithm to realize the three-dimensional search range of the traditional A* algorithm's single point, and extend the application scenarios of the traditional A* algorithm from two-dimensional environment to three-dimensional environment; S3: Downsampling is performed to address the information-dense problem of 3D environment models, thereby improving the running efficiency of the A* algorithm in 3D environments; S4: Establish a mathematical model of the relationship between ocean current direction, ocean current velocity and underwater robot navigation path direction, and quantify the additional energy consumption caused by ocean currents during underwater robot navigation. S41: Discretize the ocean current influence along the entire path into vector relationships at each point. Use a decomposition and refinement method to decompose and refine the energy consumption calculation of the entire path into the energy consumption calculation of a single node. Specifically, the angle between the navigation direction and the ocean current direction at a certain point of the planned path is used as an indicator of energy consumption or savings, and the magnitude of the ocean current vector is used as an indicator of the degree of energy consumption or savings. This realizes the mapping of the degree of energy consumption to the relationship between the angle between 0° and 180° and the vector magnitude. S42: Construct a mapping function to numerically map the influence of ocean currents and navigation angles, mapping linear angles to... The interval and the mapping function are shown in equation (2): (2) in, ang represents the angle between the direction of navigation and the direction of ocean current at a certain point on the planned path, and the specific value obtained by mapping the angle is in the range of 0 to 1. The larger the ang value, the more significant the obstruction effect of the ocean current per unit speed on the current direction of travel; S43: Decompose and refine the ocean current size and direction, consider the ocean current size at the current navigation path point and the angle between the ocean current and the navigation direction, and design the energy consumption cost function under the influence of the ocean current at a certain navigation path point as shown in equation (3): （3） Where i represents the ocean current at the current node, This indicates the current ocean current velocity at the current node. This indicates the angle between the current ocean current and the direction of travel. The value represents the additional energy consumption caused by the ocean current at the i-th navigation path point. The larger the value, the greater the additional energy consumption at that waypoint. The energy cost accumulated by the ocean currents along this path is shown in equation (4): （4） in, The value represents the cumulative additional energy consumption caused by ocean currents along this path; The larger the value, the greater the additional energy consumption of this path; S5: Design a cost function, improve the heuristic function of the traditional A* algorithm, improve the optimal direction selection of the traditional A* algorithm, integrate the energy consumption factor and distance factor under the influence of ocean currents, and plan the path with the optimal comprehensive cost; S51: The distance factor in the underwater robot's movement process is quantified using the Euclidean distance formula, as shown in equation (5): （5） Among them, S( , , ) and E( , , Let S and E be two points in three-dimensional space, and let d be the Euclidean distance between them. The larger the value of d, the farther the distance from the current target node to the previous node; the smaller the value of d, the closer the distance from the current target node to the previous target node; the direction with the smaller value of d is the optimal search direction of the algorithm. S52: Introducing the energy consumption cost function designed in S43, the energy consumption cost under the influence of ocean currents at a certain node is calculated as shown in equation (6): （6） Where i represents the ocean current at the current node, This indicates the current ocean current velocity at the current node. This indicates the angle between the current ocean current and the direction of travel. The value represents the additional energy consumption caused by the ocean current at the i-th navigation path point; like The larger the value, the stronger the ocean current that the current target node needs to resist, and the more energy is required to resist the ocean current; if The smaller the value, the weaker the ocean current intensity that the current target node needs to resist, and the less energy is required to resist the ocean current; The direction with the smallest value is taken as the optimal search direction for the algorithm. S53: By combining distance factors with energy consumption costs, a comprehensive cost function is constructed, improving the heuristic function of the traditional A* algorithm. Priority is given to searching for collision-free paths with shorter distances and less ocean current influence. The comprehensive cost function formula that comprehensively considers path distance and energy consumption costs is shown in Equation (7): + （7） Among them, S( , , ) and E( , , Let and represent two points in three-dimensional space, and let i represent the ocean current at the current node. This indicates the current ocean current velocity at the current node. This indicates the angle between the current ocean current and the direction of travel. The total cost from the current node S to the next node E is calculated using equation (7). By comparing the cost function values of different nodes, the node with the smaller value is selected as the next node to move to. The path with the smallest numerical value is the optimal path; S54: Use Bezier smoothing to smooth the optimal path planned in S52 to obtain an optimal path with smooth corners.
2. The underwater robot path planning algorithm considering the influence of ocean currents according to claim 1, characterized in that, Step S1 specifically includes: S11: Change the traditional method of using grid points as neighbors to using equal distances as neighbors; S12: A terrain generation function based on grid size is used to design and simulate terrain obstacles to generate different terrains; the terrain generation function based on grid size is shown in equation (1): （1） in, These are the rasterized X and Y data axes, respectively. The vertical coordinates of the constructed terrain obstacle, together with the XY data axes, constitute a complete terrain obstacle. To construct auxiliary parameters for the height of the current node of the terrain obstacle, To construct auxiliary parameters for the top x-coordinate position of the current node of the terrain obstacle, To construct auxiliary parameters for the top ordinate position of the current node of the terrain obstacle, To construct auxiliary parameters for the bottom x-coordinate position of the current node of the terrain obstacle, Auxiliary parameters for constructing the bottom ordinate position of the current node to represent terrain obstacles; S13: Adopts traditional raster map background, integrates octree map data processing methods, optimizes model data structure, integrates ocean current semantic map, adds ocean current semantics, improves ocean model construction, and fits the actual environment.