Method and apparatus for autonomous obstacle avoidance for underwater vehicles based on machine vision

Abstract

The application discloses a kind of method and device for autonomous obstacle avoidance of submersible based on machine vision, and it relates to submersible technical field.The method comprises: collecting underwater environment data, generating environment three-dimensional point cloud, and extracting the spatial position and envelope boundary of obstacle in the environment three-dimensional point cloud, and constructing local environment map;Based on local environment map, using improved artificial potential field method, combined with the current position and target point of submersible, calculate the resultant vector, generate initial discrete path point sequence containing obstacle avoidance action;Using vertical bisection plane optimization algorithm, the initial discrete path point sequence is carried out path smoothing processing, and the optimized smooth path of submersible autonomous obstacle avoidance is obtained;The optimized smooth path is converted into navigation coordinates, and the propulsion system of submersible is controlled to track the optimized smooth path to execute submersible autonomous obstacle avoidance.The application solves the problems of low underwater environment perception accuracy, path planning easy to fall into local minimum value and the generated path is not smooth in the prior art.

Description

Technical Field

[0001] This invention relates to the field of submersible technology, and in particular to a method and apparatus for autonomous obstacle avoidance by a submersible based on machine vision. Background Technology

[0002] With the increasing demand for marine resource development and marine tourism, submersibles are playing an increasingly important role in underwater operations. Due to the complexity of the underwater environment, such as insufficient lighting, turbid water, and various shapes of obstacles, the autonomous obstacle avoidance capability of submersibles has become a key indicator for measuring their level of intelligence.

[0003] Currently, underwater obstacle avoidance environmental perception mainly faces the following two technical challenges: On the one hand, traditional obstacle avoidance methods rely heavily on sonar sensors. Although sonar uses sound waves for propagation and has the advantages of being unaffected by water turbidity and having a long detection range, it has inherent physical limitations: sonar resolution is relatively low, making it difficult to accurately depict the contours of complex obstacles at close range, and the generated point clouds are sparse and have blurred edges; in addition, sonar has blind spots when detecting at close range, and path planning based directly on sonar data often results in overly conservative and coarse paths, which are difficult to meet the needs of submersibles to navigate precisely among complex obstacles.

[0004] On the other hand, while obstacle avoidance solutions based on binocular machine vision can acquire rich environmental texture information and generate high-density 3D point clouds through stereo matching, achieving extremely high contour recognition accuracy in clear water, in turbid water, due to the strong scattering and absorption effect of suspended particles on light (i.e., light curtain effect), traditional passive vision systems will quickly become "blinded," resulting in extremely low image contrast, loss of features, inability to effectively generate 3D point clouds, and complete loss of obstacle avoidance perception capabilities.

[0005] To overcome the limitations of single sensors, existing technologies have attempted to simply combine sonar with optical cameras. However, this simple combination suffers from significant technical bottlenecks: First, traditional optical cameras still cannot form images in turbid water, causing the so-called "fusion" to degenerate into pure sonar obstacle avoidance in poor water conditions, failing to truly solve the problem of optical sensing failure; Second, acoustic point clouds and optical point clouds differ greatly in data structure, density, and noise models, lacking effective spatiotemporal registration and feature-level fusion mechanisms, easily resulting in "ghosting" or "confidence conflicts" at obstacle boundaries, causing subsequent path planning algorithms to fail due to map inaccuracies.

[0006] At the path planning algorithm level, the traditional artificial potential field method is widely used due to its simple calculation and good real-time performance. However, the traditional artificial potential field method suffers from the problems of "target unreachability" and "local minima". When there are obstacles near the target point or the submersible is trapped in the potential field, it is difficult to plan an effective path. In addition, the paths generated by conventional planning algorithms are mostly polyline segments, which do not conform to the kinematic characteristics of the submersible. Direct tracking will lead to frequent acceleration and deceleration, affecting control stability and energy efficiency.

[0007] Therefore, how to overcome the bottleneck of optical imaging in turbid water, construct a high-precision acoustic-optical fusion three-dimensional environmental map to achieve high-precision environmental perception in the entire water area, and combine improved path planning algorithms to solve the problems of local minima, unreachable targets, and non-smooth paths in traditional methods has become a technical problem that urgently needs to be solved in the field of underwater robots. Summary of the Invention

[0008] This invention provides a machine vision-based autonomous obstacle avoidance method and device for submersibles. This invention solves the problems of low accuracy in underwater environment perception, easy getting trapped in local minima in path planning, and unsmooth generated paths in existing technologies.

[0009] In a first aspect, embodiments of the present invention provide a machine vision-based autonomous obstacle avoidance method for submersibles, the method comprising: The underwater environment data is collected using forward-looking sonar and binocular machine vision system. A three-dimensional point cloud of the environment is generated through an acoustic-optical fusion algorithm. The spatial position and envelope boundary of obstacles in the three-dimensional point cloud of the environment are extracted to construct a local environment map. Based on the local environment map, the improved artificial potential field method is used to calculate the resultant force vector by combining the current position of the submersible with the target point, and generate an initial discrete path point sequence containing obstacle avoidance actions. The vertical bisecting plane optimization algorithm is used to smooth the initial discrete path point sequence to obtain the optimized smooth path for the submersible's autonomous obstacle avoidance. The optimized smooth path is converted into navigation coordinates, and the submersible's propulsion system is controlled to track the optimized smooth path to perform autonomous obstacle avoidance.

[0010] The technical solution provided in this application has at least the following beneficial effects: By introducing a multimodal fusion perception system combining forward-looking sonar and range-gated binocular vision, the perception bottleneck in turbid waters has been completely overcome. Range gating technology precisely filters backscattered light through a time window, enabling the optical system to maintain "automatic imaging" capabilities even in turbid water. The acoustic-optical point cloud fusion algorithm combines the advantages of sonar's "long-range vision" and optics' "clear vision" to generate high-precision 3D maps with confidence levels. This achieves a significant improvement over traditional pure sonar obstacle avoidance in terms of contour accuracy and planning quality across all water environments. An improved artificial potential field method is used for path planning, incorporating target distance adjustment into the repulsion force calculation. The factor and gravity-adjusted direction unit vector effectively solve the problem of target unreachability in the traditional artificial potential field method. At the same time, a virtual target point escape mechanism is introduced. When the resultant force is too small and falls into a local minimum, it can break the balance and guide the submersible to bypass the trap area, improving the success rate and robustness of path planning. The vertical bisecting plane optimization algorithm is introduced to smooth the initial discrete path. By constructing a two-dimensional search space and a comprehensive fitness function, the path length and smoothness are optimized under the premise of ensuring obstacle avoidance safety. This makes the generated path more in line with the kinematic characteristics of the submersible and improves the stability of tracking control.

