Autonomous obstacle avoidance and path planning system for underwater robots based on multimodal sensors

Abstract

The present invention discloses an autonomous obstacle avoidance and path planning system for underwater robots based on multimodal sensors, which specifically relates to the field of underwater robot navigation and solves the problems of multimodal perception and path planning in complex environments. It unifies the feature space of multi-source heterogeneous sensors based on a physical field correction method based on the law of acoustic and optical fluctuations, overcomes the fusion deviation of sonar and visual data caused by differences in physical properties, and significantly improves the obstacle representation accuracy and adaptability to dynamic environments. The dynamic confidence decision mechanism is combined with a path curvature-torque constraint model to synchronously integrate the environmental perception credibility and the robot's motion performance into the path generation process, while avoiding the risk of geometric collisions. The feasibility of motion control is ensured, and the contradiction between safety and executability in path planning is resolved. Global path optimization eliminates the hidden dangers of local path mutations through iterative smoothing under the dual constraints of physical fields and dynamics, forming an optimal navigation solution that takes into account obstacle avoidance efficiency, energy consumption and motion stability.

Description

Technical Field

[0001] The present invention relates to the field of underwater robot navigation, and more particularly to an underwater robot autonomous obstacle avoidance and path planning system based on a multimodal sensor. Background Art

[0002] Current autonomous obstacle avoidance systems for underwater robots generally utilize multimodal sensor fusion technology, leveraging the complementary capabilities of heterogeneous data such as sonar and vision to enhance perception in complex environments. Sonar, with its strong penetrating power, captures the geometric outlines of obstacles, while vision, relying on high-resolution imaging, interprets surface semantic details. The combination of these two can theoretically construct a complete representation of the three-dimensional environment, providing a basis for precise decision-making in path planning.

[0003] However, existing systems fail to fully deconstruct the physical mechanisms of acoustic and optical sensing, and instead employ simple data-level fusion strategies, resulting in inherent cognitive biases in cross-modal feature space. Differences in the physical laws governing the propagation attenuation, interface scattering, and motion perception of sound and light waves in water media result in distinct physical representations of the same obstacle in sonar point clouds and visual images. Traditional fusion methods, lacking physical field coupling modeling, are unable to eliminate cross-scale feature repulsion and spatiotemporal misalignment, severely limiting the accuracy of obstacle avoidance decisions and the efficiency of path planning.

[0004] In order to solve the above problems, a technical solution is now provided. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, an embodiment of the present invention provides an autonomous obstacle avoidance and path planning system for underwater robots based on multimodal sensors. The physical field correction method based on the law of acoustic and optical fluctuations unifies the feature space of multi-source heterogeneous sensors, overcomes the fusion deviation of sonar and visual data caused by differences in physical properties, and significantly improves the obstacle characterization accuracy and adaptability to dynamic environments; the dynamic confidence decision mechanism is combined with the path curvature-torque constraint model to simultaneously integrate the environmental perception credibility and robot motion performance into the path generation process, while avoiding the risk of geometric collisions and ensuring the feasibility of motion control, solving the contradiction between safety and executability in path planning; global path optimization eliminates the hidden dangers of local path mutations through iterative smoothing under the dual constraints of physical fields and dynamics, forming an optimal navigation solution that takes into account obstacle avoidance efficiency, energy consumption and motion stability, so as to solve the problems raised in the above-mentioned background technology.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] Data acquisition module, physical correction module, feature fusion module, confidence assessment module, path planning module and path optimization module;

[0008] Data acquisition module: Synchronously acquires sonar point cloud and binocular vision RGB-D data and divides them into alignment processing units based on a unified spatiotemporal reference;

[0009] Physical correction module: solves the Navier-Stokes equations for the sonar data of each alignment processing unit to compensate for sound wave attenuation, solves the Maxwell equations for the visual data to correct light wave scattering, and outputs a physical field correction data set;

[0010] Feature fusion module: Input the physical field correction dataset into the adversarial generative network based on acoustic-optical fluctuation constraints to obtain the sonar-vision fusion feature matrix;

[0011] Confidence Assessment Module: Calculates the quantitative values ​​of sonar penetration and visual resolution based on the sonar-vision fusion feature matrix, and obtains confidence parameters through comprehensive analysis based on obstacle motion parameters.

[0012] Path planning module: This module divides sensitive areas based on confidence parameters, embeds the curvature-moment mapping function into the A* algorithm, and iteratively adjusts the curvature of path nodes to within the moment threshold to obtain candidate paths.

[0013] Path optimization module: Verifies the curvature continuity of candidate paths and eliminates torque mutations, and outputs a global obstacle avoidance path.

[0014] In a preferred embodiment, the processing of the data acquisition module includes the following:

[0015] The sonar sensor generates three-dimensional sonar point cloud data reflecting the geometric outline of the obstacle, and the binocular vision sensor generates binocular vision RGB-D data containing color information and depth information. A time synchronization mechanism is used to align the sonar point cloud data and binocular vision RGB-D data to a unified time base, ensuring that the sonar point cloud data and binocular vision RGB-D data reflect the underwater environment status at the same moment. The binocular vision RGB-D data is converted from the camera coordinate system to the robot coordinate system to achieve spatial alignment with the sonar point cloud data. The continuous sonar point cloud data stream and binocular vision RGB-D data stream are divided into discrete processing units at fixed time intervals. Each processing unit contains a time-synchronized and spatially aligned subset of sonar point cloud data and binocular vision RGB-D data. The sonar point cloud data subset is downsampled to reduce data redundancy, and the binocular vision RGB-D data subset is image segmented to extract obstacle boundary information.

[0016] In a preferred embodiment, the processing of the physical correction module includes the following:

[0017] For sonar point cloud data, by simulating the propagation process of sound waves in fluid media, the pressure field distribution during the sound wave propagation process is calculated, the attenuation coefficient of each position is determined, and the density value of each point in the sonar point cloud is adjusted according to the attenuation coefficient and the distance from the point to the sonar sensor to generate a corrected sonar point cloud. Subsequently, the corrected sonar point cloud is clustered and surface reconstructed to generate a sonar geometry set that reflects the geometric outline of the obstacle.

[0018] In a preferred embodiment, the processing of the physical correction module further includes the following:

[0019] For visual data, by simulating the propagation and scattering behavior of light waves in inhomogeneous media, the propagation path and scattering intensity distribution of light waves are calculated, the image distortion coefficient and depth error value caused by scattering are determined, and the pixel value of the RGB image is adjusted according to the image distortion coefficient to enhance the image clarity. The depth value of the depth image is adjusted according to the depth error value to generate corrected visual data. Subsequently, semantic segmentation and feature extraction are performed on the corrected visual data to generate a visual semantic set that reflects the surface details and semantic information of the obstacle. Finally, the sonar geometry set and the visual semantic set are integrated into a physical field correction dataset.

[0020] In a preferred embodiment, the processing of the feature fusion module includes the following:

[0021] The physical field correction dataset is input into the adversarial generative network, which consists of a generator and a discriminator. The generator receives the sonar geometry set and the visual semantic set, extracts the deep features of the sonar geometry set and the visual semantic set through a convolutional neural network, and uses the attention mechanism to perform weighted fusion of the deep features to generate a sonar-vision fusion feature matrix. The discriminator evaluates the difference between the sonar-vision fusion feature matrix and the true feature matrix.

[0022] In a preferred embodiment, the processing of the feature fusion module further includes the following:

[0023] During the training process of the adversarial generative network, the generator's loss function consists of the adversarial loss and the acoustic-optical wave constraint loss. The acoustic-optical wave constraint loss is calculated based on the residual of the acoustic wave equation and the optical wave equation. The adversarial loss measures the probability that the sonar-vision fusion feature matrix is ​​identified as a generated feature by the discriminator; the discriminator's loss function is used to optimize the ability to distinguish between the real feature matrix and the sonar-vision fusion feature matrix; after training is completed, the sonar-vision fusion feature matrix is ​​output.

