Underwater robot autonomous obstacle avoidance and path planning system based on multi-modal sensor

Abstract

The invention discloses an underwater robot autonomous obstacle avoidance and path planning system based on a multi-modal sensor, particularly relates to the field of underwater robot navigation, solves the problems of multi-modal sensing and path planning in a complex environment, and unifies the feature space of a multi-source heterogeneous sensor based on a physical field correction mode of an acousto-optic fluctuation rule. Fusion deviation caused by physical property difference of sonar and visual data is overcome, and obstacle characterization precision and dynamic environment adaptability are remarkably improved; a dynamic confidence decision-making mechanism is combined with a path curvature-moment constraint model, the environmental perception credibility and the robot motion performance are synchronously fused into a path generation process, the feasibility of motion control is ensured while the geometric collision risk is avoided, and the contradiction between the safety and the performability in path planning is solved; the global path optimization eliminates the hidden danger of local path sudden change through iterative smoothing under double constraints of physical field and dynamics, and forms an optimal navigation scheme considering obstacle avoidance efficiency, energy consumption and motion stability.

Description

Technical Field

[0001] The present invention relates to the field of underwater robot navigation. More specifically, the present invention relates to an autonomous obstacle avoidance and path planning system for underwater robots based on multi-modal sensors. Background Art

[0002] Currently, multi-modal sensor fusion technology is commonly used in autonomous obstacle avoidance systems for underwater robots. By complementing heterogeneous data such as sonar and vision, the ability to perceive complex environments is enhanced. Sonar obtains the geometric contour of obstacles with strong medium penetration, and vision relies on high-resolution imaging to analyze surface semantic details. The combination of the two can theoretically construct a complete representation of the three-dimensional environment and provide accurate decision-making basis for path planning.

[0003] However, existing systems do not deeply deconstruct the physical field action mechanism of acoustic and optical sensing, and simply adopt a data-level fusion strategy, resulting in an essential cognitive bias in the cross-modal feature space. Due to the differences in physical laws such as the propagation attenuation, interface scattering, and motion perception of sound waves and light waves in water media, the same obstacle presents completely different physical representations in sonar point clouds and visual images. Traditional fusion methods lack physical field coupling modeling and cannot eliminate cross-scale feature mutual exclusion and spatio-temporal misalignment, seriously restricting the accuracy of obstacle avoidance decisions and the efficiency of path planning.

[0004] To solve the above problems, a technical solution is provided. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, the embodiments of the present invention provide an autonomous obstacle avoidance and path planning system for underwater robots based on multi-modal sensors. The physical field correction method based on the laws of acoustic and optical waves unifies the feature spaces of multi-source heterogeneous sensors, overcomes the fusion deviation caused by the physical property differences between sonar and visual data, and significantly improves the obstacle representation accuracy and dynamic environment adaptability; the dynamic confidence decision-making mechanism combines the path curvature-torque constraint model, synchronously integrates the environmental perception credibility and the robot motion performance into the path generation process, ensures the feasibility of motion control while avoiding the risk of geometric collision, and solves the contradiction between safety and executability in path planning; the global path optimization eliminates potential local path mutations through iterative smoothing under the dual constraints of the physical field and dynamics, and forms an optimal navigation plan that takes into account obstacle avoidance efficiency, energy consumption, and motion stability, so as to solve the problems raised in the above background art.

[0006] To achieve the above object, the present invention provides the following technical solutions: A data acquisition module, a physical correction module, a feature fusion module, a confidence evaluation module, a path planning module, and a path optimization module; Data acquisition module: Synchronously acquire sonar point clouds and binocular vision RGB-D data, and divide them into alignment processing units according to a unified spatio-temporal reference; Physical correction module: Solve the Navier-Stokes equation for the sonar data of each alignment processing unit to compensate for acoustic wave attenuation, solve the Maxwell equation for the visual data to correct light wave scattering, and output a physical field correction data set; Feature fusion module: Input the physical field correction data set into the adversarial generation network based on acousto-optic wave constraints to obtain a sonar-vision fusion feature matrix; Confidence evaluation module: Calculate the sonar penetration ability quantization value and the visual resolution ability quantization value according to the sonar-vision fusion feature matrix, and comprehensively analyze and obtain the confidence parameter in combination with the obstacle movement parameters; Path planning module: Divide the sensitive area according to the confidence parameter, embed the curvature-moment mapping function into the A* algorithm and iteratively adjust the curvature of the path nodes to within the moment threshold to obtain a candidate path; Path optimization module: Verify the curvature continuity of the candidate path and eliminate moment mutations, and output a global obstacle avoidance path.

[0007] In a preferred embodiment, the processing process of the data acquisition module includes the following contents: Generate three-dimensional sonar point cloud data reflecting the geometric contour of the obstacle through the sonar sensor, and generate binocular vision RGB-D data containing color information and depth information through the binocular vision sensor; Use the time synchronization mechanism to align the sonar point cloud data and the binocular vision RGB-D data to a unified time reference to ensure that the sonar point cloud data and the binocular vision RGB-D data reflect the underwater environment state at the same moment; Convert the binocular vision RGB-D data from the camera coordinate system to the robot coordinate system to achieve spatial alignment with the sonar point cloud data; Divide the continuous sonar point cloud data stream and the binocular vision RGB-D data stream into discrete processing units at fixed time intervals, and each processing unit contains a sonar point cloud data subset and a binocular vision RGB-D data subset that are time-synchronized and spatially aligned; Perform downsampling processing on the sonar point cloud data subset to reduce data redundancy, and perform image segmentation on the binocular vision RGB-D data subset to extract obstacle boundary information.

[0008] In a preferred embodiment, the processing process of the physical correction module includes the following contents: For the sonar point cloud data, by simulating the propagation process of sound waves in a fluid medium, calculate the pressure field distribution during the sound wave propagation process, determine the attenuation coefficient at each position, and adjust the density value of each point in the sonar point cloud according to the attenuation coefficient and the distance from the point to the sonar sensor to generate a corrected sonar point cloud. Subsequently, perform clustering and surface reconstruction on the corrected sonar point cloud to generate a sonar geometry set reflecting the geometric contour of the obstacle.

[0009] In a preferred embodiment, the processing process of the physical correction module further includes the following contents: For visual data, by simulating the propagation and scattering behavior of light waves in a non-uniform medium, calculate the propagation path of the light waves and the distribution of scattering intensity, determine the image distortion coefficient and depth error value caused by scattering, and adjust the pixel values of the RGB image according to the image distortion coefficient to enhance the image clarity. Adjust the depth value of the depth image according to the depth error value to generate corrected visual data. Subsequently, perform semantic segmentation and feature extraction on the corrected visual data to generate a visual semantic set that reflects the surface details and semantic information of the obstacle. Finally, integrate the sonar geometric set and the visual semantic set into a physical field correction data set.

[0010] In a preferred embodiment, the processing process of the feature fusion module includes the following: Input the physical field correction data set into the adversarial generation network, which includes a generator and a discriminator. The generator receives the sonar geometric set and the visual semantic set, extracts the deep features of the sonar geometric set and the visual semantic set through a convolutional neural network, and uses the attention mechanism to perform weighted fusion on the deep features to generate a sonar-visual fusion feature matrix. The discriminator evaluates the difference between the sonar-visual fusion feature matrix and the real feature matrix.