[0011] In one alternative implementation, underwater environmental data is acquired using forward-looking sonar and a binocular machine vision system. A three-dimensional point cloud of the environment is generated through an acoustic-optical fusion algorithm. The spatial locations and envelope boundaries of obstacles in the three-dimensional point cloud are extracted to construct a local environmental map, including: The forward-looking sonar onboard the submersible is used to acquire acoustic distance data of the underwater environment and generate corresponding coarse acoustic point clouds. Using a binocular machine vision system mounted on a submersible, underwater optical image data after water scattering suppression is acquired, and image denoising enhancement and optical stereo matching are performed to generate corresponding optical point clouds. Based on the spatiotemporal synchronization relationship, the acoustic coarse point cloud and the optical fine point cloud are spatially registered and feature-level fused to generate an environmental 3D point cloud that takes into account both long-distance perception and short-distance high precision. Clustering algorithms are used to extract obstacles and model their envelopes from 3D point clouds of the environment, resulting in the spatial location and envelope boundaries of the obstacles. Map the spatial locations and envelope boundaries of all obstacles to a raster map to construct a local environment map that includes restricted areas.

[0012] In one alternative implementation, a binocular machine vision system mounted on a submersible is used to acquire underwater optical image data after water scattering suppression, and image denoising enhancement and optical stereo matching are performed to generate a corresponding optical point cloud, including: The distance gating synchronization control unit of the binocular machine vision system on the submersible is used to control the pulsed laser to emit high-frequency laser pulses toward the target water area; Within a preset time window during which the laser pulse is reflected back by an obstacle, the shutter of the binocular camera is opened for exposure, filtering backscattered light generated by suspended particles in the water and acquiring underwater optical image data that penetrates the turbid water. The intrinsic parameter matrix, distortion coefficients and relative pose relationship of the binocular camera are obtained by using Zhang Zhengyou's calibration method. Based on the calibration parameters, the left and right views in the underwater optical image data are stereo corrected so that the corresponding points of the left and right views are located on the same horizontal row, and the stereo corrected underwater image is obtained. The stereo-corrected underwater image is then denoised and enhanced to obtain a denoised and enhanced underwater image. A semi-global matching algorithm is used to calculate the disparity map of the denoised and enhanced underwater image. Based on the pinhole camera model, the three-dimensional spatial coordinates of the pixels are calculated in reverse using the triangulation principle according to the disparity value of the disparity map, and the corresponding optical point cloud is generated.

[0013] In one alternative implementation, a clustering algorithm is used to extract obstacles and model their envelopes from the 3D point cloud of the environment, obtaining the spatial location and envelope boundaries of the obstacles, including: The statistical outlier filter is used to remove isolated noise points caused by matching errors in the 3D point cloud of the environment, and the voxel mesh filter is used to downsample the 3D point cloud of the environment to obtain the filtered 3D point cloud of the environment. The RANSAC algorithm is used to fit the equation of the horizontal plane, and based on the equation of the horizontal plane, the background point cloud in the filtered 3D point cloud of the environment is removed to obtain the obstacle point cloud. The DBSCAN algorithm is used to segment the obstacle point cloud, and the segmented point cloud clusters are identified as independent obstacles, resulting in several clustered obstacle point cloud clusters. For each cluster of obstacle point cloud, calculate the corresponding minimum bounding box, and based on the minimum bounding box, extract the corresponding geometric center coordinates and envelope size to obtain the spatial position and envelope boundary of the obstacle.

[0014] In one alternative implementation, based on a local environment map, an improved artificial potential field method is used to calculate the resultant force vector by combining the current position of the submersible with the target point, generating an initial discrete path point sequence that includes obstacle avoidance actions, including: The current position of the submersible and the target point are determined, and the key parameters of the potential field algorithm are set. The key parameters of the potential field algorithm include the gravitational gain coefficient, the repulsive gain coefficient, the radius of influence of obstacles, the path planning step size, and the target distance threshold. Based on the key parameters of the potential field algorithm, the gravitational vector from the current position of the submersible to the target point is calculated. Access the local environment map and use a spatial search algorithm to obtain a set of information on all obstacles within the submersible's current sensing range; For each obstacle in the obstacle information set, calculate the nearest distance from the current position of the submersible to the surface of the obstacle and its gradient direction based on its envelope boundary in the local environment map; Based on the key parameters of the potential field algorithm, the nearest distance and its gradient direction are substituted into the improved repulsive force formula to calculate the repulsive force vector generated by each obstacle. The total repulsive force is obtained by vector superposition of the repulsive forces generated by all obstacles in the local environment map. Calculate the resultant force vector acting on the submersible based on the gravitational vector from the submersible's current position to the target point and the total repulsive force exerted on the submersible by the obstacle; If the resultant force is less than the preset resultant force threshold, the virtual target point escape mechanism is activated. The system returns to the direction perpendicular to the current resultant force or the tangent to the obstacle, and generates a temporary virtual target point in the local environment map. The virtual target point temporarily replaces the target point in the gravity calculation, breaks the force balance, and guides the submersible around the trap area. After the escape is successful, the real target point is restored; otherwise, the system proceeds to the next step. Based on the key parameters of the potential field algorithm, the direction of the resultant force vector is normalized to obtain the next movement direction vector, and the position of the next path point is calculated. Update the current position, return to the gravity vector calculation step, until the distance between the current position and the target point is less than the target distance threshold or the current iteration number exceeds the maximum iteration number, and generate an initial discrete path point sequence containing obstacle avoidance actions.

[0015] In one alternative implementation, a vertical bisecting plane optimization algorithm is used to smooth the initial discrete path point sequence, resulting in an optimized smooth path for the submersible's autonomous obstacle avoidance, including: The initial discrete path point sequence is downsampled to remove redundant collinear points, resulting in a sparse node set that retains key turning points. For each adjacent path pair in the sparse node set, calculate the midpoint of the line connecting the path pairs, construct a perpendicular bisector plane as the feasible solution space, and decompose the feasible solution space into two orthogonal dimensions to construct the two-dimensional parametric equations of the two-dimensional search space. Construct a fitness function for the perpendicular bisector plane optimization algorithm and encode the three-dimensional spatial coordinates of the insertion point as the position vector of the individual in the perpendicular bisector plane optimization algorithm; Based on the fitness function, the vertical bisecting plane optimization algorithm is used to search for the optimal insertion point for each adjacent path point pair in the sparse node set, and the optimal insertion point for the adjacent path point pair is obtained. Feasibility pruning is performed on the optimal insertion point of all adjacent path point pairs. Optimal insertion points located inside obstacles or not satisfying kinematic constraints are removed. The remaining optimal insertion points are inserted into the sparse node set, replacing the original straight-line connection segments, and reconstructing and generating the optimized smooth path for autonomous obstacle avoidance of the submersible.

[0016] In one alternative implementation, the fitness function is formulated as follows:

[0017] In the formula, For the first i Insertion point position fitness value; For the first i Insertion point position The path length connecting corresponding adjacent path points; For the first i Insertion point position Safety risk items; For the first i Insertion point position The depth smoothness constraint term; These are the weight coefficients for the fitness function.