[0024] In a preferred embodiment, the processing of the confidence assessment module includes the following:

[0025] Extract sonar-related feature subsets from the sonar-vision fusion feature matrix, calculate the overall response strength of the sonar feature subsets, and combine them with the attenuation coefficient to generate a quantitative value of sonar penetration capability;

[0026] Extracting a subset of visual features from the sonar-vision fusion feature matrix, calculating the overall gradient strength of the visual feature subset, and combining it with the light wave scattering distortion coefficient to generate a quantitative value of visual resolution.

[0027] Extract the obstacle's motion speed and acceleration from the sonar-vision fusion feature matrix to generate obstacle motion parameters;

[0028] The quantitative value of sonar penetration ability, the quantitative value of visual resolution ability and the obstacle motion parameters are combined into a feature vector and input into a pre-trained logistic regression model. The feature vector is weighted and summed using the weight vector and bias value of the logistic regression model and mapped to a value between 0 and 1 to generate a confidence parameter.

[0029] In a preferred embodiment, the processing of the path planning module includes the following:

[0030] Sensitive areas are divided according to confidence parameters, and the environmental space is divided into grid cells. The radius of the sensitive area is calculated according to the confidence parameter value. The spherical area with the obstacle as the center and the sensitive area radius as the radius is defined as the sensitive area. The grid cells falling into the sensitive area are marked as high-risk areas.

[0031] In a preferred embodiment, the processing of the path planning module further includes the following:

[0032] A curvature-torque mapping function is embedded in the A* algorithm to calculate the curvature and corresponding torque value of the path nodes. Candidate nodes whose torque values ​​exceed the maximum torque threshold are eliminated, and candidate nodes whose torque values ​​are within the maximum torque threshold are retained. After the initial path is generated, the nodes with excessive torque are identified, and local smoothing adjustments are performed on the nodes with excessive torque and their adjacent nodes to reduce the local curvature until the torque values ​​of all nodes are within the maximum torque threshold or the maximum number of iterations is reached, thus generating a candidate path.

[0033] In a preferred embodiment, the processing of the path optimization module includes the following contents:

[0034] The curvature continuity of the candidate path is verified by calculating the local curvature and curvature change rate of each node, identifying the nodes with discontinuous curvature, and adjusting the node positions of these curvature discontinuous nodes using the cubic spline interpolation method until the curvature change rate of all nodes is less than or equal to the set curvature change threshold; the torque mutation in the smooth path is detected by calculating the torque value and torque change rate of each node, identifying the nodes with torque mutation, and adjusting the path segments of these torque mutation nodes using the Bezier curve method until the torque change rate of all nodes is less than or equal to the set torque change threshold, and finally outputting the global obstacle avoidance path.

[0035] The technical effects and advantages of the underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors of the present invention are as follows:

[0036] The present invention constructs a physical field-driven autonomous obstacle avoidance and path planning system through deep coupling of multimodal perception data and robot dynamics, achieving full-link optimized navigation in complex underwater environments. The physical field correction technology based on the law of acoustic and optical fluctuations effectively unifies the feature space of multi-source heterogeneous sensors, overcomes the fusion deviation caused by physical property differences between sonar and visual data in traditional methods, and significantly improves the obstacle characterization accuracy and adaptability to dynamic environments. The dynamic confidence decision mechanism combines the path curvature-torque constraint model to simultaneously integrate the environmental perception credibility and robot motion performance into the path generation process, ensuring the feasibility of motion control while avoiding geometric collision risks, and resolving the contradiction between safety and executability in path planning. Global path optimization eliminates the hidden dangers of local path mutations through iterative smoothing under the dual constraints of physical fields and dynamics, forming an optimal navigation solution that takes into account obstacle avoidance efficiency, energy consumption and motion stability, providing highly robust autonomous navigation capabilities for complex underwater scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 The figure is a schematic structural diagram of the underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors of the present invention.

[0038] Figure 2 The figure is a schematic diagram of the processing flow of the sensitive area division of the underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors of the present invention. DETAILED DESCRIPTION

[0039] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0040] Example 1: Figure 1 The present invention provides an underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors, including: a data acquisition module, a physical correction module, a feature fusion module, a confidence assessment module, a path planning module and a path optimization module;

[0041] Data acquisition module: Synchronously acquires sonar point cloud and binocular vision RGB-D data, and divides them into alignment processing units based on a unified spatiotemporal reference.

[0042] Physical correction module: solves the Navier-Stokes equations for the sonar data of each aligned processing unit to compensate for the sound wave attenuation, solves the Maxwell equations for the visual data to correct the light wave scattering, and outputs the physical field correction data set.

[0043] Feature fusion module: The physical field correction dataset is input into the adversarial generative network based on acoustic-optical fluctuation constraints to obtain the sonar-vision fusion feature matrix.

[0044] Confidence Assessment Module: Calculates the quantitative values ​​of sonar penetration and visual resolution based on the sonar-vision fusion feature matrix, and obtains confidence parameters through comprehensive analysis based on obstacle motion parameters.

[0045] Path planning module: The sensitive area is divided according to the confidence parameter, the curvature-moment mapping function is embedded in the A* algorithm, and the curvature of the path node is iteratively adjusted to within the moment threshold to obtain the candidate path.

[0046] Path optimization module: Verifies the curvature continuity of candidate paths and eliminates torque mutations, and outputs a global obstacle avoidance path.

[0047] In the autonomous obstacle avoidance and path planning execution of underwater robots, sonar sensors and binocular vision sensors are two core perception methods. Sonar sensors can obtain geometric contour information of obstacles by penetrating water bodies with sound waves, but their resolution is low and they are easily affected by noise. Binocular vision sensors capture high-resolution surface details and semantic information through light waves. However, due to the influence of underwater light scattering and turbidity, their perception ability may decrease in complex environments. Against this background, the present invention proposes an underwater robot perception and path planning method based on multimodal sensor fusion. Through the coordinated processing of sonar point cloud and binocular vision RGB-D data, combined with physical field coupling correction and dynamic constraints, the obstacle avoidance accuracy and path reliability of underwater robots in complex environments are improved.

[0048] The purpose of the data acquisition module is to synchronously acquire the point cloud data generated by the sonar sensor and the RGB-D data generated by the binocular vision sensor, and divide these data into aligned processing units based on a unified time and space reference, thereby providing a reliable data foundation for subsequent physical field correction and feature fusion.

[0049] The following is a specific implementation of the data acquisition module:

[0050] The process for collecting sonar point cloud data is as follows: The sonar sensor detects the location and shape of obstacles by emitting sound waves and receiving signals reflected from underwater obstacles, generating three-dimensional point cloud data reflecting the obstacle's geometric outline. This 3D point cloud data consists of multiple spatial coordinate points, each representing the position of a specific obstacle surface in the underwater environment. The coordinates are expressed using the robot coordinate system, centered on the robot itself.

[0051] The binocular vision RGB-D data acquisition process is as follows: The binocular vision sensor uses two cameras to simultaneously capture images of the underwater environment, generating RGB-D data containing both color and depth information. The color information portion is a color image, providing the color and texture characteristics of obstacles; the depth information portion is a depth image, providing the distance of each pixel relative to the binocular vision sensor. The initial reference frame for this data is the binocular vision sensor's camera coordinate system.

[0052] The implementation process of the time synchronization mechanism is as follows: Because the sonar sensor and binocular vision sensor have different sampling frequencies and the timestamps of the data they generate may differ, the sonar point cloud data and binocular vision RGB-D data need to be aligned to a unified time point. First, a unified time base is determined, and then the data is adjusted through interpolation methods to align the sampling timestamps of the two types of data to the time point closest to the unified time base. Specifically, for each sensor's data, the data sampling time closest to the unified time base time point is calculated based on the order of the timestamps. The sonar point cloud data and binocular vision RGB-D data corresponding to this time point are output as the synchronized data, ensuring that the two types of data reflect the state of the underwater environment at the same time.