[0011] In a preferred embodiment, the processing process of the feature fusion module further includes the following: During the training process of the adversarial generation network, the loss function of the generator consists of an adversarial loss and an acoustic-optical wave fluctuation constraint loss. The acoustic-optical wave fluctuation constraint loss is calculated based on the residuals of the acoustic wave equation and the optical wave equation. The adversarial loss measures the probability that the sonar-visual fusion feature matrix is recognized as a generated feature by the discriminator. The loss function of the discriminator is used to optimize the ability to distinguish between the real feature matrix and the sonar-visual fusion feature matrix. After training is completed, output the sonar-visual fusion feature matrix.

[0012] In a preferred embodiment, the processing process of the confidence evaluation module includes the following: Extract the sonar-related feature subset from the sonar-visual fusion feature matrix, calculate the overall response intensity of the sonar feature subset, and generate a sonar penetration ability quantization value in combination with the attenuation coefficient; Extract the vision-related feature subset from the sonar-visual fusion feature matrix, calculate the overall gradient intensity of the vision feature subset, and generate a vision resolution ability quantization value in combination with the light wave scattering distortion coefficient; Extract the movement speed and acceleration of the obstacle from the sonar-visual fusion feature matrix to generate obstacle movement parameters; Quantize the sonar penetration ability value, the visual resolution ability value, and the obstacle motion parameters into a feature vector, input it into a pre-trained Logistic regression model, and perform a weighted sum on the feature vector through the weight vector and bias value of the Logistic regression model and map it between 0 and 1 to generate a confidence parameter.

[0013] In a preferred embodiment, the processing process of the path planning module includes the following: Divide the sensitive area according to the confidence parameter, divide the environmental space into grid cells, calculate the sensitive area radius according to the confidence parameter value, define the spherical area centered on the obstacle and with the sensitive area radius as the radius as the sensitive area, and mark the grid cells falling into the sensitive area as high-risk areas.

[0014] In a preferred embodiment, the processing process of the path planning module further includes the following: Embed a curvature-moment mapping function in the A* algorithm, calculate the curvature and corresponding moment values of the path nodes, eliminate the candidate nodes whose moment values exceed the maximum moment threshold, and retain the candidate nodes whose moment values are within the maximum moment threshold; after generating the initial path, identify the nodes with moment overrun, and perform local smoothing adjustment on the nodes with moment overrun and their neighboring nodes to reduce the local curvature until the moment values of all nodes are within the maximum moment threshold or the maximum number of iterations is reached, and generate a candidate path.

[0015] In a preferred embodiment, the processing process of the path optimization module includes the following: Verify the curvature continuity of the candidate path by calculating the local curvature and curvature change rate of each node, identify the nodes with discontinuous curvature, and use the cubic spline interpolation method to adjust the node positions of these nodes with discontinuous curvature until the curvature change rate of all nodes is less than or equal to the set curvature change threshold; detect the moment mutation in the smooth path by calculating the moment value and moment change rate of each node, identify the nodes with moment mutation, and use the Bezier curve method to adjust the path segment of these nodes with moment mutation until the moment change rate of all nodes is less than or equal to the set moment change threshold, and finally output the global obstacle avoidance path.

[0016] The technical effects and advantages of the underwater robot autonomous obstacle avoidance and path planning system based on multi-modal sensors of the present invention: Through the deep coupling of multi-modal perception data and robot dynamics, the present invention constructs a physical field-driven autonomous obstacle avoidance and path planning system, achieving full-link optimized navigation in complex underwater environments. The physical field correction technology based on the laws of acoustic and optical waves effectively unifies the feature spaces of multi-source heterogeneous sensors, overcomes the fusion deviation caused by the physical property differences between sonar and visual data in traditional methods, and significantly improves the accuracy of obstacle representation and the adaptability to dynamic environments; the dynamic confidence decision-making mechanism combines the path curvature-moment constraint model, synchronously integrates the environmental perception credibility and the robot's motion performance into the path generation process, ensures the feasibility of motion control while avoiding the risk of geometric collision, and solves the contradiction between safety and executability in path planning; the global path optimization eliminates potential local path mutations through iterative smoothing under the dual constraints of the physical field and dynamics, forming an optimal navigation solution that takes into account obstacle avoidance efficiency, energy consumption, and motion stability, providing high-robustness autonomous navigation capabilities for complex underwater scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a schematic structural diagram of the underwater robot autonomous obstacle avoidance and path planning system based on multi-modal sensors of the present invention.

[0018] Figure 2 It is a schematic processing flow diagram of the sensitive area division of the underwater robot autonomous obstacle avoidance and path planning system based on multi-modal sensors of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0019] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0020] Embodiment 1: Figure 1 An underwater robot autonomous obstacle avoidance and path planning system based on multi-modal sensors of the present invention is given, including: a data acquisition module, a physical correction module, a feature fusion module, a confidence evaluation module, a path planning module, and a path optimization module; Data acquisition module: Synchronously acquire sonar point cloud and binocular vision RGB-D data, and divide them into alignment processing units according to a unified spatio-temporal reference.

[0021] Physical correction module: Solve the Navier-Stokes equation for the sonar data of each alignment processing unit to compensate for acoustic wave attenuation, and solve the Maxwell equation for the visual data to correct light wave scattering, and output a physical field correction data set.

[0022] Feature Fusion Module: Input the physically corrected dataset into the adversarial generative network based on acousto-optic wave constraints to obtain the sonar-vision fusion feature matrix.

[0023] Confidence Evaluation Module: Calculate the sonar penetration ability quantization value and the visual resolution ability quantization value according to the sonar-vision fusion feature matrix, and comprehensively analyze and obtain the confidence parameter by combining the obstacle motion parameters.

[0024] Path Planning Module: Divide the sensitive area according to the confidence parameter, embed the curvature-torque mapping function into the A* algorithm and iteratively adjust the curvature of the path nodes within the torque threshold to obtain the candidate path.

[0025] Path Optimization Module: Verify the curvature continuity of the candidate path and eliminate the torque mutation, and output the global obstacle avoidance path.

[0026] In the autonomous obstacle avoidance and path planning execution of an underwater robot, sonar sensors and binocular vision sensors are two core perception means. Sonar sensors can obtain the geometric contour information of obstacles by penetrating the water body with sound waves, but their resolution is low and they are easily affected by noise; binocular vision sensors can 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 decline in complex environments. In view of this background, the present invention proposes a method for underwater robot perception and path planning based on multi-modal sensor fusion. By jointly processing sonar point clouds 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.

[0027] The purpose of the data acquisition module is to synchronously obtain 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 according to a unified time reference and space reference, providing a reliable data basis for subsequent physical field correction and feature fusion.

