A marine environment underwater robot path planning method and system

Abstract

The application provides a kind of marine environment underwater robot path planning method and system, comprising: based on ROS Melodic, Gazebo11 and UUV Simulator, high-fidelity marine environment simulation platform is constructed, global path planning module uses improved algorithm in the high-fidelity marine environment simulation platform searches the feasible path of underwater robot from starting point to target terminal, obtains global reference path, with global reference path as guide, local path planning module obtains local optimal path through multi-stage processing flow, in global-local collaborative framework, through hierarchical intelligent cooperation mechanism, global optimal path is obtained. By constructing high-fidelity environment simulation platform, integrating spatial sampling path planning algorithm, establishing multidimensional verification system, realizing the accurate evaluation of algorithm performance, getting rid of the dependence of path planning on physical experiment, shortening the development cycle.

Description

Technical Field

[0001] This invention relates to the field of marine engineering technology, and in particular to a method and system for underwater robot path planning in marine environments. Background Technology

[0002] The validation of path planning algorithms for underwater robots, especially remotely operated vehicles (ROVs) and small-to-medium-sized autonomous underwater vehicles (AUVs), has long relied heavily on physical pool experiments. This approach not only faces the problem of high hardware costs but is also limited by the reproducibility and controllability of the experimental environment, severely restricting the iterative efficiency and validation reliability of the algorithm.

[0003] To overcome the aforementioned limitations, simulation-based verification technology has become an important development direction. Among these, the simulation framework built using the open-source 3D robot simulation environment (Gazebo) combined with the open-source underwater robot simulation platform (UUV_Simulator) extension toolkit is currently the mainstream solution for underwater robot simulation research. The Gazebo simulation environment, with its high-quality 3D rendering capabilities, support for multiple physics engines (ODE, Bullet, etc.), and rich sensor model plugins, provides a powerful foundation for underwater environment modeling. Its authority in the field of robot simulation is internationally recognized, achieving a high degree of consistency between virtual scenes and physical experiments. UUV_Simulator, as an open-source extension toolkit specifically for underwater robots, provides professional underwater dynamics modeling functions through modular design, including core modules such as water flow resistance and buoyancy calculation. It also supports accurate simulation of typical underwater robot simulation models (RexROV), laying a solid foundation for simulating underwater motion characteristics.

[0004] However, existing technical solutions still have significant shortcomings. Gazebo and UUV_Simulator mainly solve the modeling problems of the environment and the robot itself, but they are essentially open simulation "toolboxes" that lack an integrated solution that efficiently coordinates and closes the loop between path planning, dynamic environment interaction, motion control, and simulation verification. Specifically, the communication interfaces and data flows between planning algorithms, control algorithms, and the simulation environment often rely on researchers to design their own, resulting in low coupling and complex construction of the verification system. Furthermore, it is difficult to systematically and comprehensively evaluate the adaptability of the planning algorithm, the response performance of the controller, and the final task completion effect.

[0005] As marine engineering tasks place increasing demands on the autonomy and operational flexibility of underwater robots, higher requirements are being placed on the reliability of path planning algorithms in complex dynamic environments. Therefore, there is an urgent need in this field for an integrated high-fidelity simulation solution capable of achieving closed-loop verification across the entire chain from environmental perception and intelligent decision-making to motion control, significantly improving the verification credibility and R&D iteration efficiency of path planning algorithms without increasing hardware costs. Summary of the Invention

[0006] This invention provides a method and system for underwater robot path planning in marine environments, which solves the technical problems of high physical experiment costs, low simulation platform fidelity, and single verification dimension in existing underwater robot path planning verification.

[0007] The first aspect of this invention provides a path planning method for underwater robots in a marine environment, comprising:

[0008] S1. Construct a high-fidelity marine environment simulation platform based on ROS Melodic, Gazebo11, and UUV Simulator; the construction of the high-fidelity marine environment simulation platform includes building a marine environment model, integrating an underwater robot model, and configuring the simulation environment startup file;

[0009] S2, the global path planning module adopts an improved version. The algorithm searches for feasible paths for the underwater robot from the starting point to the target endpoint in the high-fidelity marine environment simulation platform and obtains a global reference path;

[0010] S3. Guided by the global reference path, the local path planning module obtains the local optimal path through a multi-stage processing flow.

[0011] S4. Under the global-local collaboration framework, the optimal global path is obtained through a hierarchical intelligent collaboration mechanism.

[0012] Furthermore, the high-fidelity marine environment simulation platform built based on ROS Melodic, Gazebo11, and UUV Simulator includes:

[0013] Build a marine environment model: Load a custom underwater scene model (lake.world) in the physical simulation environment (Gazebo11), import a digital elevation model (DEM), configure the Underwater Current Plugin and Wavefield Plugin for simulating the marine dynamic environment, and enable underwater illumination attenuation.

[0014] Integrated underwater robot simulation model: An underwater robot simulation model is built based on a general robot description file (URDF file), and extended with the integration of an inertial measurement unit (IMU), a Doppler velocity log (DVL) sensor, and a three-dimensional acoustic environment perception (3D sonar) sensor; the data generated by each sensor simulation is published through a preset independent communication interface within the high-fidelity marine environment simulation platform;

[0015] Write a startup configuration file to initialize the simulation environment and integrate and load the core functions: initialize the initial spatial pose of the underwater robot (ROV), build the physical simulation environment and generate the corresponding robot dynamics model; load the motion controller (PID controller) to realize the closed-loop control of the robot, start the path planning function to publish the target destination information and generate the corresponding visual path; simultaneously open the pre-configured visual monitoring interface and integrate the data recording function to store sensor data and key result data generated by path planning in real time.

[0016] Furthermore, the global path planning module includes an environment modeling unit, a path search unit, a path optimization unit, a constraint processing unit, and a result output unit; the global path planning module adopts an improved... The algorithm searches for feasible paths for the underwater robot from the starting point to the target endpoint in the high-fidelity marine environment simulation platform, and obtains a global reference path, including:

[0017] The environmental modeling unit integrates terrain elevation information and static obstacle information, and generates a grid map with passage cost through spatial discretization;

[0018] The constraint processing unit, relying on the grid map with passage costs, transforms the robot's motion capability limitations and environmental navigation rules into specific path search constraints, forming a boundary framework for intelligent search.

[0019] The path search unit runs an optimized heuristic search algorithm within a preset boundary framework, explores the space based on preset path search constraints, and generates an original path sequence.

[0020] The path optimization unit performs fine-grained optimization on the original path obtained by the path search unit, including smoothing the path trajectory and optimizing the path data structure to remove redundancy and improve path quality.

[0021] The output unit normalizes and integrates the optimized path, generating and outputting a complete global reference path data package containing a three-dimensional spatial coordinate sequence and related navigation attribute information.