[0053] The temporal partitioning process is as follows: the continuous data stream formed by sonar point cloud data and binocular RGB-D data is divided into multiple discrete processing units at fixed time intervals. Each processing unit corresponds to a specific time point and contains a subset of the sonar point cloud data and binocular RGB-D data at that time point. The time interval is determined by the robot's movement speed and the complexity of the underwater environment, and is typically set between 0.1 and 1 second to ensure that the data partitioning reflects dynamic changes in the environment and facilitates subsequent processing.

[0054] The spatial coordinate conversion process is as follows: To achieve spatial alignment between the sonar point cloud data and the binocular RGB-D data, both data types are converted to the robot coordinate system. The sonar point cloud data is natively represented in the robot coordinate system, so no coordinate conversion is required. The binocular RGB-D data is initially represented in the camera coordinate system. The depth information in the binocular RGB-D data is converted to 3D point coordinates in the robot coordinate system using a pre-calibrated rotation matrix and translation vector.

[0055] The specific conversion process is as follows: for each pixel point in the depth image, the three-dimensional coordinates of the pixel point in the robot coordinate system are calculated based on the depth value and the position in the camera coordinate system, combined with the rotation matrix and translation vector, thereby generating three-dimensional point data that is aligned with the sonar point cloud data space.

[0056] The alignment processing unit generation process is as follows: At each selected time point, an alignment processing unit is generated. This alignment processing unit contains the subset of sonar point cloud data and binocular vision RGB-D data at that time point, which have undergone time synchronization and spatial coordinate system conversion. This data is aligned to a unified time base in time and to the robot coordinate system in space, ensuring temporal consistency and spatial comparability of the data.

[0057] The data preprocessing optimization process is as follows: To improve the efficiency of subsequent processing, a subset of the sonar point cloud data is downsampled. Specifically, voxel grid filtering technology is used to divide the point cloud data into multiple voxel grids. Within each voxel grid, key points representing the geometric features of obstacles are retained, while redundant points are removed, thereby reducing the total number of point cloud data. Image segmentation is performed on the subset of binocular vision RGB-D data. By analyzing the color and depth images, boundary information related to obstacles is extracted, background data unrelated to obstacle avoidance is removed, and semantic features meaningful for path planning are retained.

[0058] A time synchronization mechanism aligns sonar point cloud data and binocular RGB-D data to the same point in time, ensuring that both types of data reflect the state of the underwater environment at the same moment. This synchronization process avoids environmental perception errors caused by time deviations, enabling subsequent analysis based on multimodal data fusion to achieve higher accuracy.

[0059] The binocular vision RGB-D data is converted from the camera coordinate system to the robot coordinate system to achieve spatial alignment with the sonar point cloud data. This unified spatial reference frame enables the two types of data to be compared and fused in the same coordinate system, facilitating the subsequent unified representation and feature extraction of underwater obstacles, thereby improving the integrity and consistency of perception results.

[0060] By dividing a continuous data stream into discrete processing units at fixed time intervals, complex dynamic data can be broken down into manageable independent units. This division not only facilitates independent processing of data at each time point, but also maintains the coherence of the data in the time series.

[0061] Downsampling a subset of sonar point cloud data and performing image segmentation on a subset of binocular vision RGB-D data can effectively reduce data redundancy and highlight the geometric and semantic features of obstacles. This reduces computational complexity and improves the speed and efficiency of data processing in complex underwater environments, thereby enhancing the responsiveness of underwater robots.

[0062] The data acquisition module simultaneously collects sonar point cloud data and binocular RGB-D data, and then divides them into alignment processing units based on a unified time and space reference. This completes the initial integration of the underwater robot's multimodal perception data. The generated alignment processing units are temporally aligned to a selected time point and spatially unified to the robot's coordinate system, ensuring temporal consistency and spatial comparability of the data. This result provides a reliable data foundation for subsequent physical field correction and feature fusion, enabling the underwater robot to perform further tasks based on accurate environmental perception.

[0063] In the data acquisition module, sonar point clouds and binocular vision RGB-D data are synchronously acquired and divided into alignment processing units based on a unified spatiotemporal reference, providing a multimodal data foundation for spatiotemporal alignment for subsequent processing. However, due to the propagation attenuation of sound waves in water and the scattering of light waves in underwater environments, sonar point clouds and visual data have perceptual biases in their original state, affecting the accurate representation of obstacles. Therefore, the goal of the physical correction module is to perform physical field correction on the sonar data and visual data in each alignment processing unit to compensate for the attenuation of sound waves in water and the scattering of light waves in underwater environments, thereby outputting a physical field corrected dataset.

[0064] The following is a specific implementation of the physical correction module:

[0065] Sonar data physics correction:

[0066] The process of compensating for acoustic wave attenuation in a sonar point cloud is as follows: When sound waves propagate through a water medium, energy is lost due to absorption and scattering. This results in a reduced point cloud density for distant obstacles, affecting the accurate representation of the obstacle's geometric outline. To address this issue, the Navier-Stokes equations are used to simulate the propagation of sound waves in a fluid medium. Specifically, the Navier-Stokes equations describe the coupling relationship between fluid motion and acoustic wave propagation, and include parameters such as fluid density, velocity field, pressure field, viscosity, and external forces. Numerically solving the Navier-Stokes equations calculates the pressure field distribution at different locations during acoustic wave propagation, thereby determining the acoustic wave attenuation coefficient at each location. The attenuation coefficient reflects the degree to which the acoustic wave energy is attenuated with increasing distance. For each point in the sonar point cloud, the corresponding attenuation coefficient is calculated based on its position in the robot coordinate system and its distance from the sonar sensor. The density value at that point is then adjusted using an inverse compensation method.

[0067] The specific compensation method is to multiply the original density value of the point by an exponential factor based on the attenuation coefficient and distance, thereby increasing the density of distant points and generating a corrected sonar point cloud. This process ensures that the sonar point cloud can more accurately reflect the geometric outline of the obstacle. An example of physical field correction of sonar data is as follows:

[0068] When sound waves propagate in water, they are attenuated due to absorption and scattering, resulting in sonar point clouds. The point cloud density of obstacles at medium and long distances decreases, affecting the accuracy of geometric contour representation. To compensate for this attenuation, the Navier-Stokes equations are used to simulate the sound wave propagation process, calculate energy loss, and adjust the point cloud density. The specific steps are as follows:

[0069] The underwater environment is divided into multiple fluid units, and the coupling relationship between fluid motion and acoustic wave propagation is described based on the Navier-Stokes equations:

[0070] ;

[0071] in, represents the fluid density, represents the fluid velocity field, represents the pressure field, represents the fluid viscosity, represents the external force term (including the pressure disturbance caused by the sound wave).

[0072] By numerically solving the Navier-Stokes equations, the pressure field distribution during sound wave propagation is calculated. , and then determine the attenuation coefficient at each position .

[0073] Sonar point cloud Each point in , calculate the corresponding attenuation coefficient according to its position , adjust the point cloud density through the inverse compensation formula:

[0074] ;

[0075] in, Indicates a point The distance to the sonar sensor, To attenuate the compensation factor, ensure that the long-distance point cloud data is enhanced and generate the corrected sonar point cloud .

[0076] Represents the index of a single point in the sonar point cloud; Represents the time point index in the time series, used to identify the specific time base corresponding to the alignment processing unit.

[0077] Sonar geometry set generation:

[0078] The sonar geometry set generation process is as follows: the rectified sonar point cloud is clustered, grouping the point cloud data by spatial proximity to form point cloud clusters representing different obstacles. Subsequently, surface reconstruction techniques are used to fit each point cloud cluster into a continuous geometric surface, generating structured data reflecting the geometric outline of the obstacle, called the sonar geometry set. This geometry set preserves the 3D shape information of the obstacle and provides reliable geometric feature input for feature fusion in the subsequent feature fusion module.

[0079] Physical field correction of visual data:

[0080] The light wave scattering correction process for visual data is as follows: Light waves are scattered by suspended particles and turbidity in underwater environments, resulting in blurry RGB images and errors in depth values ​​in depth images. To correct for scattering, Maxwell's equations are used to simulate the propagation and scattering of light waves in inhomogeneous media. Maxwell's equations describe the electric and magnetic field distributions of light waves, including parameters such as electric field intensity, magnetic induction intensity, magnetic field intensity, electric displacement, and current density. Numerical methods are used to solve Maxwell's equations to calculate the propagation path and scattering intensity distribution of light waves in underwater environments, and to determine the image distortion coefficient and depth error caused by scattering. The image distortion coefficient reflects the pixel brightness attenuation caused by light wave scattering, while the depth error reflects the interference of scattering on depth measurement. For RGB images, a dehazing factor is calculated based on the image distortion coefficient. The dehazing factor is the complement of the distortion coefficient and is used to enhance pixel brightness. Specifically, the original RGB value of each pixel is divided by the corresponding dehazing factor to restore image clarity. For the depth image, the depth value of each pixel is adjusted based on the depth error value. The specific method is to subtract the corresponding depth error value from the original depth value to generate a corrected depth image. The corrected RGB image and depth image are combined to form the corrected visual data. This process significantly improves the clarity and accuracy of visual data in complex underwater environments. The following is an example of physical field correction of visual data:

[0081] Light waves are scattered underwater by suspended particles and turbidity, resulting in visual data To correct for the scattering effect, we use Maxwell's equations to simulate the light wave propagation and scattering process. The specific steps are as follows:

[0082] The underwater environment is modeled as an inhomogeneous medium, and the electric and magnetic field distributions of light waves are described based on Maxwell's equations:

[0083] ;

[0084] in, represents the electric field strength, represents the magnetic induction intensity, represents the magnetic field strength, represents the electric displacement, Represents the current density.

[0085] By numerically solving Maxwell's equations, the light wave propagation path and scattering intensity distribution are calculated, and the image distortion coefficient caused by scattering is determined and depth error . For visual data Make corrections:

[0086] For RGB images, the dehazing factor is calculated based on the scattering model. , adjust the pixel value:

[0087] ;

[0088] For the depth image, according to the depth error Adjust the Depth value:

[0089] ;

[0090] After integration, the rectified visual data is generated .

[0091] Visual semantic set generation:

[0092] The process for generating a visual semantic set is as follows: semantic segmentation is performed on the rectified visual data. By analyzing the color and texture features of the RGB image and the spatial information of the depth image, the image is divided into different semantic regions, such as obstacle regions and background regions. Subsequently, features of each semantic region are extracted, including the surface texture, color distribution, and boundary shape of the obstacle. This generates structured data reflecting the surface details and semantic information of the obstacle, called the visual semantic set. This semantic set provides rich semantic feature input for feature fusion in the subsequent feature fusion module.

[0093] The physical field correction dataset is generated as follows: the sonar geometry set and the visual semantic set are integrated into a unified dataset, called the physical field correction dataset. This dataset contains obstacle geometry information compensated for acoustic attenuation and obstacle semantic information corrected for light scattering, providing accurate and complementary perception data for the subsequent sonar-visual feature fusion module.

[0094] By solving the Navier-Stokes equations to simulate sound wave propagation and calculate the attenuation coefficient, the long-range point density of the sonar point cloud is effectively enhanced. This processing eliminates the geometric contour distortion caused by sound wave attenuation, allowing the sonar point cloud to more accurately reflect the three-dimensional shape of obstacles.

[0095] By clustering and surface reconstruction to generate sonar geometry sets, discrete point cloud data is converted into structured geometric representations. This process preserves the core shape features of obstacles, reduces noise interference, and improves the usability of geometric information.

[0096] By solving Maxwell's equations to calculate the distortion coefficient and depth error caused by scattering, the clarity of RGB images and the accuracy of depth images are significantly improved. This processing effectively alleviates the interference of underwater turbidity and suspended particles on visual data, allowing the visual data to more realistically reflect the surface details and spatial location of obstacles.

[0097] Through semantic segmentation and feature extraction, a visual semantic set is generated, extracting semantic information such as the texture, color, and boundaries of obstacles. This process simplifies the complex information in visual data into structured semantic features, facilitating cross-modal fusion with the sonar geometry set and improving the completeness of obstacle representation.

[0098] The physical correction module generates a physical field correction dataset by solving the Navier-Stokes equations on the sonar point cloud to compensate for acoustic attenuation and the Maxwell equations on the visual data to correct for light scattering. This dataset contains accurate obstacle geometry and semantic information, eliminating perception bias caused by acoustic and light wave propagation in water. This provides high-quality data input for the feature fusion module's sonar-visual feature fusion, ensuring the underwater robot's perception accuracy in complex environments.

[0099] In the physical correction module, the Navier-Stokes equations are solved for sonar data to compensate for acoustic attenuation, and the Maxwell equations are solved for visual data to correct for light scattering. This generates a physical field correction dataset, providing an accurate perception foundation for subsequent processing. However, due to the heterogeneity of the sonar geometry set and the visual semantic set in feature space, direct fusion may result in information loss or redundancy. Therefore, the feature fusion module uses specific technical means to map the sonar geometry set and the visual semantic set into a unified feature space to generate a sonar-visual fusion feature matrix. The goal of the feature fusion module is to input the physical field correction dataset output by the physical correction module into a generative adversarial network based on acoustic-optical wave constraints to generate a sonar-visual fusion feature matrix.

[0100] The following is a specific implementation of the feature fusion module:

[0101] The processing logic of the adversarial generation network architecture:

[0102] The generative adversarial network used in the feature fusion module consists of two parts: a generator and a discriminator. The generator's task is to fuse the sonar geometry set and the visual semantic set in the physical field correction dataset output by the physical correction module to generate a sonar-visual fusion feature matrix. Specifically, the generator receives the sonar geometry set and the visual semantic set as input, extracts and integrates their features through its internal neural network structure, and ultimately outputs a sonar-visual fusion feature matrix. The discriminator's task is to evaluate whether the sonar-visual fusion feature matrix output by the generator truly reflects the comprehensive characteristics of the obstacle. The discriminator receives as input either the sonar-visual fusion feature matrix or the real feature matrix obtained from annotated data. By comparing the feature distributions of the two, it outputs a discriminant result, which is used to distinguish between the real feature matrix and the generated feature matrix.

[0103] Processing logic of acoustic and optical wave constraints:

[0104] To ensure that the sonar-vision fusion feature matrix conforms to the physical propagation laws of sound and light waves in feature space, an acoustic-optical wave constraint loss function is designed. This loss function quantifies the degree of deviation of the sonar-vision fusion feature matrix from the acoustic and optical wave equations, ensuring the physical consistency of the feature matrix output by the generator. In its implementation, the residual of the sonar-vision fusion feature matrix in the acoustic wave equation is first calculated, measuring the degree of deviation by comparing the sound pressure distribution of the feature matrix with the distribution predicted by the acoustic wave equation. The residual of the feature matrix in the optical wave equation is then calculated, measuring the degree of deviation by comparing the electric field distribution of the feature matrix with the distribution predicted by the optical wave equation.

[0105] Network training and loss function processing logic:

[0106] The training process of the generator and discriminator uses an alternating optimization approach to achieve high-quality generation of sonar-vision fusion feature matrices. The training goal of the generator is to generate realistic sonar-vision fusion feature matrices that can deceive the discriminator while satisfying the acoustic-optical wave constraint. The generator's loss function consists of two parts: the first is an adversarial loss, which measures the probability that the discriminator identifies the sonar-vision fusion feature matrix as a generated feature. This is quantified by calculating the difference between the discriminator output and the expected value. The second is a wave constraint loss, which measures the degree of deviation of the sonar-vision fusion feature matrix from the acoustic and optical wave equations. This is quantified by calculating the sum of the squares of the residuals. These two losses are weighted and summed using a preset weight coefficient. The training goal of the discriminator is to accurately distinguish between the true feature matrix and the generated feature matrix. Its loss function consists of two terms: a recognition loss for the true feature matrix, quantified by calculating the difference between the discriminator output and the true label; and a recognition loss for the generated feature matrix, quantified by calculating the difference between the discriminator output and the generated label. The two losses are summed to optimize the discriminator parameters.