[0028] The following is the specific implementation manner of the data acquisition module: The acquisition process of sonar point cloud data is as follows: The sonar sensor emits sound waves and receives the signals reflected by obstacles in the underwater environment, detects the positions and shapes of obstacles, and generates three-dimensional point cloud data reflecting the geometric contours of obstacles. The three-dimensional point cloud data consists of multiple spatial coordinate points, and each spatial coordinate point represents the position of the surface of a certain obstacle in the underwater environment. The coordinates are centered on the robot body and represented using the robot coordinate system.

[0029] The acquisition process of binocular vision RGB-D data is as follows: The binocular vision sensor uses the left and right cameras to simultaneously capture images of the underwater environment, generating RGB-D data containing color information and depth information. Among them, the color information part is a color image, providing the color and texture features of obstacles; the depth information part is a depth image, providing the distance information of each pixel point relative to the binocular vision sensor. The initial reference frame of these data is the camera coordinate system of the binocular vision sensor.

[0030] The implementation process of the time synchronization mechanism is as follows: Since the sampling frequencies of the sonar sensor and the binocular vision sensor are different, and there may be differences in the timestamps of the data generated by each, it is necessary to align the sonar point cloud data and the binocular vision RGB-D data to a unified time point. First, a unified time reference is determined, and then the data is adjusted by interpolation methods to align the sampling timestamps of the two types of data to the time point closest to the unified time reference. Specifically, for the data of each sensor, according to the order of timestamps, the data sampling moment closest to the unified time reference time point is calculated, and the sonar point cloud data and the binocular vision RGB-D data corresponding to this moment are output as the synchronized data, so as to ensure that these two types of data reflect the state of the underwater environment at the same moment.

[0031] The process of time reference division is as follows: The continuous data stream formed by the sonar point cloud data and the binocular vision 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 a subset of the binocular vision RGB-D data at this time point. The selection of the time interval is determined according to the movement speed of the robot and the complexity of the underwater environment, usually set between 0.1 second and 1 second to ensure that the data division can reflect the dynamic changes of the environment and is convenient for subsequent processing.

[0032] The process of spatial coordinate transformation is as follows: To achieve the spatial alignment of the sonar point cloud data and the binocular vision RGB-D data, the two types of data are uniformly transformed into the robot coordinate system. The sonar point cloud data itself is represented in the robot coordinate system, so no coordinate transformation is required. The binocular vision RGB-D data is initially represented in the camera coordinate system. Through the rotation matrix and translation vector obtained by pre-calibration, the depth information in the binocular vision RGB-D data is converted into three-dimensional point coordinates in the robot coordinate system.

[0033] The specific transformation process is as follows: For each pixel point in the depth image, according to the depth value and the position in the camera coordinate system, combined with the rotation matrix and translation vector, the three-dimensional coordinates of this pixel point in the robot coordinate system are calculated, thus generating three-dimensional point data that is spatially aligned with the sonar point cloud data.

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

[0035] The process of optimizing data preprocessing is as follows: To improve the efficiency of subsequent processing, downsampling is performed on the subset of sonar point cloud data. The specific method is to use voxel grid filtering technology to divide the point cloud data into multiple voxel grids, and retain key points representing the geometric features of obstacles within each voxel grid, while removing redundant points, thereby reducing the total number of point cloud data. For the subset of binocular vision RGB-D data, image segmentation is performed. By analyzing the color image and depth image, boundary information related to obstacles is extracted, background data irrelevant to obstacle avoidance is removed, and semantic features meaningful for path planning are retained.

[0036] The sonar point cloud data and the binocular vision RGB-D data are aligned to the same time point through the time synchronization mechanism, ensuring that the two types of data reflect the state of the underwater environment at the same moment. This synchronization process avoids environmental perception errors caused by time deviation, enabling subsequent analysis based on multi-modal data fusion to obtain higher accuracy.

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

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

[0039] Performing downsampling on the subset of sonar point cloud data and image segmentation on the subset of binocular vision RGB-D data can effectively reduce data redundancy and highlight the geometric and semantic features of obstacles. This reduces the computational complexity, improves the speed and efficiency of processing data in a complex underwater environment, and thus enhances the response ability of the underwater robot.

[0040] The data acquisition module synchronously acquires sonar point cloud data and binocular vision RGB-D data, and divides them into alignment processing units based on a unified time reference and spatial reference, completing the preliminary integration of multi-modal perception data of the underwater robot. The generated alignment processing units are aligned in time to a selected time point and unified in space to the robot coordinate system, ensuring the temporal consistency and spatial comparability of the data. This result provides a reliable data basis for subsequent physical field correction and feature fusion, enabling the underwater robot to perform further task processing based on accurate environmental perception.

[0041] In the data acquisition module, sonar point cloud and binocular vision RGB-D data have been synchronously acquired and divided into alignment processing units based on a unified spatio-temporal reference, providing a spatio-temporally aligned multi-modal data basis for subsequent processing. However, due to the propagation attenuation of sound waves in the water medium and the scattering phenomenon of light waves in the underwater environment, there are perceptual biases in the sonar point cloud and visual data 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 the water medium and the scattering of light waves in the underwater environment, thereby outputting a physically corrected data set.

[0042] The following is the specific implementation method of the physical correction module: Physical field correction of sonar data: The process of compensating for the attenuation of sound waves in the sonar point cloud is as follows: When sound waves propagate in the water medium, energy loss occurs due to absorption and scattering, resulting in a decrease in the point cloud density of distant obstacles, affecting the accurate representation of the geometric contour of the obstacles. To solve this problem, the Navier-Stokes equation is used to simulate the propagation process of sound waves in the fluid medium. Specifically, the Navier-Stokes equation describes the coupling relationship between fluid motion and sound wave propagation, including parameters such as fluid density, velocity field, pressure field, viscosity, and external force terms. By numerically solving the Navier-Stokes equation, the pressure field distribution at different positions during the sound wave propagation process is calculated, and then the attenuation coefficient of the sound wave at each position is determined. The attenuation coefficient reflects the degree to which the sound wave energy weakens with increasing distance. For each point in the sonar point cloud, according to its position in the robot coordinate system and the distance to the sonar sensor, the corresponding attenuation coefficient is calculated, and the density value of this point is adjusted through a reverse compensation method.

[0043] 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 enhancing 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 contour of the obstacle. The following is an example of physical field correction of sonar data: When sound waves propagate in a water medium, they attenuate due to absorption and scattering, resulting in a decrease in the point cloud density of mid- and long-range obstacles and affecting the accuracy of geometric contour representation. To compensate for this attenuation, the Navier-Stokes equation is used to simulate the sound wave propagation process, calculate the energy loss, and adjust the point cloud density. The specific steps are as follows: Divide the underwater environment into multiple fluid units and describe the coupling relationship between fluid motion and sound wave propagation based on the Navier-Stokes equation: ; ; where 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 perturbation caused by sound waves).

[0044] By numerically solving the Navier-Stokes equation, calculate the pressure field distribution during the sound wave propagation process , and then determine the attenuation coefficient at each position .

[0045] For each point in the sonar point cloud , calculate the corresponding attenuation coefficient according to its position, and adjust the point cloud density through the reverse compensation formula: ; where represents the distance from point to the sonar sensor, is the attenuation compensation factor to ensure that the far-distance point cloud data is enhanced, and generate the corrected sonar point cloud .