[0018] In one alternative implementation, based on the fitness function, a vertical bisecting plane optimization algorithm is used to search for the optimal insertion point for each adjacent path pair in the sparse node set, obtaining the optimal insertion point for the adjacent path pair, including: The chaotic sequence is generated using the Logistic mapping, and then mapped to a two-dimensional search space to obtain the initial population. Using a fitness function, the fitness of each initial individual in the initial population is calculated. Based on the gray wolf cooperative idea, the top three individuals with the highest fitness, the second best individuals, and the third best individuals are selected from the initial population. The other initial individuals are ordinary individuals. The population converges by using the best, second-best, and third-best individuals. The position of all ordinary individuals in the initial population is updated based on the two-dimensional search space parameter equation to obtain the updated population. For each updated individual in the updated population, perform dimensional boundary correction, and force updated individuals that exceed the boundary range of the two-dimensional search space to be projected back into the two-dimensional search space. Using the fitness function, calculate the fitness of each updated individual in the updated population, and update the updated individual with the best fitness value as the best individual; The population position is updated repeatedly. When the number of iterations reaches the maximum number of iterations or the fitness value of the best individual meets the requirements, the iterative update of the population is terminated, and the position vector of the best individual is output. Decode the position vector of the optimal individual to obtain the optimal insertion point for adjacent path point pairs.

[0019] In one optional implementation, feasibility pruning is performed on the optimal insertion points of all adjacent path point pairs. Optimal insertion points located inside obstacles or not satisfying kinematic constraints are removed, and the remaining optimal insertion points are inserted into a sparse node set, replacing the original straight-line connection segments. This reconstructs and generates an optimized smooth path for the submersible's autonomous obstacle avoidance, including: Query the local environment map, calculate the safe Euclidean distance from the optimal insertion point to the nearest obstacle for each adjacent path point pair, determine the optimal insertion point with a safe Euclidean distance less than the envelope boundary as having a collision risk, perform feasibility pruning, remove the optimal insertion point located inside the obstacle, and obtain several optimal insertion points after feasibility pruning. After feasibility pruning, the maximum pitch angle constraint and maximum curvature constraint are checked for several optimal insertion points. The optimal insertion points that do not meet the kinematic constraints are removed, and the remaining optimal insertion points are obtained. The remaining optimal insertion points are inserted into the sparse node set, replacing the original straight-line connection segments, and the optimized smooth path for autonomous obstacle avoidance of the submersible is reconstructed.

[0020] Secondly, embodiments of the present invention provide a machine vision-based autonomous obstacle avoidance device for submersibles, used to implement an autonomous obstacle avoidance method for submersibles. The device includes: The machine vision acquisition unit is used to acquire underwater environmental data using forward-looking sonar and binocular machine vision system, generate a three-dimensional point cloud of the environment through an acoustic-optical fusion algorithm, and extract the spatial position and envelope boundary of obstacles in the three-dimensional point cloud of the environment to construct a local environmental map. The path point sequence generation unit is used to calculate the resultant force vector based on the local environment map, using the improved artificial potential field method, and combining the current position of the submersible with the target point, to generate an initial discrete path point sequence containing obstacle avoidance actions. The path smoothing unit is used to smooth the initial discrete path point sequence using the vertical bisecting plane optimization algorithm to obtain the optimized smooth path for autonomous obstacle avoidance of the submersible. The autonomous obstacle avoidance execution unit is used to convert the optimized smooth path into navigation coordinates and control the submersible's propulsion system to track the optimized smooth path and execute the submersible's autonomous obstacle avoidance.

[0021] A third aspect of this invention provides an electronic device, which includes: At least one processor; and a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by at least one processor, such that the at least one processor can perform the method proposed in the first aspect of the present invention.

[0022] A fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in the first aspect of the present invention. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of the electronic device structure of the hardware operating environment involved in the embodiments of the present invention; Figure 2 This is a flowchart illustrating the steps of an autonomous obstacle avoidance method for a submersible based on machine vision, provided in an embodiment of the present invention. Figure 3 This is a schematic diagram of the functional units of an autonomous obstacle avoidance device for a submersible based on machine vision, provided in an embodiment of the present invention. Detailed Implementation

[0024] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0025] The present invention will be further described below with reference to the accompanying drawings.

[0026] Reference Figure 1 , Figure 1 This is a schematic diagram of the electronic device structure of the hardware operating environment involved in the embodiments of the present invention.

[0027] like Figure 1As shown, the electronic device may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to enable communication between these components. The user interface 1003 may include a display screen or an input unit such as a keyboard; optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface). The memory 1005 may be a high-speed random access memory (RAM) or a stable non-volatile memory (NVM), such as a disk drive. The memory 1005 may also optionally be a storage device independent of the aforementioned processor 1001.

[0028] Those skilled in the art will understand that Figure 1 The structure shown does not constitute a limitation on the electronic device and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0029] like Figure 1 As shown, the memory 1005, which serves as a storage medium, may include an operating system, a data storage module, a network communication module, a user interface module, and an electronic program for a machine vision-based autonomous obstacle avoidance device for submersibles.

[0030] exist Figure 1 In the electronic device shown, the network interface 1004 is mainly used for data communication with the network server; the user interface 1003 is mainly used for data interaction with the user; the processor 1001 and the memory 1005 in the electronic device of the present invention can be set in the electronic device. The electronic device calls the electronic program of the machine vision-based autonomous obstacle avoidance device for submersibles stored in the memory 1005 through the processor 1001, and executes the machine vision-based autonomous obstacle avoidance method for submersibles provided in the embodiment of the present invention.

[0031] Reference Figure 2 The present invention provides a machine vision-based autonomous obstacle avoidance method for submersibles, the method comprising: S201: Use forward-looking sonar and binocular machine vision system to collect underwater environmental data, generate a three-dimensional point cloud of the environment through an acoustic-optical fusion algorithm, and extract the spatial position and envelope boundary of obstacles in the three-dimensional point cloud of the environment to construct a local environmental map. S202: Based on the local environment map, using the improved artificial potential field method, combined with the current position of the submersible and the target point, the resultant force vector is calculated to generate an initial discrete path point sequence containing obstacle avoidance actions; S203: The vertical bisecting plane optimization algorithm is used to smooth the initial discrete path point sequence to obtain the optimized smooth path for autonomous obstacle avoidance of the submersible. S204: Convert the optimized smooth path into navigation coordinates, and control the submersible's propulsion system to track the optimized smooth path to perform autonomous obstacle avoidance.

[0032] The technical solution provided in this application has at least the following beneficial effects: By introducing a multimodal fusion perception system combining forward-looking sonar and range-gated binocular vision, the perception bottleneck in turbid waters has been completely overcome. Range gating technology precisely filters backscattered light through a time window, enabling the optical system to maintain "automatic imaging" capabilities even in turbid water. The acoustic-optical point cloud fusion algorithm combines the advantages of sonar's "long-range vision" and optics' "clear vision" to generate high-precision 3D maps with confidence levels. This achieves a significant improvement over traditional pure sonar obstacle avoidance in terms of contour accuracy and planning quality across all water environments. An improved artificial potential field method is used for path planning, incorporating target distance adjustment into the repulsion force calculation. The factor and gravity-adjusted direction unit vector effectively solve the problem of target unreachability in the traditional artificial potential field method. At the same time, a virtual target point escape mechanism is introduced. When the resultant force is too small and falls into a local minimum, it can break the balance and guide the submersible to bypass the trap area, improving the success rate and robustness of path planning. The vertical bisecting plane optimization algorithm is introduced to smooth the initial discrete path. By constructing a two-dimensional search space and a comprehensive fitness function, the path length and smoothness are optimized under the premise of ensuring obstacle avoidance safety. This makes the generated path more in line with the kinematic characteristics of the submersible and improves the stability of tracking control.