[0107] Feature fusion and output processing logic:

[0108] The generator uses a convolutional neural network to extract deep features from both the sonar geometry set and the visual semantic set. Specifically, the convolutional neural network performs multi-layer convolution and pooling operations on the sonar geometry set to extract the geometric outline features of obstacles; similar operations are performed on the visual semantic set to extract the surface detail features of obstacles. After extraction, an attention mechanism is used to perform a weighted fusion of the features from the sonar geometry set and the visual semantic set. This mechanism dynamically adjusts the contribution weights of the two sets based on feature importance. The attention mechanism calculates and normalizes the correlation score for each feature channel to generate a weighting coefficient. The features from the sonar geometry set and the visual semantic set are then summed according to the weighting coefficients to generate a sonar-visual fusion feature matrix. After training, the adversarial generative network outputs the sonar-visual fusion feature matrix, which simultaneously represents the geometric outlines and surface details of obstacles in a unified feature space, ensuring physical consistency through the constraints of acoustic and optical fluctuations.

[0109] The feature fusion module generates a sonar-vision fusion feature matrix by inputting the physical field correction dataset output by the physical correction module into a generative adversarial network based on acoustic-optical wave constraints. This process utilizes the generator to fuse features from the sonar geometric set and the visual semantic set. This process then combines adversarial training of the discriminator with acoustic-optical wave constraints to optimize the fusion effect. The resulting sonar-vision fusion feature matrix represents the geometric outlines and surface details of obstacles in a unified feature space, conforming to the propagation laws of both acoustic and optical waves.

[0110] In the feature fusion module, the sonar geometry set and the visual semantic set are fused into a sonar-visual fusion feature matrix through a generative adversarial network based on acoustic-optical fluctuation constraints. This matrix represents the geometric outline and surface details of obstacles in a unified feature space and conforms to the laws of acoustic-optical propagation. However, relying solely on the fused feature matrix cannot directly provide reliable guidance for path planning. Quantitative indicators of perception capabilities must be extracted from it to assess the reliability of sonar and visual data in the current environment. Therefore, the confidence assessment module aims to calculate quantitative values ​​of sonar penetration and visual resolution from the fused feature matrix, and combine it with obstacle motion parameters to generate confidence parameters to provide decision support for sensitive area delineation and path planning in the path planning module.

[0111] The following is a specific implementation of the confidence assessment module:

[0112] Calculation logic for the quantitative value of sonar penetration capability:

[0113] The calculation process for the sonar penetration quantification begins by extracting a subset of features relevant to the sonar data from the sonar-vision fusion feature matrix. Next, the length of each eigenvector in this feature subset is quantified by calculating the sum of the squares of each eigenvector and taking the square root. The lengths of all eigenvectors are then summed to determine the overall response strength. This overall response strength characterizes the sonar data's ability to perceive the geometric contours of obstacles. This is then combined with the attenuation coefficient, obtained by solving the Navier-Stokes equations in the physical correction module. This coefficient represents the energy loss of sound waves propagating through water. Finally, the sonar penetration quantification is calculated by dividing the overall response strength by a denominator adjusted by the attenuation coefficient, specifically the sum of 1 and the attenuation coefficient. This denominator combines the perceived strength of the sonar data with the attenuation characteristics of sound waves in water to produce a sonar penetration quantification value, which is used to assess the reliability of the sonar data in the current underwater environment.

[0114] The calculation of sonar penetration quantification is based on the impact of water turbidity and obstacle density on sonar data perception. The rationale behind this is that by quantifying sonar penetration, the reliability of sonar data in different underwater environments can be dynamically assessed. This allows for adaptive adjustment of trust in sonar data, particularly in environments with high turbidity or high obstacle density, thereby reducing over-reliance on sonar data.

[0115] Calculation logic of the visual resolution quantification value:

[0116] The calculation process for the visual resolution quantification begins by extracting a subset of features relevant to the visual data from the sonar-vision fusion feature matrix. Next, the rate of change of each eigenvector in this feature subset is quantified. This is done by calculating the spatial or temporal difference of each eigenvector to obtain its rate of change. The square root of the sum of the squares of these rates of change is then taken to determine the intensity of change for each eigenvector. The intensity of change for all eigenvectors is then summed to obtain the overall gradient intensity. This overall gradient intensity represents the clarity of the visual data in perceiving surface details of obstacles. This is then combined with the light scattering distortion coefficient, obtained by solving Maxwell's equations in the physical correction module. This coefficient represents the effect of light scattering in water. Finally, the visual resolution quantification is calculated by dividing the overall gradient intensity by a denominator adjusted by the light scattering distortion coefficient, specifically the sum of 1 and the light scattering distortion coefficient. This calculation combines the visual data's detail perception with the scattering interference of light waves in water to produce a visual resolution quantification value, which is used to assess the reliability of the visual data in the current underwater environment.

[0117] The calculation of the quantified value of visual resolution is based on the impact of water turbidity and lighting conditions on visual data perception. The rationale is that by quantifying visual resolution, the reliability of visual data in different underwater environments can be dynamically assessed. This allows for adaptive adjustment of the level of trust in visual data, particularly in environments with severe light scattering or high water turbidity, thereby reducing over-reliance on visual data.

[0118] Obstacle motion parameter extraction logic:

[0119] The process of extracting obstacle motion parameters begins by analyzing the dynamic characteristics of the obstacle from the sonar-vision fusion feature matrix. First, the obstacle's velocity is generated by calculating the rate of change over time for the sonar-vision fusion feature matrix of consecutive time frames. This is done by dividing the difference between the feature matrices of adjacent time frames by the time interval to obtain the velocity. Next, the obstacle's acceleration is generated by calculating the second-order rate of change over time for the sonar-vision fusion feature matrix of consecutive time frames. This is done by dividing the difference between the velocity of adjacent time frames by the time interval to obtain the acceleration. Finally, the velocity and acceleration are combined to form the obstacle motion parameters, which serve as input data for the subsequent confidence parameter generation. This extraction method accurately captures the dynamic characteristics of the obstacle by analyzing the temporal changes in the feature matrix.

[0120] The extraction of obstacle motion parameters is based on the impact of dynamic changes of obstacles in underwater environments on the reliability of sensor data. By quantifying the motion speed and acceleration, the degree of trust in sensor data can be dynamically adjusted.

[0121] Confidence parameter generation logic:

[0122] The confidence parameter generation process uses a logistic regression approach. It begins by combining the quantified sonar penetration, the quantified visual resolution, and the obstacle motion parameters of velocity and acceleration into a single feature vector. Next, a pre-trained logistic regression model is used to process this feature vector. The method assigns a weight to each component in the feature vector, sums the products of all components and their corresponding weights, and adds a bias value to obtain an intermediate value. This intermediate value is then input into a function that maps the intermediate value to a value between 0 and 1 to generate the confidence parameter. The mapping function's output smoothly transitions from 0 to 1 as the intermediate value increases, indicating the reliability of the fusion of sonar and visual data in the current environment. The model's weights and bias are determined using training data, which includes feature vectors and corresponding confidence labels for different underwater environments. These labels are derived from expert evaluations or simulation experiments. This generation method integrates sonar penetration, visual resolution, and obstacle motion parameters to derive the confidence parameter.

[0123] Confidence parameters are generated using logistic regression, a method that processes multiple input features and generates probabilistic outputs, making it suitable for evaluating the reliability of sonar and visual data fusion. The advantage of this technical feature is that by integrating sonar penetration, visual resolution, and obstacle motion parameters, the confidence parameter dynamically reflects the impact of the current environment on sensor data, providing a precise basis for the delineation of sensitive areas.