[0046] represents the index of a single point in the sonar point cloud; represents the time point index in the time series, which is used to identify the specific time reference corresponding to the alignment processing unit. Sonar geometry set generation: The generation process of the sonar geometry set is as follows: Cluster the corrected sonar point cloud, group the point cloud data according to spatial proximity, and form point cloud clusters representing different obstacles. Subsequently, through surface reconstruction technology, fit each point cloud cluster into a continuous geometric surface to generate structured data reflecting the geometric contour of the obstacle, which is called the sonar geometry set. This geometry set retains the three-dimensional shape information of the obstacle and provides reliable geometric feature input for the feature fusion of the subsequent feature fusion module.

[0047] Physical field correction of visual data: The light wave scattering correction process of visual data is as follows: Light waves are scattered in the underwater environment due to suspended particles and turbidity, resulting in blurred RGB images and depth errors in depth images. To correct the scattering effect, the Maxwell equations are used to simulate the propagation and scattering behavior of light waves in inhomogeneous media. The Maxwell equations describe the distribution of the electric and magnetic fields of light waves and contain parameters such as electric field strength, magnetic induction intensity, magnetic field strength, electric displacement, and current density. By numerically solving the Maxwell equations, the propagation path and scattering intensity distribution of light waves in the underwater environment are calculated, and the image distortion coefficient and depth error value caused by scattering are determined. The image distortion coefficient reflects the attenuation of pixel brightness caused by light wave scattering, and the depth error value reflects the interference of scattering on depth measurement. For RGB images, the defogging factor is calculated according to the image distortion coefficient. The defogging factor is the complement of the distortion coefficient and is used to enhance the brightness value of pixels. The specific method is to divide the original RGB value of each pixel by the corresponding defogging factor to restore the clarity of the image. For depth images, the depth value of each pixel is adjusted according to 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 integrated to form corrected visual data. This process significantly improves the clarity and accuracy of visual data in complex underwater environments. For example, the physical field correction of visual data is as follows: Light waves are scattered in water due to suspended particles and turbidity, resulting in blurred images and depth estimation errors in visual data To correct the scattering effect, the Maxwell equations are used to simulate the light wave propagation and scattering process. The specific steps are as follows: Model the underwater environment as an inhomogeneous medium and describe the distribution of the electric and magnetic fields of light waves based on the Maxwell equations: ; where represents the electric field strength, represents the magnetic induction intensity, represents the magnetic field strength, represents the electric displacement, represents the current density.

[0048] By numerically solving the Maxwell equations, calculate the light wave propagation path and scattering intensity distribution, and determine the image distortion coefficient caused by scattering and the depth error . Correct the visual data : For RGB images, calculate the defogging factor according to the scattering model and adjust the pixel values: ; For the depth image, adjust the depth value according to the depth error : ; After integration, generate corrected visual data .

[0049] Generation of visual semantic set: The generation process of the visual semantic set is as follows: Perform semantic segmentation on the corrected visual data. By analyzing the color and texture features of the RGB image and the spatial information of the depth image, divide the image into different semantic regions, such as the obstacle region and the background region. Subsequently, extract the features of each semantic region, including the surface texture, color distribution, and boundary shape of the obstacle, and generate structured data reflecting the surface details and semantic information of the obstacle, called the visual semantic set. This semantic set provides rich semantic feature inputs for the feature fusion of the subsequent feature fusion module.

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

[0051] By solving the Navier-Stokes equation to simulate the acoustic wave propagation and calculate the attenuation coefficient, the long-distance point density of the sonar point cloud is effectively enhanced. This processing eliminates the geometric contour distortion caused by acoustic wave attenuation, enabling the sonar point cloud to more accurately reflect the three-dimensional shape of the obstacle.

[0052] Generate the sonar geometry set through clustering and surface reconstruction, converting the discrete point cloud data into a structured geometric representation. This processing retains the core shape features of the obstacle, reduces noise interference, and improves the availability of geometric information.

[0053] By solving the Maxwell equation to calculate the distortion coefficient and depth error caused by scattering, the clarity of the RGB image and the accuracy of the depth image are significantly improved. This processing effectively alleviates the interference of underwater turbidity and suspended particles on the visual data, enabling the visual data to more realistically reflect the surface details and spatial positions of the obstacles.

[0054] Generate the visual semantic set through semantic segmentation and feature extraction, extracting semantic information such as the texture, color, and boundary of the obstacle. This processing simplifies the complex information of the visual data into structured semantic features, facilitating cross-modal fusion with the sonar geometry set and enhancing the integrity of the obstacle representation.

[0055] The physical correction module compensates for acoustic wave attenuation by solving the Navier-Stokes equation for sonar point clouds, and corrects light wave scattering by solving the Maxwell equation for visual data, generating a physical field correction data set. This data set contains accurate obstacle geometric contours and semantic information, eliminates the perception bias caused by the propagation of acoustic and light waves in the water medium, provides high-quality data input for the sonar-visual feature fusion of the feature fusion module, and ensures the perception accuracy of the underwater robot in complex environments.

[0056] In the physical correction module, the Navier-Stokes equation is solved for sonar data to compensate for acoustic wave attenuation, and the Maxwell equation is solved for visual data to correct light wave scattering, generating a physical field correction data set, which provides an accurate perception basis for subsequent processing. However, due to the heterogeneity of the sonar geometric set and the visual semantic set in the feature space, direct fusion may lead to information loss or redundancy. Therefore, the feature fusion module needs to map the sonar geometric set and the visual semantic set to a unified feature space through specific technical means to generate a sonar-visual fusion feature matrix. The goal of the feature fusion module is to input the physical field correction data set output by the physical correction module into an adversarial generation network based on acousto-optic wave constraints to generate a sonar-visual fusion feature matrix.

[0057] The following is the specific implementation manner of the feature fusion module: The processing logic of the adversarial generation network architecture: The adversarial generation network adopted by the feature fusion module consists of a generator and a discriminator. The task of the generator is to fuse the sonar geometric set and the visual semantic set in the physical field correction data set output by the physical correction module to generate a sonar-visual fusion feature matrix. Specifically, the generator receives the sonar geometric set and the visual semantic set as inputs, extracts and integrates the features of the two through the internal neural network structure, and finally outputs the sonar-visual fusion feature matrix. The task of the discriminator is to evaluate whether the sonar-visual fusion feature matrix output by the generator truly reflects the comprehensive features of the obstacles. The discriminator receives the sonar-visual fusion feature matrix or the real feature matrix obtained from the labeled data as inputs, and outputs a discriminant result by comparing the feature distributions of the two, which is used to distinguish the real feature matrix and the generated feature matrix.

[0058] The processing logic of acousto-optic wave constraints: To ensure that the sonar-vision fusion feature matrix conforms to the physical propagation laws of sound waves and light waves in the feature space, an acousto-optic wave constraint loss function is designed. This loss function ensures the physical consistency of the feature matrix output by the generator by quantifying the deviation of the sonar-vision fusion feature matrix from the acoustic wave equation and the optical wave equation. In specific implementation, first, the residual of the sonar-vision fusion feature matrix in the acoustic wave equation is calculated, that is, the deviation degree is measured by comparing the sound pressure distribution of the feature matrix with the distribution predicted by the acoustic wave equation; then, the residual of it in the optical wave equation is calculated, that is, the deviation degree is measured by comparing the electric field distribution of the feature matrix with the distribution predicted by the optical wave equation.