[0033] In one alternative implementation, underwater environmental data is acquired using forward-looking sonar and a binocular machine vision system. A three-dimensional point cloud of the environment is generated through an acoustic-optical fusion algorithm. The spatial locations and envelope boundaries of obstacles in the three-dimensional point cloud are extracted to construct a local environmental map, including: S2011: Using the forward-looking sonar mounted on the submersible, acquire acoustic distance data of the underwater environment and generate corresponding coarse acoustic point clouds; S2012: Using a binocular machine vision system mounted on a submersible, underwater optical image data after water scattering suppression is acquired, and image denoising enhancement and optical stereo matching are performed to generate corresponding optical point clouds. S2013: Based on the spatiotemporal synchronization relationship, the acoustic coarse point cloud and the optical fine point cloud are spatially registered and feature-level fused to generate an environmental 3D point cloud that takes into account both long-distance perception and short-distance high precision. In this embodiment, the timestamps provided by the combined navigation of the Global Navigation Satellite System / Inertial Navigation System, as well as the extrinsic parameter calibration matrix of the sonar and camera, are used to transform the acoustic coarse point cloud and the range-gated optical fine point cloud to the same carrier coordinate system. Voxel meshes are used to uniformly downsample the registered dual-modal point cloud, and feature points of the optical fine point cloud are retained within the same voxel. If there is only acoustic coarse point cloud in the voxel, a lower spatial confidence weight is assigned to the point to generate an environmental 3D point cloud with confidence labels. S2014: Use clustering algorithms to extract obstacles and model the envelope of the 3D point cloud of the environment to obtain the spatial location and envelope boundary of the obstacles; S2015: Map the spatial location and envelope boundaries of all obstacles to a grid map to construct a local environment map that includes restricted areas; In this embodiment, a local grid map is established, three-dimensional obstacles are projected onto a two-dimensional plane and three-dimensional voxel information is retained, and restricted areas are marked on the map according to the envelope boundary of the obstacles, thus completing the construction of the local environment map.

[0034] In one alternative implementation, a binocular machine vision system mounted on a submersible is used to acquire underwater optical image data after water scattering suppression, and image denoising enhancement and optical stereo matching are performed to generate a corresponding optical point cloud, including: S20131: The distance gating synchronization control unit of the binocular machine vision system on the submersible controls the pulsed laser to emit high-frequency laser pulses toward the target water area; S20132: Within the preset time window when the laser pulse is reflected back by the obstacle, the shutter of the binocular camera is controlled to open for exposure, filtering the backscattered light generated by suspended particles in the water body, and acquiring underwater optical image data that penetrates the turbid water body. S20133: The intrinsic parameter matrix, distortion coefficients and relative pose relationship of the binocular camera are obtained by using Zhang Zhengyou calibration method. The left and right views in the underwater optical image data are stereo corrected according to the calibration parameters so that the corresponding points of the left and right views are located on the same horizontal row, and the stereo corrected underwater image is obtained. In this embodiment, the Zhang Zhengyou calibration method is used to calculate the intrinsic parameter matrix (focal length, principal point coordinates), distortion coefficients, and relative pose relationship (rotation matrix and translation vector) between the two cameras by capturing images of the chessboard calibration board in different poses. Based on the calibration parameters, stereo correction (Bouguet algorithm) is performed on the left and right views to eliminate distortion and make the corresponding points of the left and right images lie on the same horizontal epipolar line, simplifying the subsequent matching search process. S20134: Perform image denoising and enhancement on the stereo-corrected underwater image to obtain a denoised and enhanced underwater image; S20135: A semi-global matching algorithm is used to calculate the disparity map of the underwater image after denoising and enhancement. Based on the pinhole camera model, the three-dimensional spatial coordinates of the pixels are calculated in reverse according to the disparity value of the disparity map and the corresponding optical point cloud is generated. In this embodiment, a semi-global matching algorithm is used to calculate the disparity map. The semi-global matching algorithm uses mutual information as the matching cost and combines dynamic programming in multiple directions to aggregate the cost, which has high computational efficiency while ensuring matching accuracy.

[0035] In one alternative implementation, a clustering algorithm is used to extract obstacles and model their envelopes from the 3D point cloud of the environment, obtaining the spatial location and envelope boundaries of the obstacles, including: S20141: Use a statistical outlier filter to remove isolated noise points in the 3D point cloud of the environment caused by matching errors, and use a voxel mesh filter to downsample the 3D point cloud of the environment to obtain the filtered 3D point cloud of the environment. In this embodiment, a statistical outlier filter is used to remove discrete noise points in the generated original point cloud. The filter calculates the average distance from each point to its nearest neighbors and removes points whose distance exceeds a set threshold. To improve subsequent processing efficiency, a voxel grid filter is used to downsample the point cloud. By setting a cubic grid, the centroid of all points in the grid is used to approximate the points in the grid, thereby reducing the amount of data while preserving the shape features of the point cloud. S20142: The Random Sample Consensus (RANSAC) algorithm is used to fit the equation of the horizontal plane, and based on the equation of the horizontal plane, the background point cloud in the filtered 3D point cloud of the environment is removed to obtain the obstacle point cloud. In this embodiment, considering the characteristics of the underwater environment, the RANSAC algorithm is used to fit the equation of the horizontal plane (such as the seabed plane or horizontal plane), identify and remove the background planar point cloud, and retain the point cloud of suspended or protruding obstacles. S20143: Use the Density-Based Spatial Clustering of Applications with Noise (DBSCAN) algorithm to segment the obstacle point cloud, identify the segmented point cloud clusters as independent obstacles, and obtain several clustered obstacle point cloud clusters; In this embodiment, the DBSCAN algorithm is used to segment the obstacle point cloud. This algorithm does not require a preset number of clusters and can cluster adjacent density reachable points into one class, thereby separating nearby obstacles into independent point cloud clusters. S20144: For each cluster of obstacle point cloud clusters, calculate the corresponding minimum bounding box, and based on the minimum bounding box, extract the corresponding geometric center coordinates and envelope size to obtain the spatial position and envelope boundary of the obstacle. In this embodiment, for each segmented obstacle point cloud cluster, its minimum bounding box is calculated. Principal component analysis is used to determine the principal axis direction of the point cloud, and an oriented bounding box is constructed. The geometric center of the bounding box is extracted as the obstacle position, and the length, width, and height are used as the envelope boundary.

[0036] In one alternative implementation, based on a local environment map, an improved artificial potential field method is used to calculate the resultant force vector by combining the current position of the submersible with the target point, generating an initial discrete path point sequence that includes obstacle avoidance actions, including: S2021: Determine the current position of the submersible and the target point, and set the key parameters of the potential field algorithm. The key parameters of the potential field algorithm include the gravitational gain coefficient, the repulsive gain coefficient, the radius of influence of obstacles, the path planning step size, and the target distance threshold. S2022: Based on key parameters of the potential field algorithm, calculate the gravitational vector from the submersible's current position to the target point. The formula is:

[0037] In the formula, It is the gravitational vector; Current position; This refers to the gravitational gain coefficient, a key parameter in the potential field algorithm. This represents the Euclidean distance from the current location to the target point. The threshold is the Euclidean distance. The unit vector pointing from the current position to the target point;