[0124] The confidence assessment module calculates quantitative values ​​for sonar penetration and visual resolution from the sonar-vision fusion feature matrix, combines them with obstacle motion parameters, and generates confidence parameters using logistic regression. These confidence parameters comprehensively assess the reliability of sonar and vision data in the current underwater environment, providing precise decision support for the sensitive area demarcation in the path planning module. The technical logic of the confidence assessment module ensures adaptive and robust path planning for underwater robots in complex and dynamic environments. All calculations are based on quantitative assessments, accurately and consistently expressed, and meet the requirements of the invention specification.

[0125] In the confidence assessment module, quantitative values ​​of sonar penetration and visual resolution are extracted from the sonar-vision fusion feature matrix. Combined with obstacle motion parameters, a confidence parameter is calculated using logistic regression. This parameter quantifies the reliability of the sonar and vision data fusion in the current environment. However, confidence parameters alone are insufficient to directly generate feasible obstacle avoidance paths. Sensitive areas must be divided based on environmental complexity, and robot dynamic constraints must be incorporated into path planning to ensure that the path avoids obstacles while meeting the torque limits of the robot's thrusters. Therefore, the path planning module divides sensitive areas based on the confidence parameters output by the confidence assessment module, embeds a curvature-torque mapping function into the A* algorithm, and iteratively adjusts the curvature of path nodes to within the torque threshold to generate candidate paths.

[0126] Example 2: The following is a specific implementation of the path planning module:

[0127] like Figure 2 As shown in the figure, the processing flow of sensitive area division is as follows:

[0128] The sensitive area delineation process begins with gridding the environment based on a confidence parameter. The environment is divided into multiple grid cells, each of which is assigned a confidence parameter value, reflecting the reliability of the sonar and visual data fusion within that area. Higher confidence parameter values ​​indicate greater credibility of the perception data and lower environmental complexity. Lower confidence parameter values ​​indicate greater environmental complexity and require a larger sensitive area to enhance obstacle avoidance safety. The sensitive area radius is calculated based on the confidence parameter by dividing a preset baseline radius by the confidence parameter value. When the confidence parameter is close to 1, the sensitive area radius approaches the baseline radius. Lower confidence parameter values ​​increase the sensitive area radius, ensuring more conservative obstacle avoidance for the underwater vehicle in areas with less reliable perception data. For each obstacle location, a sphere centered at the obstacle and with the sensitive area radius as its radius is defined as the sensitive area. All grid cells falling within the sensitive area are marked as high-risk areas, requiring careful attention during path planning to avoid these high-risk areas.

[0129] Sensitive areas are dynamically divided based on confidence parameters, which are derived from the fusion reliability assessment of multimodal sensor data (sonar and vision). Specifically:

[0130] Confidence parameter calculation: The confidence assessment module extracts the quantitative values ​​of sonar penetration and visual resolution from the sonar-vision fusion feature matrix, combines them with obstacle motion parameters, and generates confidence parameters (ranging from 0 to 1) through a logistic regression model.

[0131] Sensitive Area Definition: The path planning module calculates the sensitive area radius based on the confidence parameter. The formula is: base radius divided by the confidence parameter value. When the confidence level is low, the sensitive area radius increases, and vice versa. The sensitive area is defined as a sphere within the radius, centered around the obstacle.

[0132] High-risk area marking: Grid cells falling into sensitive areas are marked as high-risk areas and should be avoided as a priority during route planning.

[0133] In path planning, the A algorithm searches for the optimal path by evaluating the cost of each grid cell. High-risk areas are set to have significantly higher costs than other areas, forcing the algorithm to generate candidate paths that avoid these areas. Sensitive and high-risk areas directly guide candidate path generation, ensuring that paths avoid areas with low perception reliability or dense obstacles, thereby ensuring safety.

[0134] The A* algorithm embeds the processing logic of curvature-moment mapping:

[0135] The A* algorithm searches for the optimal path from a starting point to a destination by minimizing a path cost function, which consists of an actual cost and a heuristic cost estimate. To ensure that the path satisfies the robot's dynamic constraints, the path planning module embeds a curvature-torque mapping function within the A* algorithm. This curvature-torque mapping function correlates the curvature of a path node with the required torque of the robot's thrusters. Specifically, the curvature and its rate of change of the path node are multiplied by preset dynamic model parameters to obtain the torque value. The curvature of a path node is calculated based on the geometric relationship between the node and the nodes before and after it, reflecting the degree of path curvature. The rate of change of curvature is calculated based on the change in path curvature over time, reflecting the robot's dynamic response requirements. A preset maximum torque threshold for the robot's thrusters is set; path nodes exceeding this threshold are considered infeasible. During the path extension process of the A* algorithm, for each candidate node, its curvature and corresponding moment value are calculated. If the moment value exceeds the maximum moment threshold, the candidate node is marked as infeasible and no further path extension is performed; if the moment value is within the maximum moment threshold, the candidate node is retained and path extension continues.

[0136] Because traditional A* algorithms only consider geometric path costs and fail to account for the robot's dynamic constraints, they can generate paths that the robot cannot execute. By linking curvature with torque, dynamically infeasible path nodes are directly eliminated during the path planning phase, ensuring that the generated path meets the robot's motion capabilities. This improves the practicality and safety of path planning, avoids robot loss of control or thruster overload caused by excessive path curvature, and enhances the underwater robot's autonomous navigation capabilities in complex environments.

[0137] The processing logic for iteratively adjusting the curvature of path nodes:

[0138] The process of iteratively adjusting the curvature of path nodes starts with generating an initial path using the standard A* algorithm. At this time, only the grid cost and sensitive area obstacle avoidance are considered, and no curvature constraints are imposed. Next, for each node in the initial path, its curvature and corresponding torque value are calculated, and nodes whose torque values ​​exceed the maximum torque threshold are identified, which are called torque-exceeding nodes. For each torque-exceeding node and its front and back adjacent nodes, local smoothing adjustment is performed. The specific method is to adjust the node position through interpolation and reduce the local curvature until the torque value of the node falls within the maximum torque threshold. After adjustment, a new path is generated and the above process is repeated until the torque values ​​of all nodes in the path are within the maximum torque threshold, or the preset maximum number of iterations is reached. The termination condition of the iterative adjustment is that the torque values ​​of all nodes in the path meet the maximum torque threshold requirements, or the number of iterations reaches the upper limit.

[0139] The technical feature of iteratively adjusting path node curvature is that the initial path may have areas of excessive local curvature, resulting in excessive torque. This iterative adjustment gradually optimizes the path to ensure that all nodes meet dynamic constraints. The resulting candidate path effectively avoids sensitive areas while ensuring that the robot's thruster torque is within a controllable range, improving the path's feasibility and safety.

[0140] Processing logic for candidate path generation:

[0141] The candidate path generation process begins by integrating the iteratively adjusted paths to produce a final candidate path. This candidate path effectively avoids obstacles within sensitive areas while satisfying the robot's dynamic constraints for curvature and torque at all nodes. This candidate path, as the output of the path planning module, provides a safe and feasible path solution for path verification in the path optimization module.

[0142] Based on confidence parameters, the path planning module generates candidate paths by partitioning sensitive regions and embedding a curvature-torque mapping function using the A* algorithm. This path effectively avoids obstacles within the sensitive regions and meets the torque threshold of the robot's thrusters. However, candidate paths may have local curvature discontinuities or sudden torque changes, resulting in an uneven path or causing dynamic instability in the robot. Therefore, the path optimization module verifies the curvature continuity of the candidate paths and eliminates sudden torque changes to generate a global obstacle avoidance path. This ensures that the path meets physical field perception and dynamic constraints, providing a safe and efficient navigation solution for underwater robots.

[0143] Sensitive and high-risk areas, as inputs to path planning, restrict the feasible search space of the A* algorithm and directly affect the geometry and dynamic characteristics of candidate paths. Therefore, there is a clear causal relationship between sensitive and high-risk areas and candidate paths.

[0144] The path optimization module further processes the candidate paths, eliminating curvature discontinuities and torque mutations to ensure path smoothness and feasibility. Sensitive areas and high-risk areas still play a key role in this process:

[0145] 1. Objectives and constraints of path optimization.