[0059] Processing logic of network training and loss function: The training processes of the generator and the discriminator adopt an alternating optimization method to achieve the high-quality generation of the sonar-vision fusion feature matrix. The training objective of the generator is to generate a realistic sonar-vision fusion feature matrix that can deceive the discriminator and meet the acousto-optic wave constraints. The loss function of the generator consists of two parts: the first part is the adversarial loss, which is used to measure the probability that the sonar-vision fusion feature matrix is recognized as a generated feature by the discriminator and is quantified by calculating the difference between the discriminator output and the expected value; the second part is the wave constraint loss, which is used to measure the deviation of the sonar-vision fusion feature matrix from the acoustic wave equation and the optical wave equation and is quantified by calculating the sum of the squares of the above residuals. The two parts of the loss are weighted and summed through a preset weight coefficient. The training objective of the discriminator is to accurately distinguish between the real feature matrix and the generated feature matrix. Its loss function contains two terms: one is the recognition loss of the real feature matrix, which is quantified by calculating the difference between the discriminator output and the real label; the other is the recognition loss of the generated feature matrix, which is quantified by calculating the difference between the discriminator output and the generated label. The two losses are added together to optimize the discriminator parameters.

[0060] Processing logic of feature fusion and output: Internally, the generator extracts the deep features of the sonar geometric set and the visual semantic set through a convolutional neural network. Specifically, the convolutional neural network performs multi-layer convolution and pooling operations on the sonar geometric set to extract the geometric contour features of obstacles; it performs similar operations on the visual semantic set to extract the surface detail features of obstacles. After the extraction, an attention mechanism is used to weight and fuse the features of the sonar geometric set and the visual semantic set. The attention mechanism dynamically adjusts the contribution weights of the two according to the importance of the features. By calculating and normalizing the correlation scores of each feature channel, a weighting coefficient is generated, and then the features of the sonar geometric set and the visual semantic set are added according to the weighting coefficient to generate a sonar-visual fusion feature matrix. After training, the adversarial generation network outputs the sonar-visual fusion feature matrix, which represents the geometric contour and surface details of obstacles in a unified feature space and ensures physical consistency through the acoustic-optical wave fluctuation constraint.

[0061] The feature fusion module generates a sonar-visual fusion feature matrix by inputting the physical field correction data set output by the physical correction module into an adversarial generation network based on the acoustic-optical wave fluctuation constraint. This process uses the generator to fuse the features of the sonar geometric set and the visual semantic set, combines the adversarial training of the discriminator and the acoustic-optical wave fluctuation constraint to optimize the fusion effect. Finally, the output sonar-visual fusion feature matrix represents the geometric contour and surface details of obstacles in a unified feature space and conforms to the propagation laws of sound waves and light waves.

[0062] In the feature fusion module, through an adversarial generation network based on the acoustic-optical wave fluctuation constraint, the sonar geometric set and the visual semantic set are fused into a sonar-visual fusion feature matrix, which represents the geometric contour and surface details of obstacles in a unified feature space and conforms to the acoustic-optical propagation laws. However, relying solely on the fusion feature matrix cannot directly provide reliable guidance for path planning. It is also necessary to extract quantitative indicators of the perception ability from it to evaluate the reliability of sonar and visual data in the current environment. Therefore, the confidence evaluation module aims to calculate the quantitative values of the sonar penetration ability and the visual discrimination ability from the fusion feature matrix, and generate confidence parameters in combination with the obstacle motion parameters to provide decision-making support for the sensitive area division and path planning of the path planning module.

[0063] The following is the specific implementation method of the confidence evaluation module: Calculation logic of the quantitative value of sonar penetration ability: The calculation process of the quantization value of sonar penetration ability starts with extracting the feature subset related to sonar data from the sonar-vision fusion feature matrix. Next, the length of each feature vector in this feature subset is quantified. Specifically, the square sum of each feature vector is calculated and the square root is taken to obtain the length of each feature vector, and then the lengths of all feature vectors are added together to obtain the overall response intensity. The overall response intensity is used to characterize the ability of sonar data to perceive the geometric contour of obstacles. Subsequently, combined with the attenuation coefficient obtained by solving the Navier-Stokes equation in the physical correction module, this coefficient represents the energy loss of sound waves during water body propagation. Finally, the quantization value of sonar penetration ability is calculated by dividing the overall response intensity by a denominator adjusted by the attenuation coefficient. The denominator is specifically the sum of 1 and the attenuation coefficient. By integrating the perception intensity of sonar data and the attenuation characteristics of sound waves in the water body, the quantization value of sonar penetration ability is obtained, which is used to evaluate the reliability of sonar data in the current underwater environment.

[0064] The calculation of the quantization value of sonar penetration ability is based on the influence of water turbidity and obstacle density on the perception ability of sonar data. The reason is that by quantifying sonar penetration ability, the reliability of sonar data in different underwater environments can be dynamically evaluated. It can adaptively adjust the degree of trust in sonar data, especially in environments with high water turbidity or large obstacle density, reducing the over-reliance on sonar data.

[0065] Calculation logic of the quantization value of visual resolution ability: The calculation process of the quantization value of visual resolution ability starts with extracting the feature subset related to visual data from the sonar-vision fusion feature matrix. Next, the change rate of each feature vector in this feature subset is quantified. Specifically, the difference of each feature vector in space or time is calculated to obtain the change rate of each feature vector, and then the square root of the square sum of these change rates is taken to obtain the change intensity of each feature vector, and then the change intensities of all feature vectors are added together to obtain the overall gradient intensity. The overall gradient intensity is used to characterize the clarity of visual data in perceiving the surface details of obstacles. Subsequently, combined with the light wave scattering distortion coefficient obtained by solving the Maxwell equation in the physical correction module, this coefficient represents the scattering effect of light waves during water body propagation. Finally, the quantization value of visual resolution ability is calculated by dividing the overall gradient intensity by a denominator adjusted by the light wave scattering distortion coefficient. The denominator is specifically the sum of 1 and the light wave scattering distortion coefficient. This calculation method integrates the detail perception ability of visual data and the scattering interference of light waves in the water body, thereby obtaining the quantization value of visual resolution ability, which is used to evaluate the reliability of visual data in the current underwater environment.

[0066] The calculation of the quantization value of visual resolution ability is based on the influence of water turbidity and lighting conditions on the perception ability of visual data. The reason is that by quantifying the visual resolution ability, the reliability of visual data in different underwater environments can be dynamically evaluated. It can adaptively adjust the degree of trust in visual data, especially in environments with severe light scattering or high water turbidity, reducing the over-reliance on visual data.