[0038] In the formula, For the target point; To find the norm sign; S2023: Access the local environment map, use a spatial search algorithm to obtain a set of information on all obstacles within the current submersible's perception range; S2024: For each obstacle in the obstacle information set, calculate the nearest distance from the current position of the submersible to the surface of the obstacle and its gradient direction based on its envelope boundary in the local environment map; S025: Based on the key parameters of the potential field algorithm, the nearest distance and its gradient direction are substituted into the improved repulsive force formula to calculate the repulsive force vector generated by each obstacle. The formula is as follows:

[0039] In the formula, For the firstj The repulsive force vector generated by an obstacle on the submersible; For the current position of the submersible to the number j The minimum Euclidean distance to each obstacle; The obstacle's influence radius, i.e., the safe distance threshold; This represents the Euclidean distance from the submersible's current position to the target point. This is the repulsive gain coefficient, used to control the strength of the repulsive field. The larger the obstacle, the stronger the repulsive force generated by the obstacle, and the farther the submersible will have to detour; conversely, the smaller the obstacle, the shorter the detour. Adjustments need to be made based on the submersible's maneuverability and the complexity of the environment. The adjustment factor exponent is a positive integer, taking [value]. ≥1, this parameter controls the target distance item. The influence weight of the repulsive force, when When the target distance is large, the weakening effect of the repulsive force is more significant, which is more conducive to solving the problem of target unreachability. When =0, the formula degenerates into the traditional artificial potential field method; The repulsive direction unit vector is defined as the direction in which the submersible moves away from the obstacle. This vector ensures that the repulsive force experienced by the submersible is always away from the obstacle. The unit vector for gravity adjustment direction is defined as the direction from the submersible to the target point. This vector ensures that the direction of the adjustment component points to the target point, playing a guiding role in "moving away from obstacles while moving towards the target". j For obstacle indication; The first part is the repulsive component. ,direction The size increases as the distance to the obstacle decreases and decreases as the distance to the target decreases. This ensures that the repulsive force of the obstacle will automatically weaken when the submersible approaches the target, thus solving the problem of the target being unreachable. The second part is the gravity adjustment component. ,direction Pointing to the target, guiding the submersible to maintain a tendency to approach the target while circling; Applicable to ≤ In the case of, if > The repulsive force is zero;

[0040] In the formula, For the first j For each obstacle, take its spatial coordinates. For extended obstacles, take the coordinates of its geometric center or the boundary point closest to the submersible.

[0041] In the formula, The spatial coordinates of the target point; S2026: The total repulsive force is obtained by vector superposition of the repulsive forces generated by all obstacles in the local environment map, using the following formula:

[0042] In the formula, Total repulsive force; The total number of obstacles; S2027: Based on the gravitational vector from the submersible's current position to the target point and the total repulsive force exerted on the submersible by the obstacles, calculate the resultant force vector acting on the submersible using the following formula:

[0043] In the formula, The resultant force vector; S2028: If the magnitude of the resultant force Less than the preset resultant force threshold If the target point escape mechanism is activated, the target point will return along the direction perpendicular to the current resultant force or the tangent to the obstacle, and a temporary virtual target point will be generated in the local environment map to temporarily replace the target point. It participates in gravitational calculations, disrupts the force balance, guides the submersible around the trap area, and restores the true target point after a successful escape. Otherwise, proceed to the next step; S2029: Based on the key parameters of the potential field algorithm, the direction of the resultant force vector is normalized to obtain the next movement direction vector, and the position of the next path point is calculated. The formula is as follows:

[0044] In the formula, The location of the next path point; This refers to the location of the path point corresponding to the current position. This is the direction vector for the next movement; Plan the step size for the path; S20210: Update the current position, return to the gravity vector calculation step, until the distance between the current position and the target point is less than the target distance threshold or the current iteration number exceeds the maximum iteration number, generate an initial discrete path point sequence containing obstacle avoidance actions, the formula is:

[0045] In the formula, The target distance threshold.

[0046] In one alternative implementation, a vertical bisecting plane optimization algorithm is used to smooth the initial discrete path point sequence, resulting in an optimized smooth path for the submersible's autonomous obstacle avoidance, including: S2031: Downsample the initial discrete path point sequence, remove redundant collinear points, and obtain a sparse node set that retains key turning points; In this embodiment, the initial path point sequence is traversed, redundant points that are collinear or nearly collinear are removed, and only the starting point, ending point and key turning points of the path are retained to form a sparse node set, thereby improving optimization efficiency. S2032: For each adjacent path pair in the sparse node set, calculate the midpoint of the line connecting the path pairs, construct a perpendicular bisector plane as the feasible solution space, and decompose the feasible solution space into two orthogonal dimensions to construct a two-dimensional parametric equation for the two-dimensional search space, the formula of which is:

[0047] In the formula, Insertion point position X The position vector, i.e., the coordinates of the candidate point on the perpendicular bisector plane; It is the midpoint of the line connecting adjacent path pairs in a sparse node set; The insertion point position variable corresponds to the individual position in the perpendicular bisector plane optimization algorithm; The reference search direction unit vector in the orthogonal dimension is defined as the direction of the intersection of the perpendicular bisector of the plane and the horizontal plane. It is used to find the shortest obstacle avoidance path in the horizontal plane, and the optimization variable is set as the position of the insertion point in the horizontal plane. Set its horizontal search boundary[ ], used for obstacle avoidance pruning in the horizontal plane; These are the minimum and maximum values ​​of the horizontal search boundary; The unit vector for adjusting the search direction in the orthogonal dimension is defined as the unit vector perpendicular to the reference search direction in the perpendicular bisector plane. The direction is used to fine-tune the depth to adapt to terrain undulations, and the optimization variable is set as the position of the insertion point in the vertical plane. Set its depth adjustment boundary[ ], used for smooth adaptation in the depth direction; The minimum and maximum values ​​of the depth adjustment boundary; S2033: Construct the fitness function for the perpendicular bisector plane optimization algorithm, and assign the three-dimensional spatial coordinates of the insertion point position. The position vector of an individual is encoded as an optimization algorithm for the perpendicular bisector plane; S2034: Based on the fitness function, the vertical bisecting plane optimization algorithm is used to search for the optimal insertion point for each adjacent path point pair in the sparse node set, and the optimal insertion point for the adjacent path point pair is obtained. S2035: Perform feasibility pruning on the optimal insertion point of all adjacent path point pairs, remove the optimal insertion points located inside obstacles or not satisfying kinematic constraints, and insert the remaining optimal insertion points into the sparse node set, replacing the original straight line connection segments, and reconstructing and generating the optimized smooth path for autonomous obstacle avoidance of the submersible.

[0048] In one alternative implementation, the fitness function is formulated as follows:

[0049] In the formula, For the first i Insertion point position fitness value; For the first i Insertion point position Find the path length of corresponding adjacent path points and minimize the total path length; For the first i Insertion point position The safety risk factor is to maximize the distance between the insertion point and the nearest obstacle; the closer the insertion point is to the obstacle, the greater the risk. For the first i Insertion point position The depth smoothness constraint term minimizes the rate of depth change between adjacent path segments to prevent the submersible from pitching violently. The smaller the depth change between adjacent path segments, the smoother the surface. These are the weight coefficients for the fitness function.