[0146] The goals of the path optimization module are to:

[0147] Curvature continuity check: Adjust curvature discontinuity nodes through cubic spline interpolation.

[0148] Eliminate torque mutation: Adjust the torque mutation node through Bezier curve.

[0149] However, the optimization process is not performed independently, but is constrained by sensitive areas and high-risk areas. For example:

[0150] When adjusting the position of path nodes, it is important to ensure that the adjusted path does not enter high-risk areas, otherwise obstacle avoidance will fail. The optimized path must maintain the same safety as the candidate path, that is, it must not approach sensitive areas due to smoothing.

[0151] Physics constraints include awareness of sensitive and high-risk areas, ensuring the optimized path does not sacrifice safety.

[0152] 2. Balance between security and enforceability.

[0153] Sensitive and high-risk areas are not only the starting point for path planning but also throughout the optimization process, ensuring that the path is smoothed while still avoiding dangerous areas. This deep coupling demonstrates the invention's balance between safety and feasibility. For example, if high-risk areas are ignored during optimization, the adjusted path may enter dangerous areas, increasing the risk of collision. By retaining constraints in sensitive areas, the optimized path achieves the optimal balance between smoothness and safety.

[0154] Example 3: The following is a specific implementation of the path optimization module:

[0155] Processing logic of curvature continuity check:

[0156] The curvature continuity verification process begins at the node locations of the candidate path. A candidate path consists of a series of nodes, each with spatial coordinates. The goal of the verification is to ensure that the curvature of the path is continuous across all nodes, avoiding sharp turns or uneven sections, thereby ensuring the stability of the underwater robot's motion. To this end, the local curvature of each node is calculated based on the geometric relationship between the node and its adjacent nodes. The specific calculation method is to construct two vectors from the previous node to the current node and from the current node to the next node. The curvature value is derived from the direction change and length relationship between these two vectors. The curvature value reflects the degree of curvature of the path at that node. Next, the difference between the curvature values ​​of adjacent nodes is calculated and combined with the time interval or distance relationship to obtain the curvature change rate. If the curvature change rate exceeds a preset curvature change threshold, the node is marked as a curvature discontinuity node. The curvature change threshold is determined based on the underwater robot's dynamic model to ensure that the path curvature change is within the robot's controllable range.

[0157] Processing logic for smoothing curvature discontinuous nodes:

[0158] The curvature discontinuity node smoothing process performs local adjustments on nodes marked as curvature discontinuities. The specific method is to use cubic spline interpolation to adjust the position of the curvature discontinuity node based on the adjacent nodes before and after the curvature discontinuity node. Cubic spline interpolation constructs a smooth curve to connect the previous node, the curvature discontinuity node, the next node, and the next next node to generate a smooth path segment. The shape of the interpolation curve is controlled by the interpolation parameters. By optimizing the interpolation parameters, the curvature change rate between adjacent nodes of the adjusted path segment is reduced to within the curvature change threshold. This process is repeated until all nodes meet the curvature continuity condition and a smooth path is generated. This smoothing method can effectively reduce local curvature changes and ensure a smooth transition of path curvature.

[0159] Through local interpolation adjustments, curvature discontinuities can be eliminated without changing the overall path structure, ensuring the continuity of the path curvature. The resulting smooth path can reduce abrupt changes in the underwater robot's motion, improving the path's smoothness and executability while maintaining the path's obstacle avoidance capabilities, ensuring safe navigation in complex underwater environments.

[0160] The processing logic of torque mutation detection:

[0161] The torque mutation detection process begins at the nodes of a smooth path. It aims to identify sudden torque changes caused by curvature or the rate of curvature change, ensuring that the AUV thruster does not become unstable due to drastic torque changes while executing the path. To this end, the torque value is calculated for each node based on the curvature-torque mapping established in the previous step. The torque value consists of two components: one is the curvature value multiplied by a preset curvature-torque scaling factor, which accounts for the direct effect of curvature on torque; the other is the curvature rate of change multiplied by a preset dynamic influence factor, which accounts for the dynamic effect of curvature changes on torque. The curvature rate of change is calculated by dividing the difference between the curvature values ​​of adjacent nodes by the time interval. Next, the difference between the torque values ​​of adjacent nodes is calculated and combined with the time interval to obtain the torque rate of change. If the torque rate of change exceeds a preset torque change threshold, the node is marked as a torque mutation node. The torque change threshold is determined based on the AUV thruster's performance to ensure that the torque changes are within the thruster's tolerance range.

[0162] When an underwater robot executes a path, sudden changes in torque can cause a sharp change in thruster load, leading to dynamic instability or equipment damage. By detecting sudden changes in torque, we ensure smooth path torque changes, avoid thruster overload, and improve path safety and reliability.

[0163] The processing logic for eliminating torque mutation:

[0164] The process of eliminating torque mutations involves local adjustments to nodes marked as having torque mutations. Specifically, a Bezier curve is used to adjust the path segment, based on the nodes preceding and following the torque mutation node. The Bezier curve constructs a smooth curve connecting the preceding node, the control point, and the following node, generating a smooth path segment. The positions of the control points are optimized to reduce the rate of torque change between adjacent nodes in the adjusted path segment to within the torque change threshold. This process is repeated until all nodes meet the torque continuity condition, ensuring a smooth transition of the path torque. This adjustment method effectively reduces local torque variation and ensures path torque continuity.

[0165] By adjusting the local curve, the torque mutation point can be eliminated without changing the overall structure of the path, ensuring the continuity of the path torque.

[0166] Global obstacle avoidance path output:

[0167] The global obstacle avoidance path generation process begins by integrating the adjusted paths to generate a final global obstacle avoidance path. This path is verified for curvature continuity and torque abruptness elimination to ensure smooth curvature and torque, while also complying with the perceptual and dynamic constraints established by the physics correction in the previous steps. As the output of the path optimization module, the global obstacle avoidance path provides a safe, smooth, and executable navigation path for the underwater robot.

[0168] The technical characteristics of global obstacle avoidance path output stem from its comprehensive consideration of curvature continuity, torque continuity, and physical and dynamic constraints. The resulting path enables efficient obstacle avoidance and safe navigation in complex underwater environments. This advantage lies in the path not only meeting geometric obstacle avoidance requirements but also complying with the underwater robot's motion capabilities and thruster performance, ensuring that path execution will not fail due to dynamic constraints, thereby improving the reliability and robustness of autonomous navigation.

[0169] The path optimization module verifies the curvature continuity of candidate paths and eliminates sudden torque changes to generate a global obstacle avoidance path. This path is adjusted using cubic spline interpolation and Bezier curves to ensure smooth curvature and torque, satisfying the dual constraints of physical field perception and dynamics. This provides a safe, smooth, and executable navigation path for underwater robots in complex environments, completing full-link optimization from perception to motion control.

[0170] The system architecture of the present invention forms a closed loop from multimodal perception to path optimization. Sensitive areas and high-risk areas are the key links connecting perception and control:

[0171] Perception stage: Data collection, physical correction, and feature fusion generate confidence parameters and divide sensitive areas and high-risk areas.

[0172] Planning phase: The path planning module generates candidate paths based on high-risk areas.

[0173] Optimization phase: The path optimization module adjusts the path under the constraints of high-risk areas to ensure global obstacle avoidance.

[0174] This dynamic and adaptive design enables the system to cope with changes in complex underwater environments, with sensitive areas and high-risk areas running throughout, and is inseparable from candidate paths and path optimization.

[0175] In path planning, confidence parameters and the A* algorithm are used to guide candidate paths to avoid dangerous areas.

[0176] In path optimization, as a constraint, it ensures that the optimized path is both smooth and safe.

[0177] By deeply coupling sensitive and high-risk areas with path planning and optimization, the present invention enables safe and efficient navigation of underwater robots in complex environments, fully embodying the collaborative innovation of perception and control.

[0178] The above formulas are all dimensionless and numerical calculations. The formulas are obtained by collecting a large amount of data and performing software simulation to obtain the most recent real situation. The preset parameters in the formulas are set by technicians in this field according to actual conditions.