[0022] Furthermore, the path search unit runs an optimized heuristic search algorithm within a preset boundary framework, performs spatial exploration based on preset path search constraints, and generates an initial sequence of original paths, including:

[0023] The path search unit uses an improved The algorithm performs a heuristic search on a constrained grid map, optimizing the traditional 8-way search into a 5-way search. It also employs a dynamic weight evaluation function F(n) = G(n) + (1 + r / R)H(n) to dynamically adjust the weights of heuristic terms based on the relative distance between the current node and the starting point. This guides the search process to gradually expand nodes from the starting point to the destination, ultimately backtracking to generate the original path sequence that satisfies the basic constraints. Here, G(n) represents the actual cost from the starting point to the current node, H(n) represents the estimated cost of the Manhattan distance from the current node to the target point, r represents the distance from the current node to the target point, and R represents the total distance from the starting point to the target point.

[0024] Furthermore, the local path planning module includes a ROS (Robot Operating System) data subscription module, an obstacle handling module, a global path point extraction module, a local-global endpoint calculation module, a sampling space generation module, a sampling point filtering module, an endpoint region judgment module, and a dynamic planning waypoint generation module; the multi-stage processing flow includes three key stages: environmental perception, intelligent decision-making, and path optimization; guided by the global reference path, the local path planning module obtains the optimal local path through a multi-stage processing flow, including:

[0025] In the process perception phase, the ROS data subscription module acquires raw data from sonar sensors, robot real-time pose, global reference path, and navigation start and end point coordinates in real time through a topic subscription mechanism. This raw data is then standardized and distributed in parallel to the obstacle processing module and the global path point extraction module. The obstacle processing module receives sonar sensor data and robot pose, converts the local sonar sensor data into an obstacle distribution map in the global coordinate system using a coordinate transformation algorithm, assesses the threat level of each obstacle, and outputs structured environmental information with area markers and hazard levels. The global path point extraction module analyzes the robot's relative position to the global path, determines the path tracking progress through nearest neighbor search, and calculates a suitable local planning reference length for the current environment.

[0026] In the intelligent decision-making phase, the local and global endpoint calculation module, based on the outputs of the obstacle processing module and the global path point extraction module, intelligently extracts suitable phased target points for local planning from the global reference path. The sampling space generation module, guided by the phased target points, constructs a three-dimensional discrete sampling space in front of the robot's movement. It adopts an adaptive density distribution strategy to increase sampling density in the near-obstacle area to enhance safety and optimize the distribution in the open area to improve computational efficiency. The generated sampling point set is sent to the sampling point filtering module. The sampling point filtering module evaluates the comprehensive value of each sampling point through a triple cost function and selects the optimal local endpoint.

[0027] During the path optimization phase, the dynamic planning waypoint generation module generates the optimal local path from the current position to the local endpoint based on the filtering results of the sampling point filtering module and real-time environmental information; the endpoint region judgment module continuously monitors the relative position of the robot and the global endpoint to ensure the consistency between the path planning and the global goal.

[0028] Furthermore, the sampling point selection module includes a cost calculation engine, a cost normalization processor, a weight configuration unit, and a comprehensive evaluation and ranking unit; the sampling point selection module evaluates the comprehensive value of each sampling point through a triple cost function to select the optimal local endpoint, including:

[0029] The cost calculation engine simultaneously calculates the triple cost for each sampling point generated by the sampling space generation module. The normalization processor obtains the triple cost value for each sampling point through mapping. The triple cost function includes obstacle avoidance cost, target approach cost, and path smoothing cost.

[0030] The cost of obstacle avoidance is In the formula, The distances between discrete sampling points and obstacles are represented as follows: d1 represents the danger distance, and d2 represents the critical safety distance.

[0031] The cost of target convergence is In the formula, This is expressed as the distance between discrete sampling points and the target endpoint. This represents the farthest distance between the current planning starting point and the local / global endpoint;

[0032] The cost of path smoothing is In the formula, This is expressed as the distance between a discrete sampling point and the current planning starting point. This represents the maximum distance of a discrete sampling point from the current planning starting point;

[0033] Construct a judgment matrix and calculate the consistency ratio. , CR The consistency ratio is used to determine whether the consistency of the matrix is ​​acceptable when CR < 0.1. CIIndicates consistency index and , RI This represents the average random consistency index, where n represents the order of the judgment matrix. This represents the largest eigenvalue of the judgment matrix;

[0034] The weights are obtained by normalizing the judgment matrix column-wise and then taking the arithmetic mean of the rows. , , The final expression for the total cost function is obtained. The comprehensive evaluator calculates the total cost value of each sample point based on the total cost function expression; the sorter sorts the total costs of all sample points in ascending order to obtain the local optimal waypoint.

[0035] Furthermore, the global-local collaborative framework includes a perception fusion layer, a global planning layer, a local planning layer, a control execution layer, and a collaborative management layer; the acquisition of the globally optimal path through a hierarchical intelligent collaboration mechanism within the global-local collaborative framework includes:

[0036] The high-fidelity simulation platform provides a near-realistic testing environment for the entire planning system;

[0037] The perception fusion layer constructs an accurate environmental cognition model by integrating multi-source sensor data, providing a real and reliable environmental perception foundation for the entire planning system.

[0038] The global planning layer runs the global path planning module based on the environmental perception provided by the prior environment and the perception fusion layer, and generates a global reference path that comprehensively considers path length, safety and energy efficiency.

[0039] The local planning layer runs the local path planning module within the guidance framework of the global reference path. The local planning layer receives real-time environmental data provided by the perception fusion layer, detects dynamic obstacles and environmental changes that were not anticipated in the global planning, and generates a safe and smooth local path trajectory within milliseconds.

[0040] The control execution layer transforms the local path trajectory generated by the local planning layer into precise motion control commands, driving the underwater robot in the high-fidelity simulation platform to perform motion, and feeding back the execution status to the perception fusion layer in real time to form a complete control closed loop.

[0041] The management team continuously monitors operations at all levels and dynamically coordinates work pace and resource allocation at each level based on environmental changes and task requirements.

[0042] The second aspect of this invention provides a path planning system for underwater robots in marine environments, comprising:

[0043] High-fidelity simulation platform: A simulation test platform used to provide a near-real marine environment, simulating water flow, water depth, obstacle environmental factors, and verifying and testing the performance of the entire path planning system;

[0044] Global Path Planning Module: The core component of the hierarchical planning architecture, used to receive environmental maps and navigation task inputs, and output a global reference path that takes into account path length, safety and executability;

[0045] Local path planning module: includes ROS data subscription module, obstacle handling module, global path point extraction module, local and global endpoint calculation module, sampling space generation module, sampling point filtering module, endpoint region judgment module, and dynamic planning waypoint generation module. These modules work together to form an efficient and collaborative planning closed-loop system. Under the guidance of the global reference path, it generates a safe, smooth, and locally optimal path that conforms to robot dynamics based on real-time perceived environmental information.