[0050] In one alternative implementation, based on the fitness function, a vertical bisecting plane optimization algorithm is used to search for the optimal insertion point for each adjacent path pair in the sparse node set, obtaining the optimal insertion point for the adjacent path pair, including: S20341: Use Logistic mapping to generate chaotic sequences and map the chaotic sequences to a two-dimensional search space to obtain the initial population; The formula is:

[0051] In the formula, For the first n+ 1. n There are several chaotic variables whose values ​​range from [0, 1]. The stability coefficient is typically 4. This sequence is ergodic and random, ensuring that the initial population is uniformly distributed in the solution space, avoiding getting trapped in local optima, which is superior to traditional random initialization.n For chaotic variable indicators;

[0052] In the formula, For the initial population, the first i An initial individual; For the first i One chaotic variable; These are the upper and lower bounds of the search space;

[0053]

[0054] S20342: Using the fitness function, calculate the fitness of each initial individual in the initial population, and based on the gray wolf cooperative idea, select the top three individuals with the best, second best, and third best fitness in the initial population, while the other initial individuals are ordinary individuals. S20343: Guide the population convergence with the best, second-best, and third-best individuals, and perform position updates on all ordinary individuals in the initial population based on the two-dimensional search space parameter equation to obtain the updated population. The formula is:

[0055] In the formula, For the first t+ In the first iteration of the population, the first i A newer individual; For the first t In the latent population of the next iteration i Each updated individual, in the initial iteration, For the initial population, the first i An initial individual; For the first t+ 1, t The first, second, and third potential movement vectors of the next iteration; For the first t The next iteration Distance vectors between the best individual, the second-best individual, and the third-best individual; This represents the vector of the first, second, and third control coefficients; These are the first, second, and third oscillation coefficients; A random number between [0, 1]; For control coefficients and oscillation coefficients; For the first t The convergence factor of the next iteration; t This represents the current iteration number; For the first tThe best, second-best, and third-best individuals in the next iteration;

[0056] In the formula, These are the maximum and minimum values ​​of the convergence factor; This is the threshold for the number of iterations; S20344: Perform dimensional boundary correction on each updated individual in the updated population, and force updated individuals that exceed the boundary range of the two-dimensional search space to be projected back into the two-dimensional search space. S20345: Using the fitness function, calculate the fitness of each updated individual in the updated population, and update the updated individual with the best fitness value as the best individual; S20346: Repeatedly update the position of the population. When the number of iterations reaches the maximum number of iterations or the fitness value of the best individual meets the requirements, terminate the iterative update of the population and output the position vector of the best individual. S20347: Decode the position vector of the optimal individual to obtain the optimal insertion point for adjacent path point pairs.

[0057] In one optional implementation, feasibility pruning is performed on the optimal insertion points of all adjacent path point pairs. Optimal insertion points located inside obstacles or not satisfying kinematic constraints are removed, and the remaining optimal insertion points are inserted into a sparse node set, replacing the original straight-line connection segments. This reconstructs and generates an optimized smooth path for the submersible's autonomous obstacle avoidance, including: S20351: Query the local environment map, calculate the safe Euclidean distance from the optimal insertion point to the nearest obstacle for each adjacent path point pair, determine that the optimal insertion point with a safe Euclidean distance less than the envelope boundary has a collision risk, perform feasibility pruning, remove the optimal insertion point located inside the obstacle, and obtain several optimal insertion points after feasibility pruning. In this embodiment, feasible pruning includes: Elimination strategy: Discard the insertion point directly and retain the straight line connection of the original sparse node pair (provided that the straight line segment has passed the security verification). Correction strategy: If it is necessary to bypass the obstacle, the point is retained, but this situation has been avoided in the fitness function through a penalty term; if any points are found to have slipped through the net during the pruning phase, they are forcibly removed. S20352: After feasibility pruning, perform maximum pitch angle constraint verification and maximum curvature constraint verification on several optimal insertion points, remove the optimal insertion points that do not meet the kinematic constraints, and obtain several optimal insertion points that are retained. In this embodiment, the maximum pitch angle constraint verification is performed as follows: Let the path vectors before and after the insertion point be... The formula for calculating the rate of change of depth is:

[0058] In the formula, The path pitch angle represents the angle of inclination of a path segment relative to the horizontal plane, usually expressed in degrees or radians. This angle reflects the degree of pitch maneuver required by the submersible when passing through the path point. The path vector after the insertion point The vertical component of the output path vector refers specifically to the coordinate difference of the vector from the current insertion point to the next path point on the Z-axis (depth axis). The path vector before the insertion point The vertical component of the input path vector in the text refers specifically to the coordinate difference of the vector from the previous path point to the current insertion point on the Z-axis (depth axis). for and The coordinate difference on the X-axis (horizontal vertical axis), combined with y The component is used to calculate the horizontal projection length of the path; for and The coordinate difference on the Y-axis (horizontal axis); It is the arctangent function, used to calculate the angle value based on the ratio of the opposite side (depth change) to the adjacent side (horizontal distance); like Maximum permissible pitch angle of submersible If the insertion point is deemed to cause the path to be too steep, it will be removed or a smooth transition will be performed at that point. In this embodiment, the turning angle (path deflection angle) at the insertion point is calculated. If the turning angle is too large, it means that the submersible needs to make a large-angle turn instantly, which does not meet the dynamic constraints. If the turning angle exceeds the threshold, the point needs to be removed or a transition point needs to be re-inserted near it to meet the turning radius requirements. S20353: Insert the remaining optimal insertion points into the sparse node set, replace the original straight-line connection segments, and reconstruct and generate the optimized smooth path for autonomous obstacle avoidance of the submersible.

[0059] In one alternative implementation, the optimized smooth path is converted into navigation coordinates, and the submersible's propulsion system is controlled to track the optimized smooth path to perform autonomous obstacle avoidance, including: S2041: Utilizing the installation position relationship (external parameter matrix) between the binocular camera and the submersible's center of gravity, each path point in the optimized smooth path is converted into coordinates in the submersible's coordinate system; S2042: Read the data from the attitude sensor (compass / inertial navigation system) on the submersible, obtain the current roll angle, pitch angle and yaw angle of the submersible, combine the current real-time position of the submersible in the navigation coordinate system, construct a transformation matrix, convert the path points in the carrier coordinate system into navigation coordinates in the navigation coordinate system, and generate a navigation path point sequence containing absolute latitude and longitude / planar coordinates and depth. In this embodiment, the navigation path point sequence is provided as input data to the subsequent "line-of-sight guidance law design" step to calculate the geometric deviation of the submersible from the desired path. S2043: Based on the navigation coordinate sequence, select the current target path segment and use the line-of-sight guidance method to calculate the yaw angle and pitch angle of the desired heading, i.e. the desired attitude angle; S2044: The motion controller of the submersible's propulsion system inputs the desired attitude angle and desired speed. The motion controller is used to calculate the attitude deviation based on the desired attitude angle and desired speed. S2045: Based on the attitude deviation, calculate the required control torque and thrust, and provide the control torque and thrust to the thrust distribution device of the submersible's propulsion system, indicating the physical force requirements for the submersible's movement; S2046: Based on control torque, thrust is distributed to each thruster of the propulsion system using a thrust distribution device, driving the submersible to navigate along a smooth path and complete autonomous obstacle avoidance tasks.