[0179] It should be noted that the system of the present invention can be deployed on the device itself to realize embedded applications, and can also be run on a PC or other terminal with a user interface, thereby meeting a variety of hardware environments and usage requirements.

[0180] The above description is merely illustrative of certain exemplary embodiments of the present invention. It goes without saying that those skilled in the art will be able to modify the described embodiments in various ways without departing from the spirit and scope of the present invention. Therefore, the above drawings and description are illustrative in nature and should not be construed as limiting the scope of protection of the claims.

[0181] It should be noted that, in this document, if there are relational terms such as first and second, etc., they are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprises", "includes" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device that includes a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article or device. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device that includes the element.

[0182] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of this application. Therefore, the scope of protection of this application should be based on the scope of protection of the claims.

Claims

1. An autonomous obstacle avoidance and path planning system for underwater robots based on multimodal sensors, characterized by: include: Data acquisition module, physical correction module, feature fusion module, confidence assessment module, path planning module and path optimization module; Data acquisition module: Synchronously acquires sonar point cloud and binocular vision RGB-D data, and divides them into alignment processing units based on a unified spatiotemporal benchmark; Physical correction module: solves the Navier-Stokes equations for the sonar data of each alignment processing unit to compensate for sound wave attenuation, solves the Maxwell equations for the visual data to correct light wave scattering, and outputs a physical field correction data set; Feature fusion module: Input the physical field correction data set into the adversarial generative network based on acoustic and optical fluctuation constraints to obtain the sonar-vision fusion feature matrix; Confidence assessment module: Calculates the quantitative values ​​of sonar penetration and visual resolution based on the sonar-vision fusion feature matrix, and obtains confidence parameters through comprehensive analysis based on obstacle motion parameters; Path planning module: divides sensitive areas according to confidence parameters, embeds the curvature-torque mapping function into the A* algorithm, and iteratively adjusts the path node curvature to within the torque threshold to obtain candidate paths; Path optimization module: Verifies the curvature continuity of candidate paths and eliminates torque mutations, and outputs a global obstacle avoidance path.

2. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 1 is characterized in that: The processing of the data acquisition module includes the following: The sonar sensor generates three-dimensional sonar point cloud data reflecting the geometric outline of the obstacle, and the binocular vision sensor generates binocular vision RGB-D data containing color information and depth information. A time synchronization mechanism is used to align the sonar point cloud data and binocular vision RGB-D data to a unified time base, ensuring that the sonar point cloud data and binocular vision RGB-D data reflect the underwater environment status at the same time. The binocular vision RGB-D data is converted from the camera coordinate system to the robot coordinate system and spatially aligned with the sonar point cloud data. The continuous sonar point cloud data stream and binocular vision RGB-D data stream are divided into discrete processing units at fixed time intervals. Each processing unit contains a subset of sonar point cloud data and binocular vision RGB-D data that are time-synchronized and spatially aligned. The sonar point cloud data subset is downsampled to reduce data redundancy, and the binocular vision RGB-D data subset is segmented to extract obstacle boundary information.

3. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 2 is characterized in that: The processing of the physical correction module includes the following: For sonar point cloud data, by simulating the propagation process of sound waves in fluid media, the pressure field distribution during the sound wave propagation process is calculated, the attenuation coefficient of each position is determined, and the density value of each point in the sonar point cloud is adjusted according to the attenuation coefficient and the distance from the point to the sonar sensor to generate a corrected sonar point cloud. Subsequently, the corrected sonar point cloud is clustered and surface reconstructed to generate a sonar geometry set that reflects the geometric outline of the obstacle.

4. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 3 is characterized in that: The processing of the physical correction module also includes the following: For visual data, by simulating the propagation and scattering behavior of light waves in inhomogeneous media, the propagation path and scattering intensity distribution of light waves are calculated, the image distortion coefficient and depth error value caused by scattering are determined, and the pixel value of the RGB image is adjusted according to the image distortion coefficient to enhance the image clarity. The depth value of the depth image is adjusted according to the depth error value to generate corrected visual data. Subsequently, semantic segmentation and feature extraction are performed on the corrected visual data to generate a visual semantic set that reflects the surface details and semantic information of the obstacle. Finally, the sonar geometry set and the visual semantic set are integrated into a physical field correction dataset.

5. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 4 is characterized in that: The processing of the feature fusion module includes the following: The physical field correction dataset is input into the adversarial generative network, which includes a generator and a discriminator. The generator receives the sonar geometry set and the visual semantic set, extracts the deep features of the sonar geometry set and the visual semantic set through a convolutional neural network, and uses the attention mechanism to perform weighted fusion of the deep features to generate a sonar-visual fusion feature matrix. The discriminator evaluates the difference between the sonar-vision fusion feature matrix and the true feature matrix.

6. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 5, characterized in that: The processing of the feature fusion module also includes the following: During the training process of the adversarial generative network, the generator's loss function consists of the adversarial loss and the acoustic-optical wave constraint loss. The acoustic-optical wave constraint loss is calculated based on the residual of the acoustic wave equation and the optical wave equation. The adversarial loss measures the probability that the sonar-vision fusion feature matrix is ​​identified as a generated feature by the discriminator; the discriminator's loss function is used to optimize the ability to distinguish between the real feature matrix and the sonar-vision fusion feature matrix; after training is completed, the sonar-vision fusion feature matrix is ​​output.

7. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 6, characterized in that: The processing of the confidence assessment module includes the following: Extract sonar-related feature subsets from the sonar-vision fusion feature matrix, calculate the overall response strength of the sonar feature subsets, and combine them with the attenuation coefficient to generate a quantitative value of sonar penetration capability; Extracting a subset of visual features from the sonar-vision fusion feature matrix, calculating the overall gradient strength of the visual feature subset, and combining it with the light wave scattering distortion coefficient to generate a quantitative value of visual resolution. Extract the obstacle's motion speed and acceleration from the sonar-vision fusion feature matrix to generate obstacle motion parameters; The quantitative value of sonar penetration ability, the quantitative value of visual resolution ability and the obstacle motion parameters are combined into a feature vector and input into a pre-trained logistic regression model. The feature vector is weighted and summed using the weight vector and bias value of the logistic regression model and mapped to a value between 0 and 1 to generate a confidence parameter.

8. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 7, characterized in that: The processing of the path planning module includes the following: Sensitive areas are divided according to confidence parameters, and the environmental space is divided into grid cells. The radius of the sensitive area is calculated according to the confidence parameter value. The spherical area with the obstacle as the center and the sensitive area radius as the radius is defined as the sensitive area. The grid cells falling into the sensitive area are marked as high-risk areas.

9. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 8, characterized in that: The processing of the path planning module also includes the following: A curvature-torque mapping function is embedded in the A* algorithm to calculate the curvature and corresponding torque value of the path nodes. Candidate nodes whose torque values ​​exceed the maximum torque threshold are eliminated, and candidate nodes whose torque values ​​are within the maximum torque threshold are retained. After the initial path is generated, the nodes with excessive torque are identified, and local smoothing adjustments are performed on the nodes with excessive torque and their adjacent nodes to reduce the local curvature until the torque values ​​of all nodes are within the maximum torque threshold or the maximum number of iterations is reached, thus generating a candidate path.

10. The underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors according to claim 9, characterized in that: The processing of the path optimization module includes the following: The curvature continuity of the candidate path is verified by calculating the local curvature and curvature change rate of each node, identifying the nodes with discontinuous curvature, and adjusting the node positions of these curvature discontinuous nodes using the cubic spline interpolation method until the curvature change rate of all nodes is less than or equal to the set curvature change threshold; the torque mutation in the smooth path is detected by calculating the torque value and torque change rate of each node, identifying the nodes with torque mutation, and adjusting the path segments of these torque mutation nodes using the Bezier curve method until the torque change rate of all nodes is less than or equal to the set torque change threshold, and finally outputting the global obstacle avoidance path.

Images