[0067] The extraction logic of obstacle motion parameters: The extraction process of obstacle motion parameters starts from analyzing the dynamic characteristics of obstacles in the sonar-visual fusion feature matrix. First, the motion speed of the obstacle is generated. The specific method is to calculate the rate of change over time for the sonar-visual fusion feature matrix of consecutive time frames, that is, by dividing the difference between the feature matrices of adjacent time frames by the time interval to obtain the motion speed. Then, the acceleration of the obstacle is generated. The specific method is to calculate the second-order rate of change over time for the sonar-visual fusion feature matrix of consecutive time frames, that is, by dividing the difference between the motion speeds of adjacent time frames by the time interval to obtain the acceleration. Finally, the motion speed and acceleration are combined into the obstacle motion parameters, which are used as the input data for the subsequent generation of confidence parameters. This extraction method accurately captures the dynamic characteristics of obstacles by analyzing the temporal changes of the feature matrix.

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

[0069] The generation logic of confidence parameters: The generation process of confidence parameters uses the Logistic regression method. It starts by combining the quantization value of sonar penetration ability, the quantization value of visual resolution ability, the motion speed and acceleration in the obstacle motion parameters into a feature vector. Next, a pre-trained Logistic regression model is used to process this feature vector. The specific method is to assign a weight value to each component in the feature vector, sum the products of all components and their corresponding weight values, and then add a bias value to obtain an intermediate value. Then, this intermediate value is input into a function that maps the intermediate value to between 0 and 1 to generate the confidence parameter. The mapping function is characterized in that its output value smoothly transitions from 0 to 1 as the intermediate value increases, indicating the reliability of the sonar and visual data fusion in the current environment. The weight values and bias value of the model are determined by the training data, which includes feature vectors and their corresponding confidence labels in different underwater environments. The labels are from expert evaluations or simulation experiments. This generation method combines the sonar penetration ability, visual resolution ability, and obstacle motion parameters to obtain the confidence parameter.

[0070] The generation of the confidence parameter adopts the Logistic regression method, which can handle multiple input features and generate an output in the form of probability, and is suitable for evaluating the reliability of sonar and visual data fusion. The advantage of this technical feature is that by integrating the sonar penetration ability, visual resolution ability, and obstacle movement parameters, the confidence parameter can dynamically reflect the impact of the current environment on sensor data, providing an accurate basis for sensitive area division.

[0071] The confidence evaluation module calculates the quantification value of sonar penetration ability and the quantification value of visual resolution ability from the sonar-visual fusion feature matrix, combines the obstacle movement parameters, and uses the Logistic regression method to generate the confidence parameter. The confidence parameter comprehensively evaluates the reliability of sonar and visual data in the current underwater environment, providing accurate decision-making support for the sensitive area division of the path planning module. The technical logic of the confidence evaluation module ensures the adaptability and robustness of the path planning of the underwater robot in a complex dynamic environment. All calculation processes are based on quantitative evaluation, with accurate and consistent expressions, meeting the requirements of the invention specification.

[0072] In the confidence evaluation module, the quantification value of sonar penetration ability and the quantification value of visual resolution ability are extracted from the sonar-visual fusion feature matrix, combined with the obstacle movement parameters, and the confidence parameter is calculated using the Logistic regression method. This parameter quantifies the reliability of sonar and visual data fusion in the current environment. However, only the confidence parameter is not sufficient to directly generate a feasible obstacle avoidance path. It is also necessary to divide the sensitive area according to the environmental complexity and incorporate the robot dynamics constraints into the path planning to ensure that the path can avoid obstacles and meet the torque limit of the robot thruster. Therefore, the path planning module divides the sensitive area based on the confidence parameter output by the confidence evaluation module, embeds a curvature-torque mapping function in the A* algorithm, and iteratively adjusts the curvature of the path nodes within the torque threshold to generate a candidate path.

[0073] Embodiment 2, the following is the specific implementation manner of the path planning module: As Figure 2 shown, the processing flow of sensitive area division is: The processing of sensitive area division starts with gridifying the environmental space according to the confidence parameter. The environmental space is divided into multiple grid cells, each corresponding to a confidence parameter value, which reflects the reliability of sonar and visual data fusion in that area. The higher the confidence parameter value, the higher the credibility of the perception data and the lower the environmental complexity; the lower the confidence parameter value, the higher the environmental complexity, and a larger sensitive area is required to enhance obstacle avoidance safety. The radius of the sensitive area is calculated based on the confidence parameter. Specifically, the preset reference radius is divided by the confidence parameter value to obtain the radius of the sensitive area. When the confidence parameter value approaches 1, the radius of the sensitive area approaches the reference radius; when the confidence parameter value is low, the radius of the sensitive area increases to ensure that the underwater robot can avoid obstacles more conservatively in areas with low reliability of perception data. For each obstacle position, a spherical area centered on the obstacle with the radius of the sensitive area is defined as the sensitive area. All grid cells falling within the sensitive area are marked as high-risk areas, and special attention should be paid to avoiding these high-risk areas during path planning.

[0074] The sensitive area is dynamically divided according to the confidence parameter, and the confidence parameter comes from the reliability evaluation of multi-modal sensor data (sonar and vision). Specifically: Confidence parameter calculation: The confidence evaluation module extracts the quantified values of sonar penetration ability and visual resolution ability from the sonar-vision fusion feature matrix, and combines the obstacle motion parameters to generate the confidence parameter (range 0 to 1) through the Logistic regression model.

[0075] Definition of sensitive area: The path planning module calculates the radius of the sensitive area according to the confidence parameter. The formula is the reference radius divided by the confidence parameter value. When the confidence is low, the radius of the sensitive area increases, and vice versa. Centered on the obstacle, the sensitive area is defined as the spherical area within the radius.

[0076] Marking of high-risk areas: Grid cells falling within the sensitive area are marked as high-risk areas and should be avoided first during path planning.

[0077] In path planning, the A* algorithm searches for the optimal path through the cost evaluation of grid cells. The cost of high-risk areas is set to be significantly higher than that of other areas, forcing the A* algorithm to generate candidate paths that avoid these areas. The sensitive area and high-risk areas directly guide the generation of candidate paths, ensuring that the path avoids areas with low perception reliability or dense obstacles, thus guaranteeing safety.

[0078] The processing logic of embedding curvature-moment mapping in the A* algorithm: The A* algorithm searches for the optimal path from the start point to the end point by minimizing the path cost function in path planning. The path cost function consists of two parts: the actual cost and the heuristic estimated cost. To ensure that the path meets the robot's dynamic constraints, the path planning module embeds a curvature - torque mapping function in the A* algorithm. The curvature - torque mapping function correlates the curvature of the path nodes with the torque required by the robot's thrusters. Specifically, it multiplies the curvature and its rate of change of the path nodes by the preset dynamic model parameters to obtain the torque value. The curvature of the path nodes is calculated based on the geometric relationship between the node and its adjacent front and rear nodes, reflecting the degree of bending of the path; the rate of change of the curvature is calculated based on the change of the path curvature over time, reflecting the requirements of the robot's dynamic response. The maximum torque threshold of the robot's thrusters is preset, and the path nodes exceeding this maximum torque threshold are considered infeasible. During the path expansion process of the A* algorithm, for each candidate node, its curvature and the corresponding torque value are calculated. If the torque value exceeds the maximum torque threshold, the candidate node is marked as infeasible and no further path expansion is performed; if the torque value is within the maximum torque threshold, the candidate node is retained and the expansion continues.