[0060] This invention also provides a machine vision-based autonomous obstacle avoidance device 300 for submersibles, see reference. Figure 3 The device may include the following units: The machine vision acquisition unit 301 is used to acquire underwater environmental data using forward-looking sonar and binocular machine vision system, generate a three-dimensional point cloud of the environment through an acoustic-optical fusion algorithm, and extract the spatial position and envelope boundary of obstacles in the three-dimensional point cloud of the environment to construct a local environmental map. The path point sequence generation unit 302 is used to calculate the resultant force vector based on the local environment map, using the improved artificial potential field method, and combining the current position of the submersible with the target point, and generate an initial discrete path point sequence containing obstacle avoidance actions. The path smoothing processing unit 303 is used to perform path smoothing processing on the initial discrete path point sequence using the vertical bisecting plane optimization algorithm to obtain the optimized smooth path for autonomous obstacle avoidance of the submersible. The autonomous obstacle avoidance execution unit 304 is used to convert the optimized smooth path into navigation coordinates and control the submersible's propulsion system to track the optimized smooth path and execute the submersible's autonomous obstacle avoidance.

[0061] Based on the same inventive concept, another embodiment of the present invention provides an electronic device, including a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other through the communication bus. Memory, used to store computer programs; The processor, when executing the program stored in the memory, implements the machine vision-based autonomous obstacle avoidance method for submersibles of the present invention.

[0062] The communication bus mentioned above can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EI) bus, etc. This communication bus can be divided into address bus, data bus, control bus, etc. For ease of representation, only one thick line is used in the diagram, but this does not indicate that there is only one bus or one type of bus. The communication interface is used for communication between the aforementioned terminal and other devices. The memory can include Random Access Memory (RAM), or non-volatile memory, such as at least one disk storage device. Optionally, the memory can also be at least one storage device located remotely from the aforementioned processor.

[0063] The processors mentioned above can be general-purpose processors, including central processing units (CPUs), network processors (NPs), etc.; they can also be digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.

[0064] Furthermore, to achieve the above objectives, embodiments of the present invention also propose a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the machine vision-based autonomous obstacle avoidance method for submersibles according to embodiments of the present invention.

[0065] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, apparatus, or computer program products. Therefore, embodiments of the present invention can take the form of entirely hardware embodiments, entirely software embodiments, or embodiments combining software and hardware aspects. Furthermore, embodiments of the present invention can take the form of computer program products implemented on one or more computer-usable hardware devices (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0066] The embodiments of the present invention are described with reference to flowchart illustrations and / or block diagrams of methods, terminal devices (apparatus), and computer program products according to embodiments of the invention. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing terminal device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal device, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0067] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing terminal device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0068] These computer program instructions can also be loaded onto a computer or other programmable data processing terminal equipment, causing a series of operational steps to be performed on the computer or other programmable terminal equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable terminal equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

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

[0070] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A machine vision-based autonomous obstacle avoidance method for submersibles, characterized in that, The method includes: The underwater environment data is collected using forward-looking sonar and binocular machine vision system. A three-dimensional point cloud of the environment is generated through an acoustic-optical fusion algorithm. The spatial position and envelope boundary of obstacles in the three-dimensional point cloud of the environment are extracted to construct a local environment map. Based on the local environment map, the improved artificial potential field method is used to calculate the resultant force vector by combining the current position of the submersible with the target point, and generate an initial discrete path point sequence containing obstacle avoidance actions. The vertical bisecting plane optimization algorithm is used to smooth the initial discrete path point sequence to obtain the optimized smooth path for the submersible's autonomous obstacle avoidance. The optimized smooth path is converted into navigation coordinates, and the submersible's propulsion system is controlled to track the optimized smooth path to perform autonomous obstacle avoidance.

2. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 1, characterized in that, Underwater environmental data is acquired using forward-looking sonar and a binocular machine vision system. A three-dimensional point cloud of the environment is generated through an acoustic-optical fusion algorithm. The spatial locations and envelope boundaries of obstacles in the three-dimensional point cloud are extracted to construct a local environmental map, including: The forward-looking sonar onboard the submersible is used to acquire acoustic distance data of the underwater environment and generate corresponding coarse acoustic point clouds. Using a binocular machine vision system mounted on a submersible, underwater optical image data after water scattering suppression is acquired, and image denoising enhancement and optical stereo matching are performed to generate corresponding optical point clouds. Based on the spatiotemporal synchronization relationship, the acoustic coarse point cloud and the optical fine point cloud are spatially registered and feature-level fused to generate an environmental 3D point cloud that takes into account both long-distance perception and short-distance high precision. Clustering algorithms are used to extract obstacles and model their envelopes from 3D point clouds of the environment, resulting in the spatial location and envelope boundaries of the obstacles. Map the spatial locations and envelope boundaries of all obstacles to a raster map to construct a local environment map that includes restricted areas.

3. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 2, characterized in that, Using a binocular machine vision system mounted on a submersible, underwater optical image data was acquired after water scattering suppression. Image denoising enhancement and optical stereo matching were then performed to generate corresponding optical point clouds, including: The distance gating synchronization control unit of the binocular machine vision system on the submersible is used to control the pulsed laser to emit high-frequency laser pulses toward the target water area; Within a preset time window during which the laser pulse is reflected back by an obstacle, the shutter of the binocular camera is opened for exposure, filtering backscattered light generated by suspended particles in the water and acquiring underwater optical image data that penetrates the turbid water. The intrinsic parameter matrix, distortion coefficients and relative pose relationship of the binocular camera are obtained by using Zhang Zhengyou's calibration method. Based on the calibration parameters, the left and right views in the underwater optical image data are stereo corrected so that the corresponding points of the left and right views are located on the same horizontal row, and the stereo corrected underwater image is obtained. The stereo-corrected underwater image is then denoised and enhanced to obtain a denoised and enhanced underwater image. A semi-global matching algorithm is used to calculate the disparity map of the denoised and enhanced underwater image. Based on the pinhole camera model, the three-dimensional spatial coordinates of the pixels are calculated in reverse using the triangulation principle according to the disparity value of the disparity map, and the corresponding optical point cloud is generated.

4. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 3, characterized in that, Clustering algorithms are used to extract obstacles and model their envelopes in the 3D point cloud of the environment, resulting in the spatial location and envelope boundaries of the obstacles, including: The statistical outlier filter is used to remove isolated noise points in the 3D point cloud of the environment caused by matching error, and the voxel mesh filter is used to downsample the 3D point cloud of the environment to obtain the filtered 3D point cloud of the environment. The RANSAC algorithm is used to fit the equation of the horizontal plane, and based on the equation of the horizontal plane, the background point cloud in the filtered 3D point cloud of the environment is removed to obtain the obstacle point cloud. The DBSCAN algorithm is used to segment the obstacle point cloud, and the segmented point cloud clusters are identified as independent obstacles, resulting in several clustered obstacle point cloud clusters. For each cluster of obstacle point cloud, calculate the corresponding minimum bounding box, and based on the minimum bounding box, extract the corresponding geometric center coordinates and envelope size to obtain the spatial position and envelope boundary of the obstacle.

5. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 4, characterized in that, Based on a local environment map, an improved artificial potential field method is used, combined with the current position of the submersible and the target point, to calculate the resultant force vector and generate an initial discrete path point sequence containing obstacle avoidance actions, including: The current position of the submersible and the target point are determined, and the key parameters of the potential field algorithm are set. The key parameters of the potential field algorithm include the gravitational gain coefficient, the repulsive gain coefficient, the radius of influence of obstacles, the path planning step size, and the target distance threshold. Based on the key parameters of the potential field algorithm, the gravitational vector from the current position of the submersible to the target point is calculated. Access the local environment map and use a spatial search algorithm to obtain a set of information on all obstacles within the submersible's current sensing range; For each obstacle in the obstacle information set, calculate the nearest distance from the current position of the submersible to the surface of the obstacle and its gradient direction based on its envelope boundary in the local environment map; Based on the key parameters of the potential field algorithm, the nearest distance and its gradient direction are substituted into the improved repulsive force formula to calculate the repulsive force vector generated by each obstacle. The total repulsive force is obtained by vector superposition of the repulsive forces generated by all obstacles in the local environment map. Calculate the resultant force vector acting on the submersible based on the gravitational vector from the submersible's current position to the target point and the total repulsive force exerted on the submersible by the obstacle; If the resultant force is less than the preset resultant force threshold, the virtual target point escape mechanism is activated. The system returns to the direction perpendicular to the current resultant force or the tangent to the obstacle, and generates a temporary virtual target point in the local environment map. The virtual target point temporarily replaces the target point in the gravity calculation, breaks the force balance, and guides the submersible around the trap area. After the escape is successful, the real target point is restored; otherwise, the system proceeds to the next step. Based on the key parameters of the potential field algorithm, the direction of the resultant force vector is normalized to obtain the next movement direction vector, and the position of the next path point is calculated. Update the current position, return to the gravity vector calculation step, until the distance between the current position and the target point is less than the target distance threshold or the current iteration number exceeds the maximum iteration number, and generate an initial discrete path point sequence containing obstacle avoidance actions.

6. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 5, characterized in that, The initial discrete path point sequence is smoothed using a perpendicular bisector plane optimization algorithm to obtain an optimized smooth path for autonomous obstacle avoidance by the submersible, including: The initial discrete path point sequence is downsampled to remove redundant collinear points, resulting in a sparse node set that retains key turning points. For each adjacent path pair in the sparse node set, calculate the midpoint of the line connecting the path pairs, construct a perpendicular bisector plane as the feasible solution space, and decompose the feasible solution space into two orthogonal dimensions to construct the two-dimensional parametric equations of the two-dimensional search space. Construct a fitness function for the perpendicular bisector plane optimization algorithm and encode the three-dimensional spatial coordinates of the insertion point as the position vector of the individual in the perpendicular bisector plane optimization algorithm; Based on the fitness function, the vertical bisecting plane optimization algorithm is used to search for the optimal insertion point for each adjacent path point pair in the sparse node set, and the optimal insertion point for the adjacent path point pair is obtained. Feasibility pruning is performed on the optimal insertion point of all adjacent path point pairs. Optimal insertion points located inside obstacles or not satisfying kinematic constraints are removed. The remaining optimal insertion points are inserted into the sparse node set, replacing the original straight-line connection segments, and reconstructing and generating the optimized smooth path for autonomous obstacle avoidance of the submersible.

7. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 6, characterized in that, The formula for the fitness function is: In the formula, For the first i Insertion point position fitness value; For the first i Insertion point position The path length connecting corresponding adjacent path points; For the first i Insertion point position Safety risk items; For the first i Insertion point position The depth smoothness constraint term; These are the weight coefficients for the fitness function.

8. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 7, characterized in that, Based on the fitness function, the perpendicular bisector plane optimization algorithm is used to search for the optimal insertion point for each adjacent path pair in the sparse node set, obtaining the optimal insertion point for the adjacent path pair, including: The chaotic sequence is generated using the Logistic mapping, and then mapped to a two-dimensional search space to obtain the initial population. Using a fitness function, the fitness of each initial individual in the initial population is calculated. Based on the gray wolf cooperative idea, the top three individuals with the highest fitness, the second best individuals, and the third best individuals are selected from the initial population. The other initial individuals are ordinary individuals. The population converges by using the best, second-best, and third-best individuals. The position of all ordinary individuals in the initial population is updated based on the two-dimensional search space parameter equation to obtain the updated population. For each updated individual in the updated population, perform dimensional boundary correction, and force updated individuals that exceed the boundary range of the two-dimensional search space to be projected back into the two-dimensional search space. Using the fitness function, calculate the fitness of each updated individual in the updated population, and update the updated individual with the best fitness value as the best individual; The population position is updated repeatedly. When the number of iterations reaches the maximum number of iterations or the fitness value of the best individual meets the requirements, the iterative update of the population is terminated, and the position vector of the best individual is output. Decode the position vector of the optimal individual to obtain the optimal insertion point for adjacent path point pairs.

9. The machine vision-based autonomous obstacle avoidance method for submersibles according to claim 8, characterized in that, For all adjacent path point pairs, perform feasibility pruning on the optimal insertion point, removing optimal insertion points located inside obstacles or not satisfying kinematic constraints, and inserting the remaining optimal insertion points into the sparse node set, replacing the original straight-line connection segments, and reconstructing and generating an optimized smooth path for the submersible's autonomous obstacle avoidance, including: Query the local environment map, calculate the safe Euclidean distance from the optimal insertion point to the nearest obstacle for each adjacent path point pair, determine the optimal insertion point with a safe Euclidean distance less than the envelope boundary as having a collision risk, perform feasibility pruning, remove the optimal insertion point located inside the obstacle, and obtain several optimal insertion points after feasibility pruning. After feasibility pruning, the maximum pitch angle constraint and maximum curvature constraint are checked for several optimal insertion points. The optimal insertion points that do not meet the kinematic constraints are removed, and the remaining optimal insertion points are obtained. The remaining optimal insertion points are inserted into the sparse node set, replacing the original straight-line connection segments, and the optimized smooth path for autonomous obstacle avoidance of the submersible is reconstructed.

10. A machine vision-based autonomous obstacle avoidance device for a submersible, used to implement the autonomous obstacle avoidance method for a submersible as described in any one of claims 1-9, characterized in that, The device includes: The machine vision acquisition unit is used to acquire underwater environmental data using forward-looking sonar and binocular machine vision system, generate a three-dimensional point cloud of the environment through an acoustic-optical fusion algorithm, and extract the spatial position and envelope boundary of obstacles in the three-dimensional point cloud of the environment to construct a local environmental map. The path point sequence generation unit is used to calculate the resultant force vector based on the local environment map, using the improved artificial potential field method, and combining the current position of the submersible with the target point, to generate an initial discrete path point sequence containing obstacle avoidance actions. The path smoothing unit is used to smooth the initial discrete path point sequence using the vertical bisecting plane optimization algorithm to obtain the optimized smooth path for autonomous obstacle avoidance of the submersible. The autonomous obstacle avoidance execution unit is used to convert the optimized smooth path into navigation coordinates and control the submersible's propulsion system to track the optimized smooth path and execute the submersible's autonomous obstacle avoidance.

Images