[0046] ROS Subscription Module: Used to subscribe to ROS topics, collect real-time data including sonar sensor data, robot real-time pose information, global navigation start coordinates, and global navigation end coordinates, and transmit the received data to the obstacle processing module and the global path point extraction module;

[0047] Obstacle processing module: Used to process obstacle information obtained from sensors or simulation platforms, generate obstacle position and velocity information, and provide environmental perception data for local path planning;

[0048] Global path point extraction module: used to extract the sequence of local path points related to the current robot position from the global path, and to determine the reference frame and spatial range of the local planning;

[0049] The Local / Global Endpoint Calculation Module is used to intelligently extract the phased target points of the local endpoint planning from the global path.

[0050] Sampling space generation module: used to construct a three-dimensional discrete sampling space in front of the robot's movement that meets the robot's reachability requirements;

[0051] Sampling point filtering module: used to quantitatively evaluate each candidate point in the sampling space and filter out the local endpoint with the best comprehensive cost value;

[0052] Endpoint Region Judgment Module: Used to determine whether the current local endpoint has reached the stage target point;

[0053] The dynamic programming reference waypoint module is used to transform the selected optimal local endpoints into an executable sequence of waypoints. Through curve fitting and dynamic optimization, it generates a smooth and continuous locally optimal path, ensuring that the control execution layer can accurately track the path.

[0054] A third aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, the computer program being executed by a processor as described above in a method for path planning of an underwater robot in a marine environment.

[0055] A fourth aspect of the present invention provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described underwater robot path planning method for a marine environment.

[0056] As can be seen from the above technical solutions, the present invention has the following advantages:

[0057] 1) The simulation platform built on the ROS Melodic, Gazebo 11 and UUV_Simulator toolchain has high fidelity. It loads DEM three-dimensional terrain, configures UnderwaterCurrentPlugin and WavefieldPlugin to simulate dynamic hydrodynamics, and superimposes sensor physical characteristics such as sonar noise and IMU zero drift. The robot motion response has a low deviation rate from the physical experiment, effectively bridging the "digital twin gap" and ensuring the credibility of path planning verification.

[0058] 2) The platform has excellent adaptability and can seamlessly integrate the native RexRov robot model of UUV_Simulator. It can extend multiple types of sensors such as IMU, DVL, and 3D sonar through URDF. The sensor data is compatible with the ROS ecosystem and supports flexible configuration of custom lake.world scenes and start_pid_demo_with_sonar.launch startup files to adapt to different underwater verification needs.

[0059] 3) It does not rely on costly physical sea trials, has low single simulation costs, can quickly build multiple test scenarios, significantly reduce R&D costs, shorten the path planning verification cycle, and accelerate the application of underwater robot-related technologies. Attached Figure Description

[0060] Figure 1 A flowchart illustrating the steps of a path planning method and system for underwater robots in a marine environment, provided as an embodiment of the present invention.

[0061] Figure 2 This is a schematic diagram of the Gazebo marine environment simulation provided in an embodiment of the present invention.

[0062] Figure 3 The RexRov model configuration provided in this embodiment of the invention.

[0063] Figure 4 The improved version provided for the embodiments of the present invention The global path planning graph obtained by the algorithm.

[0064] Figure 5 This is a schematic diagram illustrating the establishment of a sampling space provided in an embodiment of the present invention.

[0065] Figure 6 This is a schematic diagram of real-time path optimization planning based on spatial sampling, provided for an embodiment of the present invention.

[0066] Figure 7 This is a schematic diagram of the components of an underwater robot path planning system for a marine environment.

[0067] Figure 8 This diagram illustrates the process by which the sub-modules in the local path module collaboratively obtain the optimal local path. Detailed Implementation

[0068] This invention provides a method and system for underwater robot path planning in marine environments, which addresses the shortcomings of existing underwater robot path planning verification methods, such as high physical experiment costs, low simulation platform fidelity, and limited verification dimensions.

[0069] To enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this specification, and not all embodiments. Based on the embodiments in this specification, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this specification.

[0070] Please see Figure 1 , Figure 1 A flowchart illustrating the steps of a path planning method and system for underwater robots in a marine environment, provided as an embodiment of the present invention.

[0071] This invention provides a path planning method and system for underwater robots in marine environments, comprising:

[0072] S1. Construct a high-fidelity marine environment simulation platform based on ROS Melodic, Gazebo11, and UUV Simulator; the construction of the high-fidelity marine environment simulation platform includes building a marine environment model, integrating an underwater robot model, and configuring the simulation environment startup file;

[0073] S2, the global path planning module adopts an improved version. The algorithm searches for feasible paths for the underwater robot from the starting point to the target endpoint in the high-fidelity marine environment simulation platform and obtains a global reference path;

[0074] S3. Guided by the global reference path, the local path planning module obtains the local optimal path through a multi-stage processing flow.

[0075] S4. Under the global-local collaboration framework, the optimal global path is obtained through a hierarchical intelligent collaboration mechanism.

[0076] In one specific embodiment of this example, a high-fidelity marine environment simulation platform is constructed based on ROS Melodic, Gazebo11, and UUVSimulator, including:

[0077] 1. Environmental configuration of the high-fidelity marine environment simulation platform

[0078] To ensure the credibility of the underwater robot path planning verification, a high-fidelity marine environment simulation platform was built based on the ROS Melodic, Gazebo11, and UUV_Simulator toolchains. The environment configuration of the high-fidelity marine environment simulation platform includes: ROS Melodic and Gazebo11 were installed on an Ubuntu 18.04 system as the core framework of the simulation environment; the UUVSimulator toolkit was installed using the command `sudo apt install ros-melodic-uuv-simulator` to implement the specific simulation of the underwater robot; and MATLAB R2021a (including RoboticsToolbox) was installed, mainly for subsequent processing and analysis of the simulation data.

[0079] 2. Construct a marine environment model

[0080] To better simulate the marine environment and the scenarios that underwater robots might encounter during their movement in real ocean conditions, an environmental parameter model incorporating underwater topography, current fields, waves, and light attenuation characteristics is integrated into a simulation system that closely resembles the real marine environment. Figure 2 The image shown is a schematic diagram of the Gazebo marine environment simulation provided in an embodiment of the present invention. The steps for constructing the marine environment model are as follows:

[0081] 2.1 Scene Construction

[0082] ① Create the world file: Create the file lake.world in the uuv_simulator / worlds directory;

[0083] ② Referencing the terrain model: In the file, via <include>The tag imports a predefined lake model underwater scene, eliminating the need to repeatedly write terrain parameters. <pose>The model's position in the world coordinate system is defined;