[0079] Since the traditional A* algorithm only considers the geometric path cost and does not take into account the robot's dynamic limitations, it may generate paths that the robot cannot execute. By correlating curvature with torque, the path nodes that are dynamically infeasible are directly eliminated during the path planning stage, ensuring that the generated path conforms to the robot's motion capabilities. Furthermore, it improves the practicality and safety of path planning, avoids the robot out of control or thruster overload caused by excessive path curvature, and enhances the autonomous navigation ability of the underwater robot in complex environments.

[0080] The processing logic for iteratively adjusting the curvature of path nodes: The processing 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 obstacle avoidance in sensitive areas are considered, and no curvature constraint is imposed. Next, for each node in the initial path, its curvature and the corresponding torque value are calculated, and the nodes with torque values exceeding the maximum torque threshold are identified, which are called torque - exceeding nodes. For each torque - exceeding node and its adjacent front and rear nodes, local smoothing adjustment is performed. Specifically, the node position is adjusted by interpolation to reduce the local curvature until the torque value of the node drops 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 requirements of the maximum torque threshold or the number of iterations reaches the upper limit.

[0081] The technical feature of iteratively adjusting the curvature of path nodes lies in that the initial path may have areas with excessive local curvature, resulting in torque exceeding the limit. By iteratively adjusting, the path is gradually optimized to ensure that all nodes meet the dynamic constraints. The generated candidate path can effectively avoid sensitive areas and ensure that the torque of the robot's thrusters is within the controllable range, improving the executability and safety of the path.

[0082] Processing logic for generating candidate paths: The generation process of the candidate path starts from integrating the path after iterative adjustment to generate the final candidate path. This candidate path effectively avoids obstacles within the sensitive area, and at the same time, the curvature and torque of all nodes meet the dynamic constraints of the robot. The candidate path, as the output of the path planning module, provides a safe and feasible path plan for the path verification of the path optimization module.

[0083] The path planning module generates a candidate path based on the confidence parameter, through sensitive area division and embedding the curvature-torque mapping function into the A* algorithm. This path effectively avoids obstacles within the sensitive area and meets the torque threshold of the robot's thrusters. However, the candidate path may have problems such as discontinuous local curvature or torque mutation, resulting in the path being not smooth during actual execution or causing the robot's dynamic instability. Therefore, the path optimization module generates a global obstacle avoidance path by verifying the curvature continuity of the candidate path and eliminating torque mutation, ensuring that the path meets the physical field perception and dynamic constraints, and providing a safe and efficient navigation plan for the underwater robot.

[0084] Sensitive areas and high-risk areas, as the input of path planning, limit the feasible search space of the A* algorithm, directly affecting the geometric shape and dynamic characteristics of the candidate path. Therefore, there is a clear causal relationship between sensitive areas and high-risk areas and the candidate path.

[0085] The path optimization module further processes the candidate path to eliminate curvature discontinuity and torque mutation, ensuring the smoothness and executability of the path. Sensitive areas and high-risk areas still play a key role in this process: 1. Goals and constraints of path optimization.

[0086] The goal of the path optimization module is: Curvature continuity verification: Adjust the nodes with discontinuous curvature through cubic spline interpolation.

[0087] Torque mutation elimination: Adjust the nodes with torque mutation through Bezier curves.

[0088] However, the optimization process is not carried out independently, but is restricted by sensitive areas and high-risk areas. For example: When adjusting the position of path nodes, it is necessary to ensure that the adjusted path does not enter high-risk areas, otherwise it will lead to obstacle avoidance failure. The optimized path should maintain the same safety as the candidate path, that is, it should not approach sensitive areas due to smoothing.

[0089] Physical field constraints include the perception information of sensitive areas and high-risk areas to ensure that the optimized path does not sacrifice safety.

[0090] 2. Balance between safety and executability.

[0091] Sensitive areas and high-risk areas are not only the starting point of path planning but also run through the optimization process to ensure that the path can still avoid dangerous areas while being smoothed. This deep coupling reflects the balance between safety and executability in the invention. For example, if high-risk areas are ignored during optimization, the adjusted path may enter dangerous areas, increasing the risk of collision. By retaining the constraints of sensitive areas, the optimized path achieves an optimal balance between smoothness and safety.

[0092] Embodiment 3: The following is the specific implementation manner of the path optimization module: Processing logic for curvature continuity verification: The processing of curvature continuity verification starts from the node positions of the candidate path. The 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 between all nodes, avoiding sharp turns or non-smooth segments in the path to ensure the stability of the underwater robot's movement. For this purpose, for each node, its local curvature is calculated based on the geometric relationship between this node and its adjacent front and rear 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, and obtain the curvature value through the direction change and length relationship of these two vectors. The curvature value reflects the degree of bending of the path at this node. Next, the difference between the curvature values of adjacent nodes is calculated, and the relationship between the difference and the time interval or distance is combined to obtain the curvature change rate. If the curvature change rate exceeds the preset curvature change threshold, this node is marked as a curvature discontinuous node. The curvature change threshold is determined according to the dynamic model of the underwater robot to ensure that the path curvature change is within the controllable range of the robot.

[0093] Processing logic for smoothing curvature discontinuous nodes: The processing procedure for smoothing curvature-discontinuous nodes performs local adjustments on the nodes marked as curvature-discontinuous. The specific method is to use the adjacent nodes before and after the curvature-discontinuous node as a reference and adopt cubic spline interpolation to adjust the position of the curvature-discontinuous node. Cubic spline interpolation constructs a smooth curve connecting the previous node, the curvature-discontinuous node, the next node, and the node after that to generate a smooth path segment. The shape of the interpolation curve is controlled by interpolation parameters. By optimizing the interpolation parameters, the curvature change rate of the adjusted path segment between adjacent nodes is reduced to within the curvature change threshold. Repeat this process until all nodes meet the curvature continuity condition to generate a smooth path. This smoothing method can effectively reduce local curvature changes and ensure a smooth transition of the path curvature.

[0094] Through local interpolation adjustment, it is possible to eliminate curvature discontinuities without changing the overall structure of the path, ensuring the continuity of the path curvature. The generated smooth path can reduce sharp changes during the movement of the underwater robot, improve the smoothness and executability of the path, and at the same time maintain the obstacle avoidance effect of the path to ensure safe navigation in a complex underwater environment.

[0095] The processing logic for detecting torque mutations: The processing procedure for detecting torque mutations starts from the nodes of the smooth path and aims to identify torque mutations caused by curvature or the curvature change rate to ensure that the thrusters of the underwater robot do not become unstable due to drastic torque changes during path execution. For this purpose, for each node, its torque value is calculated based on the curvature-torque mapping relationship established in the previous step. The torque value consists of two parts: one part is the product of the curvature value and a preset curvature-torque proportionality coefficient, representing the direct influence of curvature on torque; the other part is the product of the curvature change rate and a preset dynamic influence coefficient, representing the dynamic influence of curvature change on torque. The curvature change rate is calculated by dividing the difference in curvature values between adjacent nodes by the time interval. Next, the difference in torque values between adjacent nodes is calculated and combined with the time interval to obtain the torque change rate. If the torque change rate exceeds the preset torque change threshold, the node is marked as a torque mutation node. The torque change threshold is determined based on the performance of the underwater robot's thrusters to ensure that the torque change is within the tolerable range of the thrusters.

[0096] When the underwater robot executes a path, torque mutations can cause a sharp change in the thruster load, leading to dynamic instability or equipment damage. By detecting torque mutations, it is ensured that the path torque changes smoothly, avoiding thruster overload and enhancing the safety and reliability of the path.

[0097] The processing logic for eliminating torque mutations: The processing procedure for eliminating torque mutations performs local adjustments on the nodes marked as torque mutations. The specific method is to use Bezier curves to adjust the path segments based on the adjacent nodes before and after the torque mutation node. The Bezier curve constructs a smooth curve that connects the previous node, the control point, and the next node to generate a smooth path segment. The position of the control point is optimized and adjusted so that the torque change rate between adjacent nodes of the adjusted path segment is reduced to within the torque change threshold. Repeat this process until all nodes meet the torque continuity condition, ensuring a smooth transition of the path torque. This adjustment method can effectively reduce local torque changes and ensure the continuity of the path torque.

[0098] Through local curve adjustment, it is possible to eliminate torque mutation points without changing the overall structure of the path, ensuring the continuity of the path torque.

[0099] Global obstacle avoidance path output: The generation process of the global obstacle avoidance path starts from the integrated and adjusted path to generate the final global obstacle avoidance path. This path ensures the smoothness of curvature and torque through curvature continuity verification and torque mutation elimination, while meeting the perception constraints and dynamic constraints of physical field correction in the previous steps. The global obstacle avoidance path, as the output of the path optimization module, provides a safe, smooth, and executable navigation path for the underwater robot.

[0100] The technical feature reason for the global obstacle avoidance path output is that by comprehensively considering curvature continuity, torque continuity, and physical field and dynamic constraints, the generated path can achieve efficient obstacle avoidance and safe navigation in complex underwater environments. Its advantage is that the path not only meets the geometric obstacle avoidance requirements but also conforms to the motion ability of the underwater robot and the performance of the thrusters, ensuring that it will not fail due to dynamic limitations when executing the path, and improving the reliability and robustness of autonomous navigation.

[0101] The path optimization module generates a global obstacle avoidance path by verifying the curvature continuity of the candidate path and eliminating torque mutations. This path ensures the smoothness of curvature and torque through cubic spline interpolation and Bezier curve adjustment, meets the dual constraints of physical field perception and dynamics, and provides a safe, smooth, and executable navigation path for the underwater robot in complex environments, completing the full-link optimization from perception to motion control.

[0102] The system architecture of the present invention forms a closed loop from multi-modal perception to path optimization. The sensitive areas and high-risk areas are the key links connecting perception and control: Perception stage: Data collection, physical correction, and feature fusion generate confidence parameters to divide sensitive areas and high-risk areas.

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

[0104] Optimization stage: The path optimization module adjusts the path under the constraint of high-risk areas to ensure global obstacle avoidance effect.

[0105] This dynamically adaptive design enables the system to cope with changes in complex underwater environments. Sensitive areas and high-risk areas run through the whole process and are inseparable from candidate paths and path optimization.

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

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

[0108] The present invention realizes safe and efficient navigation of an underwater robot in a complex environment by deeply coupling sensitive areas and high-risk areas with path planning and optimization, fully reflecting the collaborative innovation of perception and control.

[0109] The above formulas are all dimensionless and take their numerical values for calculation. The formulas are obtained by collecting a large amount of data for software simulation to get a formula closest to the actual situation. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0110] It should be noted that the system of the present invention can be deployed on the device itself to achieve embedded applications, or can also run on a PC or other terminals with a user interface, so as to meet various hardware environments and usage requirements.

[0111] Only some exemplary embodiments of the present invention have been described by way of illustration above. Undoubtedly, for those of ordinary skill in the art, without departing from the spirit and scope of the present invention, the described embodiments can be modified in various different ways. Therefore, the above drawings and descriptions are illustrative in nature and should not be construed as limiting the scope of protection of the claims of the present invention.

[0112] It should be noted that in this article, if there are relational terms such as first and second, 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 "comprising", "including" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or also includes elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "including one..." does not exclude the existence of another identical element in the process, method, article or device including the element.

[0113] As described above, it is only the specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present application can easily think of changes or substitutions, which should all be covered within the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claimed rights.

Claims

1. An underwater robot autonomous obstacle avoidance and path planning system based on multimodal sensors, characterized in that: The method comprises the steps of: 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 reference; Physical correction module: solves the Navier-Stokes equations for the sonar data of each alignment 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; 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: Calculate the quantitative value of sonar penetration and visual resolution based on the sonar-vision fusion feature matrix, and obtain confidence parameters by comprehensive analysis combined with obstacle motion parameters; Path planning module: divides sensitive areas according to confidence parameters, embeds the curvature-moment mapping function into the A* algorithm, and iteratively adjusts the path node curvature to within the moment threshold to obtain candidate paths; Path optimization module: Verify the curvature continuity of candidate paths and eliminate torque mutations, and output 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: Generate three-dimensional sonar point cloud data reflecting the geometric outline of obstacles through sonar sensors, and generate binocular vision RGB-D data containing color information and depth information through binocular vision sensors; use a time synchronization mechanism to align sonar point cloud data and binocular vision RGB-D data to a unified time reference to ensure that the sonar point cloud data and binocular vision RGB-D data reflect the underwater environment status at the same time; convert the binocular vision RGB-D data from the camera coordinate system to the robot coordinate system to achieve spatial alignment with the sonar point cloud data; divide the continuous sonar point cloud data stream and binocular vision RGB-D data stream into discrete processing units at fixed time intervals, each processing unit contains a subset of sonar point cloud data and a subset of 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 stereo 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 the sonar point cloud data, the propagation process of sound waves in the fluid medium is simulated, 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 contours 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 the 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 data set.

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 data set 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 the convolutional neural network, and uses the attention mechanism to perform weighted fusion on 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 loss function of the generator is composed 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 ​​recognized as a generated feature by the discriminator; the loss function of the discriminator is used to optimize the ability to distinguish between the real feature matrix and the sonar-vision fusion feature matrix; after the 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 intensity of the sonar feature subset, and generate a quantitative value of sonar penetration capability in combination with the attenuation coefficient; Extract vision-related feature subsets from the sonar-vision fusion feature matrix, calculate the overall gradient strength of the visual feature subset, and generate a quantitative value of visual resolution ability in combination with the light wave scattering distortion coefficient; 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 the 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 between 0 and 1 to generate the 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: The sensitive area is divided according to the confidence parameter, and the environmental space is divided into grid units. 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, and the grid units 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, eliminate the candidate nodes whose torque values ​​exceed the maximum torque threshold, and retain the candidate nodes whose torque values ​​are within the maximum torque threshold. 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, and the candidate path is generated.

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