[0084] ③ Model details definition: The specific attributes of the lake terrain, such as the shape of the lake bottom, water surface effects, physical collision objects, and visual appearance, are defined in its own independent model file, located at uuv_simulator / uuv_gazebo_worlds / models / lake / model.sdf. This file references meshes / LakeBottom.dae (lake bottom mesh model, defining the terrain's undulations) and meshes / WaterSurface.dae (water surface mesh, containing wave or transparency effects), configures collision attributes (such as whether objects are allowed to pass through the terrain), and configures visual rendering (such as materials, colors, lighting reflections, etc. to achieve a realistic underwater environment).

[0085] 2.2 Configuring Ocean Current Parameters: The simulated ocean current parameters are specifically implemented by modifying the configuration file of the simulation world. The core of this configuration lies in configuring an underwater current plugin called `underwater_current_plugin`, which includes the UnderwaterCurrentPlugin and the WavefieldPlugin. First, through... <namespace>The tag specifies its ROS namespace as hydrodynamics, and through <topic>The tag defines the topic for which it publishes ocean current velocity data as current_velocity. The key is that, through...<constant_current> The nested parameter groups under the tag allow for a refined definition of the ocean current velocity vector. The velocity vector is determined by parameters in three dimensions: velocity magnitude, horizontal angle, and vertical angle. Each dimension contains four key sub-parameters: mean represents the average value, defining a baseline; min / max represents the velocity fluctuation range; and mu and noiseAmp are used to superimpose random noise to simulate the uncertainties of the real ocean environment. By setting these parameters individually, ocean current fields with specific intensity, direction, and dynamic characteristics can be accurately simulated.

[0086] 3. Integrated underwater robot simulation model

[0087] The reliability of the planned path needs to be verified during the path planning process. This is achieved by controlling the underwater robot to move along the planned path in a simulation platform, while simultaneously acquiring real-time sensor data from the sensors mounted on the underwater robot to provide data support for real-time path optimization. A schematic diagram of the underwater robot (RexRov) model configuration is shown below. Figure 3 All configuration files are located in the uuv_descriptions and uuv_control packages of UUV Simulator. Through the collaborative work of multiple configuration files, the robot's kinematic model, dynamic model, and environmental perception sensor model are integrated into a realistic marine environment simulation system. A layered and modular design approach is adopted, as detailed below:

[0088] 3.1 Structural Layer: The robot's physical structure is defined using URDF / Xacro format files (such as uuv_descriptions / urdf / rexrov_base.xacro), including the base coordinate system (base_link), inertial parameters (mass, inertia matrix), visual appearance (3D appearance model), and simplified geometry for collision detection. This file serves as the integration core, through... <xacro:include>The statement associates independent thruster (rexrov_actuators.xacro) and sensor (rexrov_sensors.xacro) configuration files, achieving modular functionality, while reserving a Gazebo plugin interface for subsequent loading of hydrodynamic plugins; in addition, the sensor configuration files include inertial measurement unit (IMU sensor), Doppler velocimeter (DVL sensor) and three-dimensional acoustic environment perception (3D sonar sensor) sensors. The raw data generated by the simulation of each sensor is published through the / rexrov / imu, / rexrov / dvl, and / rexrov / sonar topics.

[0089] 3.2 Dynamics Layer: Use independent parameter files (such as uuv_control / uuv_trajectory_control / config / models / rexrov / params.yaml) to configure key hydrodynamic parameters such as mass, inertia matrix (consistent with the xacro file), added mass (inertia of the water body moving with the robot), damping coefficient (linear / quadratic damping, affecting motion resistance), center of buoyancy and center of gravity, etc., to ensure the physical accuracy of motion simulation.

[0090] 3.3 System Integration Layer: All the above configurations are loaded uniformly through the startup file (such as uuv_descriptions / launch / upload_rexrov_default.launch), and the complete robot model is instantiated into the Gazebo simulation environment. At the same time, the hydrodynamics plugin is loaded to make the parameters effective.

[0091] This modular design allows for modifications to the robot's structure, dynamics, or sensor configuration without interference, greatly improving the reusability and development efficiency of the simulation model.

[0092] 4. Configure the simulation environment startup file:

[0093] To quickly launch the different components of the high-fidelity marine environment simulation platform, a launch file named start_pid_demo_with_sonar.launch was written. This file acts as an integration manager and performs the following operations in sequence:

[0094] 4.1 Environment and Model Initialization

[0095] Initialize the Gazebo physics simulation environment and load the preset underwater scene. Then, call the model description file to generate a model instance of an underwater robot (such as RexROV) at the specified initial position;

[0096] 4.2 Control System Loading: Load the pre-configured PID controller node. This controller will receive the target command and calculate the control quantity of the output thruster to drive the robot to move.

[0097] 4.3 Task Planning and Visualization: Publish the endpoint information of the path planning. At the same time, generate a expected reference path from the robot's initial position to the endpoint and display it in the visualization tool Rviz through a pre-configured interface. This interface integrates visualization elements such as robot model, sensor data (such as sonar point cloud) and path.

[0098] 4.4 Data Management: Integrates ROS data recording function (rosbag node), subscribes to and stores key simulation data in real time, including raw sensor data, control commands, robot pose and path information, for subsequent performance analysis and algorithm verification.

[0099] In one specific implementation of this embodiment, the global path planning module adopts an improved... The algorithm searches for feasible paths for the underwater robot from the starting point to the target endpoint within the high-fidelity marine environment simulation platform, and obtains a global reference path; including:

[0100] The global path planning module obtains the global reference path through the environment modeling unit, constraint processing unit, path search unit, path optimization unit, and result output unit. The functions of each unit are as follows:

[0101] The environmental modeling unit integrates terrain elevation information and static obstacle information, and generates a grid map with passage cost through spatial discretization;

[0102] The constraint processing unit, relying on the grid map with passage costs, transforms the robot's motion capability limitations and environmental navigation rules into specific path search constraints, forming a boundary framework for intelligent search.

[0103] The path search unit runs an optimized heuristic search algorithm within a preset boundary framework, explores the space based on preset path search constraints, and generates an original path sequence.

[0104] The path optimization unit performs fine-grained optimization on the original path obtained by the path search unit, including smoothing the path trajectory and optimizing the path data structure to remove redundancy and improve path quality.

[0105] The output unit normalizes and integrates the optimized path, generating and outputting a complete global reference path data package containing a three-dimensional spatial coordinate sequence and related navigation attribute information.

[0106] In traditional underwater robot path planning, the path search unit is usually directly applied. The algorithm uses a heuristic path search, which requires traversing and evaluating multiple adjacent directions (typically 8) for each node. This results in an excessively large search range and a surge in the number of computational nodes in scenarios with a fixed grid and a preset endpoint, leading to low optimization efficiency and making it difficult to meet the urgent needs of underwater robots for real-time path planning. To overcome these shortcomings, this embodiment improves upon the traditional... The algorithm incorporates two key optimizations aimed at significantly reducing the search space and improving planning efficiency. Figure 4 The improved version provided for the embodiments of the present invention The global path planning graph obtained by the algorithm. Compared with traditional... The algorithm optimization is as follows:

[0107] 1. Strategic narrowing of search directions

[0108] This invention selectively reduces the expansion directions of each node from the traditional eight directions to five. Specifically, these five directions are preferably the robot's forward direction and its four adjacent key directions (such as forward, forward left, forward right, up, and down). This reduction is not a simple decrease, but a strategic optimization based on the kinematic characteristics of underwater robots: it eliminates redundant directions that are not strongly related to the main forward direction (such as purely lateral or backward directions), thereby effectively reducing the branching factor and significantly narrowing the algorithm's search space while ensuring path reachability.

[0109] 2. Dynamic weighting mechanism of the evaluation function:

[0110] This invention introduces a dynamic weighting factor based on relative distance into the evaluation function of the algorithm. The optimized evaluation function is defined as shown in equation (1):

[0111] (1)

[0112] Where: F(n) is the estimated total cost to pass through node n, G(n) is the actual cost from the starting point to node n, H(n) is the heuristic cost from node n to the target point, r represents the Euclidean distance from the current node n to the target point, and R represents the Euclidean distance from the starting point to the target point.

[0113] The core of this dynamic weighting mechanism lies in the fact that the weight (1+r / R) is a quantity that adaptively changes with the search process. In the early stages of the search (when r≈R), r / R≈1, and the weight factor is approximately 2. At this point, the algorithm tends to explore extensively to avoid prematurely getting trapped in local optima. In the later stages of the search (when r gradually approaches 0), r / R≈0, and the weight factor approaches 1. At this point, the algorithm's behavior approaches standard behavior. The algorithm focuses on using heuristic information to quickly converge to the target. Through extensive exploration in the early stages and rapid convergence in the later stages, the algorithm reduces the number of nodes that need to be evaluated while ensuring the optimal path is found, thus improving efficiency.

[0114] In one specific embodiment of this example, the process of establishing the sampling space includes:

[0115] The establishment of the sampling space is the foundation for ensuring the implementation of path planning. Figure 5 This is a schematic diagram illustrating the establishment of a sampling space according to an embodiment of the present invention. The process of establishing the sampling space is as follows:

[0116] 1. Define the sampling space dimension and granularity parameters:

[0117] Define a three-dimensional rectangular sampling space with the current position of the underwater robot as the reference origin. The specific scale of this space is defined by the following parameters: longitudinal (along the robot's forward direction, X-axis) sampling distance is L, the preset number of sampling points is ln, and the longitudinal sampling interval is Dx; lateral (in the horizontal plane perpendicular to the forward direction, Y-axis) sampling distance is W, the preset number of sampling points is wn, and the lateral sampling interval is Dy; vertical (in the vertical plane, Z-axis) sampling distance is H, the preset number of sampling points is hn, and the vertical sampling interval is Dz.

[0118] 2. Calculate the uniform sampling interval:

[0119] Based on the principle of "n sampling points corresponding to (n-1) intervals", the sampling intervals of each dimension are accurately calculated to ensure that waypoints are evenly distributed in space and form a regular search grid: the longitudinal sampling interval is Dx=L / (ln-1); the lateral sampling interval is Dy=W / (wn-1); and the vertical sampling interval is Dz=H / (hn-1).

[0120] 3. Set the starting position of the sampling space

[0121] The longitudinal distance between the starting point of the sampling space and the robot's current position is set as parameter S (in a preferred embodiment, S can be 5 meters). This design ensures that the sampling area is located a safe and operable distance in front of the robot, which is consistent with its forward movement characteristics and avoids invalid rear sampling.

[0122] 4. Generate discrete waypoint coordinate sets

[0123] In the established robot coordinate system, the relative coordinates of each discrete waypoint Calculate using the following formula, where i, j, k The sampling point indices for each dimension are as follows: the formula for calculating the vertical coordinate is shown in equation (2), the formula for calculating the horizontal coordinate is shown in equation (3), and the formula for calculating the vertical coordinate is shown in equation (4).

[0124] (2)

[0125] (3)

[0126] (4)

[0127] The calculation formulas for the lateral and vertical coordinates are specially designed to ensure that the entire sampling space is symmetrically distributed about the robot body in the roll and pitch directions (i.e., with the robot's current heading and depth as the axes of symmetry). This symmetrical design conforms to the kinematic and dynamic characteristics of underwater robots, ensuring the rationality and feasibility of the generated candidate paths.

[0128] Through the above steps, a three-dimensional discrete waypoint space covering a predetermined area in front of the robot can be generated efficiently and in a structured manner, laying the foundation for subsequent path search and optimization algorithms.

[0129] In one specific implementation of this embodiment, the local path planning module obtains the locally optimal path through a multi-stage processing flow, including:

[0130] The local path planning module obtains the locally optimal path through the cooperation of its internal sub-modules, as follows:

[0131] 1. Multi-source information acquisition and processing

[0132] During the local path planning process, the ROS subscription module acquires three types of key information in real time:

[0133] 1.1 Environmental perception data: Subscribe to sonar topics (e.g., / rexrov / sonar), receive raw data (sonarData) of type sensor_msgs / LaserScan, and obtain distance information of obstacles around the robot.

[0134] 1.2 Robot State Data: Subscribe to the localization topic (e.g., / truth_to_tf / rexrov / pose) to parse the underwater robot's three-dimensional position and attitude quaternions in the global coordinate system.

[0135] 1.3 Task instruction data: Subscribe to the start and end points of the topic (such as / rexrov / start_pose_topic and / rexrov / goal_pose_topic) to obtain the starting and target coordinates of the current navigation task.

[0136] 2. Obstacle Information Fusion and Region Judgment

[0137] 2.1 The obstacle handling module receives raw sonar distance data (sonarData) and the robot's real-time pose. Its core function is:

[0138] ① Coordinate transformation: Based on the robot's current pose, the polar coordinate distance data measured by the sonar is converted into the three-dimensional coordinates of the obstacle in the global coordinate system.

[0139] ② Area Determination: Based on the obstacle's position relative to the robot, determine which area the obstacle is located in (e.g., directly in front, to the left front, to the right front, etc.) and output the corresponding obstacle area marker. This step provides semantic information for subsequent avoidance decisions.

[0140] 3. Global path reference extraction

[0141] The global pathpoint extraction module is responsible for connecting global planning with local execution. Its inputs are the robot's current position and the global pathpoint sequence. This module executes the following logic:

[0142] 3.1 Nearest Point Search: Find the point in the global path point sequence that is closest to the robot's current position and return its index.

[0143] 3.2 Local Reference Calculation: Based on the nearest point index and the total number of points in the path, a local planning reference length is calculated to determine the range of path segments that the local planning should focus on.

[0144] 4. Dynamic determination of local planning target points

[0145] 4.1 Calculation of Local and Global Endpoints: The module for calculating local and global endpoints calculates a segment forward based on the nearest point index and global path obtained from the global path reference extraction. The endpoint of the extracted path is then determined as the temporary target point (local and global endpoint) for the current local planning.

[0146] 4.2 Three-dimensional sampling space generation: The sampling space generation module generates a three-dimensional discrete sampling point matrix in front of the robot based on the preset sampling parameters (such as length, width, height, and resolution) and the robot's current position, and outputs the global coordinates of each sampling point.

[0147] 4.3 Optimal Local Endpoint Selection: The sampling point selection module comprehensively considers obstacle avoidance safety and global consistency. Its inputs include the coordinates of all sampling points, the coordinates of the local global endpoint calculated in the local global endpoint calculation, and the obstacle information output from obstacle information fusion and region judgment. This module selects the point from the sampling points that has no collision risk (judged based on obstacle coordinates) and is closest to the local global endpoint as the final optimal local endpoint. This ensures that the path, while avoiding immediate obstacles, always progresses towards the global goal.

[0148] Specifically, the sampling point selection module evaluates the comprehensive value of each sampling point through a triple cost function, and selects the optimal local endpoint, including:

[0149] The cost calculation engine uses a triple cost function to simultaneously calculate the triple cost for each sampling point generated by the sampling space generation module. The triple cost function includes obstacle avoidance cost, target approach cost, and path smoothing cost. The triple cost value of each sampling point is obtained by mapping through a normalization processor, thereby evaluating each point.

[0150] The specific details of obstacle avoidance cost, target tendency cost, and path smoothing cost are as follows:

[0151] The cost of obstacle avoidance is In the formula, d1 represents the distance between the discrete sampling point and the obstacle, and d2 represents the critical safety distance, which is set to 20 meters in the preferred embodiment of this example.

[0152] The cost of target convergence is In the formula, This is expressed as the distance between discrete sampling points and the target endpoint. This represents the farthest distance between the current planning starting point and the local / global endpoint;

[0153] The cost of path smoothing is In the formula, This is expressed as the distance between a discrete sampling point and the current planning starting point. This represents the maximum distance of a discrete sampling point from the current planning starting point;

[0154] To better calculate the total value of each sampling point, a judgment matrix is ​​constructed to obtain representative weights for different costs. The specific process is as follows:

[0155] Construct a judgment matrix and calculate the consistency ratio. , CR The consistency ratio is used to determine whether the consistency of the matrix is ​​acceptable when CR < 0.1. CI Indicates consistency index and , RI This represents the average random consistency index, where n represents the order of the judgment matrix. This represents the largest eigenvalue of the judgment matrix;

[0156] The weights are obtained by normalizing the judgment matrix column-wise and then taking the arithmetic mean of the rows. , , The final expression for the total cost function is obtained. The comprehensive evaluator calculates the total cost value of each sampling point based on the total cost function expression; the sorter sorts the total costs of all sampling points in ascending order to obtain the optimal local endpoint.

[0157] 5. Safe reference path generation and task termination judgment

[0158] 5.1 Dynamic Programming Waypoint Generation: The dynamic programming reference waypoint generation module receives the robot's starting point, current position, optimal local endpoint, sample point set, and obstacle information. It uses a dynamic programming algorithm to calculate a collision-free and smooth sequence of reference waypoints from the current position to the optimal local endpoint within the space formed by the sample points.

[0159] 5.2 End-point area determination: The end-point area determination module continuously compares the distance between the robot's current position and the global end point. When the robot enters the area near the global end point, this module outputs a flag indicating that the task is about to be completed, thereby triggering the system to perform a state switch or stop procedure.

[0160] Through the above modular design and process, the decoupling and coordination of environmental perception, global reference, local decision-making and motion generation in the local path planning process are realized, which effectively improves the real-time performance, safety and reliability of underwater robots in complex underwater environments.

[0161] In one specific embodiment of this invention, a global-local collaborative framework is provided. Through a hierarchical intelligent collaboration mechanism, it optimizes local paths in real time to obtain the globally optimal path, including:

[0162] The global-local collaborative framework comprises a perception fusion layer, a global planning layer, a local planning layer, a control execution layer, and a collaborative management layer. Each layer plans and optimizes paths in real time through an intelligent collaboration mechanism. A high-fidelity simulation platform provides a near-realistic testing environment for the entire system. The intelligent collaboration process of each layer is as follows:

[0163] The perception fusion layer constructs an accurate environmental cognition model by integrating multi-source sensor data, providing a real and reliable environmental perception foundation for the entire planning system.

[0164] The global planning layer runs the global path planning module based on the environmental perception provided by the prior environment and the perception fusion layer, generating a macro-global reference path that comprehensively considers path length, safety and energy efficiency, providing a guiding framework for all subsequent navigation behaviors;

[0165] The local planning layer runs the local path planning module within the guidance framework of the global reference path. The local planning layer receives real-time environmental data provided by the perception fusion layer, detects dynamic obstacles and environmental changes that were not anticipated in the global planning, and generates a safe and smooth local path trajectory within milliseconds.

[0166] The control execution layer transforms the local path trajectory generated by the local planning layer into precise motion control commands, driving the underwater robot in the high-fidelity simulation platform to perform motion, and feeding back the execution status to the perception fusion layer in real time to form a complete control closed loop.

[0167] Throughout the process, the top-level collaborative management team continuously monitors the operational status of each level, dynamically coordinates the work pace and resource allocation at each level, and intelligently adjusts planning strategies based on environmental changes and task requirements.

[0168] This real-time optimization strategy performs real-time local optimization on the global reference path in global path planning to obtain the global reference path. For example... Figure 6 The diagram shown is a schematic diagram of real-time path optimization planning based on spatial sampling provided in an embodiment of the present invention.

[0169] In one specific embodiment of this invention, a path planning system for underwater robots in a marine environment is provided, comprising:

[0170] like Figure 7 The diagram shows the components of an underwater robot path planning system for a marine environment. It mainly includes a high-fidelity simulation platform, a global path planning module, and a local path planning module. The functions of each module are as follows:

[0171] High-fidelity simulation platform: A simulation test platform used to provide a near-real marine environment, simulating water flow, water depth, obstacle environmental factors, and verifying and testing the performance of the entire path planning system;

[0172] Global Path Planning Module: The core component of the hierarchical planning architecture, it receives environmental maps and navigation task inputs, and outputs a global reference path that takes into account path length, safety and executability, providing reliable guidance for subsequent local real-time planning;

[0173] The local path planning module includes specialized sub-units such as a ROS data subscription module, obstacle handling module, global path point extraction module, local and global endpoint calculation module, sampling space generation module, sampling point filtering module, endpoint region judgment module, and dynamic planning waypoint generation module. These sub-units collaborate in a pipeline to form an efficient and coordinated planning closed-loop system. The planning closed-loop system is used to generate a safe, smooth, and robot-dynamically-compliant locally optimal path motion trajectory within the macro framework of the global path, based on real-time perceived environmental information.

[0174] In one specific embodiment of this example, a process is provided for each sub-module in a local path module to collaboratively obtain a locally optimal path, including:

[0175] Figure 8 This diagram illustrates the collaborative process of various sub-modules within the local path module to obtain the optimal local path. These sub-modules include the ROS data subscription module, obstacle handling module, global path point extraction module, local and global endpoint calculation module, sampling space generation module, sampling point filtering module, endpoint region judgment module, and dynamic planning waypoint generation module. Guided by the global reference path, these sub-modules obtain the optimal local path through a multi-stage processing flow of process awareness, intelligent decision-making, and path optimization.

[0176] In the process perception phase, the ROS data subscription module acquires raw data from sonar sensors, robot real-time pose, global reference path, and navigation start and end point coordinates in real time through a topic subscription mechanism. This raw data is then standardized and distributed in parallel to the obstacle processing module and the global path point extraction module. The obstacle processing module receives sonar sensor data and robot pose, converts the local sonar sensor data into an obstacle distribution map in the global coordinate system using a coordinate transformation algorithm, assesses the threat level of each obstacle, and outputs structured environmental information with area markers and hazard levels. The global path point extraction module analyzes the robot's relative position to the global path, determines the path tracking progress through nearest neighbor search, and calculates a suitable local planning reference length for the current environment.

[0177] In the intelligent decision-making phase, the local-to-global endpoint calculation module, based on the outputs of the obstacle handling module and the global path point extraction module, intelligently extracts suitable phased target points for local planning from the global reference path. This local endpoint maintains consistency with the global direction while considering real-time environmental constraints, providing clear guidance for subsequent planning. The sampling space generation module, guided by the phased target points, constructs a three-dimensional discrete sampling space in front of the robot's movement. It employs an adaptive density distribution strategy to increase sampling density in near-obstacle areas to enhance safety and optimize distribution in open areas to improve computational efficiency. The generated sampling point set is then sent to the sampling point filtering module. This module evaluates the comprehensive value of each sampling point through a triple cost function to select the optimal local endpoint.

[0178] During the path optimization phase, the dynamic planning waypoint generation module generates the optimal local path from the current position to the local endpoint based on the filtering results of the sampling point filtering module and real-time environmental information, through graph structure modeling and multi-stage decision-making algorithms. The endpoint region judgment module continuously monitors the relative position of the robot and the global endpoint to ensure the consistency between the path planning and the global goal.

[0179] This invention also provides a computer-readable storage medium storing a computer program thereon, the computer program being executed by a processor using the steps of the underwater robot path planning method for marine environments as described in any of the above embodiments.

[0180] This invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the underwater robot path planning method for marine environments as described in any of the above embodiments.

[0181] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.< / xacro:include> < / topic> < / namespace> < / pose> < / include>

Claims

1. A path planning method for underwater robots in marine environments, characterized in that, include: S1. Construct a high-fidelity marine environment simulation platform based on ROS Melodic, Gazebo11, and UUV Simulator; the construction of the high-fidelity marine environment simulation platform includes building a marine environment model, integrating an underwater robot model, and configuring the simulation environment startup file; S2, the global path planning module adopts an improved version. The algorithm searches for feasible paths for the underwater robot from its starting point to its target endpoint within the high-fidelity marine environment simulation platform, obtaining a global reference path. The global path planning module includes an environment modeling unit, a path search unit, a path optimization unit, a constraint processing unit, and a result output unit. The functions of each unit are as follows: The environmental modeling unit integrates terrain elevation information and static obstacle information, and generates a grid map with passage cost through spatial discretization; The constraint processing unit, relying on the grid map with passage costs, transforms the robot's motion capability limitations and environmental navigation rules into specific path search constraints, forming a boundary framework for intelligent search. The path search unit uses an improved The algorithm performs a heuristic search on a constrained grid map, optimizing the traditional 8-way search into a 5-way search. It also employs a dynamic weight evaluation function F(n) = G(n) + (1 + r / R)H(n) to dynamically adjust the weights of heuristic terms based on the relative distance between the current node and the starting point. This guides the search process to gradually expand nodes from the starting point to the destination, ultimately backtracking to generate the original path sequence that satisfies the basic constraints. Here, G(n) represents the actual cost from the starting point to the current node, H(n) represents the estimated cost of the Manhattan distance from the current node to the target point, r represents the distance from the current node to the target point, and R represents the total distance from the starting point to the target point. The path optimization unit performs fine-grained optimization on the original path obtained by the path search unit, including smoothing the path trajectory and optimizing the path data structure to remove redundancy and improve path quality. The output unit normalizes and integrates the optimized path, generating and outputting a complete global reference path data package containing a three-dimensional spatial coordinate sequence and related navigation attribute information; S3. Guided by the global reference path, the local path planning module obtains the local optimal path through a multi-stage processing flow. S4. Under the global-local collaboration framework, the optimal global path is obtained through a hierarchical intelligent collaboration mechanism.

2. The method according to claim 1, characterized in that, The high-fidelity marine environment simulation platform built based on ROS Melodic, Gazebo11, and UUVSimulator includes: Build a marine environment model: Load a custom underwater scene model in the physical simulation environment, import a digital elevation model, configure the ocean current plugin and wave field plugin for simulating the marine dynamic environment, and enable underwater light attenuation; Integrated underwater robot simulation model: An underwater robot simulation model is built based on a general robot description file, and an inertial measurement unit, Doppler velocimeter, and three-dimensional acoustic environment perception sensor are extended and integrated; the data generated by each sensor simulation is published through the preset independent communication interface in the high-fidelity marine environment simulation platform; Write a startup configuration file to initialize the simulation environment and integrate and load the core functions: initialize the initial spatial pose of the underwater robot, build the physical simulation environment and generate the corresponding robot dynamics model; load the motion controller to realize the closed-loop control of the robot, start the path planning function to publish the target destination information and generate the corresponding visual path; simultaneously open the pre-configured visual monitoring interface and integrate the data recording function to store sensor data and key result data generated by path planning in real time.

3. The method according to claim 1, characterized in that, The local path planning module includes a ROS data subscription module, an obstacle handling module, a global pathpoint extraction module, a local-global endpoint calculation module, a sampling space generation module, a sampling point filtering module, an endpoint region judgment module, and a dynamic planning waypoint generation module; the multi-stage processing flow includes three key stages: environmental perception, intelligent decision-making, and path optimization; guided by the global reference path, the local path planning module obtains the optimal local path through a multi-stage processing flow, including: In the process perception phase, the ROS data subscription module acquires raw data from sonar sensors, robot real-time pose, global reference path, and navigation start and end point coordinates in real time through a topic subscription mechanism. This raw data is then standardized and distributed in parallel to the obstacle processing module and the global path point extraction module. The obstacle processing module receives sonar sensor data and robot pose, converts the local sonar sensor data into an obstacle distribution map in the global coordinate system using a coordinate transformation algorithm, assesses the threat level of each obstacle, and outputs structured environmental information with area markers and hazard levels. The global path point extraction module analyzes the robot's relative position to the global path, determines the path tracking progress through nearest neighbor search, and calculates a suitable local planning reference length for the current environment. In the intelligent decision-making phase, the local and global endpoint calculation module, based on the outputs of the obstacle processing module and the global path point extraction module, intelligently extracts suitable phased target points for local planning from the global reference path. The sampling space generation module, guided by the phased target points, constructs a three-dimensional discrete sampling space in front of the robot's movement. It adopts an adaptive density distribution strategy to increase sampling density in the near-obstacle area to enhance safety and optimize the distribution in the open area to improve computational efficiency. The generated sampling point set is sent to the sampling point filtering module. The sampling point filtering module evaluates the comprehensive value of each sampling point through a triple cost function and selects the optimal local endpoint. During the path optimization phase, the dynamic planning waypoint generation module generates the optimal local path from the current position to the local endpoint based on the filtering results of the sampling point filtering module and real-time environmental information; the endpoint region judgment module continuously monitors the relative position of the robot and the global endpoint to ensure the consistency between the path planning and the global goal.

4. The method according to claim 3, characterized in that, The sampling point filtering module includes a cost calculation engine, a cost normalization processor, a weight configuration unit, and a comprehensive evaluation and sorting unit. The sampling point selection module evaluates the comprehensive value of each sampling point through a triple cost function, and selects the optimal local endpoint, including: The cost calculation engine simultaneously calculates the triple cost for each sampling point generated by the sampling space generation module. The normalization processor obtains the triple cost value for each sampling point through mapping. The triple cost function includes obstacle avoidance cost, target approach cost, and path smoothing cost. The cost of obstacle avoidance is In the formula, The distances between discrete sampling points and obstacles are represented as follows: d1 represents the danger distance, and d2 represents the critical safety distance. The cost of target convergence is In the formula, This is expressed as the distance between discrete sampling points and the target endpoint. This represents the farthest distance between the current planning starting point and the local / global endpoint; The cost of path smoothing is In the formula, This is expressed as the distance between a discrete sampling point and the current planning starting point. This represents the maximum distance of a discrete sampling point from the current planning starting point; Construct a judgment matrix and calculate the consistency ratio. , CR For consistency ratio, when CR When <0.1, the consistency of the matrix is ​​acceptable, where CI Indicates consistency index and , RI This represents the average random consistency index, where n represents the order of the judgment matrix. This represents the largest eigenvalue of the judgment matrix; The weights are obtained by normalizing the judgment matrix column-wise and then taking the arithmetic mean of the rows. , , The final expression for the total cost function is obtained. The comprehensive evaluator calculates the total cost value of each sample point based on the total cost function expression; the sorter sorts the total costs of all sample points in ascending order to obtain the local optimal waypoint.

5. The method according to claim 1, characterized in that, The global-local collaborative framework includes a perception fusion layer, a global planning layer, a local planning layer, a control execution layer, and a collaborative management layer. Under this framework, obtaining the globally optimal path through a hierarchical intelligent collaboration mechanism includes: The high-fidelity simulation platform provides a near-realistic testing environment for the entire planning system; The perception fusion layer constructs an accurate environmental cognition model by integrating multi-source sensor data, providing a real and reliable environmental perception foundation for the entire planning system. The global planning layer runs the global path planning module based on the environmental perception provided by the prior environment and the perception fusion layer, and generates a global reference path that comprehensively considers path length, safety and energy efficiency. The local planning layer runs the local path planning module within the guidance framework of the global reference path. The local planning layer receives real-time environmental data provided by the perception fusion layer, detects dynamic obstacles and environmental changes that were not anticipated in the global planning, and generates a safe and smooth local path trajectory within milliseconds. The control execution layer transforms the local path trajectory generated by the local planning layer into precise motion control commands, driving the underwater robot in the high-fidelity simulation platform to perform motion, and feeding back the execution status to the perception fusion layer in real time to form a complete control closed loop. The management team continuously monitors operations at all levels and dynamically coordinates work pace and resource allocation at each level based on environmental changes and task requirements.

6. A path planning system for underwater robots in marine environments, used to implement the method described in any one of claims 1-5, characterized in that, include: High-fidelity simulation platform: A simulation test platform used to provide a near-real marine environment, simulating water flow, water depth, obstacle environmental factors, and verifying and testing the performance of the entire path planning system; Global Path Planning Module: The core component of the hierarchical planning architecture, used to receive environmental maps and navigation task inputs, and output a global reference path that takes into account path length, safety and executability; Local path planning module: includes ROS data subscription module, obstacle handling module, global path point extraction module, local and global endpoint calculation module, sampling space generation module, sampling point filtering module, endpoint region judgment module, and dynamic planning waypoint generation module. These modules work together to form an efficient and collaborative planning closed-loop system. Under the guidance of the global reference path, it generates a safe, smooth, and locally optimal path that conforms to robot dynamics based on real-time perceived environmental information. ROS Subscription Module: Used to subscribe to ROS topics, collect real-time data including sonar sensor data, robot real-time pose information, global navigation start coordinates, and global navigation end coordinates, and transmit the received data to the obstacle processing module and the global path point extraction module; Obstacle processing module: Used to process obstacle information obtained from sensors or simulation platforms, generate obstacle position and velocity information, and provide environmental perception data for local path planning; Global path point extraction module: used to extract the sequence of local path points related to the current robot position from the global path, and to determine the reference frame and spatial range of the local planning; The Local / Global Endpoint Calculation Module is used to intelligently extract the phased target points of the local endpoint planning from the global path. Sampling space generation module: used to construct a three-dimensional discrete sampling space in front of the robot's movement that meets the robot's reachability requirements; Sampling point filtering module: used to quantitatively evaluate each candidate point in the sampling space and filter out the local endpoint with the best comprehensive cost value; Endpoint Region Judgment Module: Used to determine whether the current local endpoint has reached the stage target point; The dynamic programming reference waypoint module is used to transform the selected optimal local endpoints into an executable sequence of waypoints. Through curve fitting and dynamic optimization, it generates a smooth and continuous locally optimal path, ensuring that the control execution layer can accurately track the path.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the underwater robot path planning method for a marine environment as described in any one of claims 1-5.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of a path planning method for an underwater robot in a marine environment as described in any one of claims 1-5.

Images