Large underwater vehicle three-dimensional dynamic obstacle avoidance method based on multiple single-beam sonar arrays

Abstract

The invention discloses a large-scale underwater vehicle three-dimensional dynamic obstacle avoidance method based on multiple single-beam sonar arrays, and the method comprises the steps: S1, collecting three-dimensional space detection data according to the arrangement of a space array composed of a center sonar and four peripheral sonars, and generating three-dimensional environment perception information; s2, based on the three-dimensional environment perception information, fusing data of an inertial navigation system, a Doppler velocimeter and a depth sensor, and generating real-time attitude and obstacle distribution information of the underwater vehicle; s3, on the basis of the real-time attitude of the underwater vehicle and the obstacle distribution information, risk level assessment and obstacle avoidance path planning are executed, and an obstacle avoidance control instruction is generated; and S4, based on the obstacle avoidance control instruction, controlling a power system of the underwater vehicle to adjust the course, the pitch angle and the speed so as to realize three-dimensional dynamic obstacle avoidance. The problem that a traditional single-beam sonar is insufficient in stereoscopic perception capability is solved, and the defect that the traditional single-beam sonar cannot effectively detect obstacles in the pitch angle direction (such as a submarine abrupt slope and a suspended rock frame) is overcome.

Description

Technical Field

[0001] This invention relates to the field of obstacle avoidance control technology for autonomous underwater vehicles (AUVs), and in particular to a three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays. Background Technology

[0002] With the increasing demand for marine resource development, the safe navigation of autonomous underwater vehicles (AUVs) in complex underwater environments has become a key technological challenge. Due to the complexity and unpredictability of the underwater environment, AUVs frequently encounter unknown static and dynamic obstacles during navigation. Dynamic obstacles, in particular, pose a high risk of collision due to their unpredictable motion and high kinetic energy. Existing obstacle avoidance technologies mainly employ single-beam sonar rotating scanning and forward-looking sonar schemes. The former acquires obstacle information in the left, center, and right regions through mechanical rotation, achieving obstacle avoidance within a 90° opening angle; the latter utilizes the wide-bandgap fan-shaped beam of the forward-looking sonar for horizontal obstacle detection.

[0003] However, existing technologies have significant drawbacks: First, they lack stereo perception capabilities. A single single-beam or forward-looking plane sonar can only provide two-dimensional planar detection, lacking pitch angle information and unable to identify complex three-dimensional terrain such as steep seabed slopes and overhanging rock frames. Second, dynamic response lag is prominent. Rotational scanning requires mechanical rotation and cannot respond to sudden obstacles in multiple directions in real time. Finally, there is a contradiction between the real-time performance and reliability of the algorithm. The computational load of three-dimensional point cloud reconstruction is high, and underwater embedded systems cannot meet the real-time requirements, which seriously affects the obstacle avoidance safety and mission efficiency of large AUVs in complex underwater environments.

[0004] Therefore, there is an urgent need for a three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays. Summary of the Invention

[0005] This invention provides a three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays, in order to solve the above-mentioned problems existing in the prior art.

[0006] To achieve the above objectives, the present invention provides the following technical solution: A three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays includes: S1: Collect three-dimensional spatial detection data by arranging a spatial array consisting of a central sonar and four peripheral sonars to generate three-dimensional environmental perception information; S2: Based on three-dimensional environmental perception information, it integrates data from the inertial navigation system, Doppler velocimeter, and depth sensor to generate real-time attitude and obstacle distribution information of the submersible; S3: Based on the real-time attitude and obstacle distribution information of the submersible, perform risk level assessment and obstacle avoidance path planning, and generate obstacle avoidance control commands; S4: Based on obstacle avoidance control commands, control the underwater vehicle's power system to adjust its heading, pitch angle, and speed to achieve three-dimensional dynamic obstacle avoidance.

[0007] Step S1 includes: S11: Install the central sonar along the longitudinal axis of the submersible to obtain detection data of the central area in front; S12: Install the four peripheral sonars at preset offset angles in the four directions of up, down, left, and right to form a three-dimensional detection coverage; S13: Control each sonar interval to work continuously, avoid mutual interference, and collect distance data of obstacles within a predetermined range ahead; S14: Integrate the detection data from various sonars to construct a three-dimensional spatial obstacle distribution map ahead and generate three-dimensional environmental perception information.

[0008] Step S2 includes: S21: Simultaneously receive three-dimensional environmental perception information and submarine attitude angle data provided by the inertial navigation system; S22: Combines the speed information from the Doppler velocimeter with the depth data from the depth sensor to calculate the current motion state of the submersible; S23: Spatiotemporally match the spatial distribution of obstacles with the motion state of the submersible to generate real-time attitude and obstacle distribution information containing the relative positions of obstacles and the dynamic parameters of the submersible.

[0009] Step S3 includes: S31: Divide risks into multiple levels based on the distance to obstacles, with different levels corresponding to different early warning and control strategies; S32: Develop corresponding obstacle avoidance strategies for different risk levels, including early warning display, heading adjustment, speed control and emergency braking; S33: Combine the target waypoint and the current obstacle distribution to calculate a safe passage path and generate obstacle avoidance control commands that include heading adjustment angle, pitch adjustment angle and speed control parameters.

[0010] Step S13 includes: S131: Activate part of the sonar for basic detection during cruise. S132: Activate all sonar arrays in complex terrain environments; S133: In emergency obstacle avoidance mode, enhance the sampling frequency of sonar in a specific direction; S134: Real-time monitoring of the working status of each sonar, triggering an alarm and switching to the backup obstacle avoidance mode when a fault occurs.

[0011] The offset angle θ of the four peripheral sonars is determined by the following formula: ; Where D represents the desired coverage diameter, d represents the blind zone diameter, and L represents the detection distance.

[0012] The risk levels include: A Level 1 warning is issued when the distance to an obstacle is greater than 50 meters. A level two warning is issued when the distance to the obstacle is between 36 and 50 meters. A Level 3 warning is issued when the distance to an obstacle is between 10 and 35 meters. When the distance to the obstacle is within 10 meters, it is a level four emergency braking.

[0013] Step S33 includes: S331: When an obstacle is detected ahead, the direction in which the obstacle is less than a preset value is selected as the deflection direction by comparing the sonar data in the left and right directions; S332: When an obstacle is detected below, adjust the submersible's buoyancy system and calculate the pitch angle adjustment required to ascend. S333: When encountering a large obstacle and unable to confirm a safe passage, control the underwater vehicle to rotate its yaw angle to no more than 90-θ degrees, and use multiple sonars to detect and scan the area ahead to find a passage path. S334: Based on the selected obstacle avoidance direction and path, calculate the heading adjustment angle α, pitch adjustment angle β, and speed control value v, and generate obstacle avoidance control commands.

[0014] In step S331, the heading adjustment angle α is calculated using the following formula: ; Where W represents the submersible's half-width plus safety margin, and D represents the distance to the obstacle.

[0015] In step S332, the pitch angle adjustment β is calculated using the following formula: ; Where h represents the required ascent height, v represents the current speed, and t represents the estimated time required to pass the obstacle.

[0016] Compared with the prior art, the present invention has the following advantages: A three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays includes: S1: acquiring three-dimensional spatial detection data by arranging a spatial array consisting of a central sonar and four peripheral sonars to generate three-dimensional environmental perception information; S2: based on the three-dimensional environmental perception information, fusing data from an inertial navigation system, a Doppler velocimeter, and a depth sensor to generate real-time attitude and obstacle distribution information of the underwater vehicle; S3: based on the real-time attitude and obstacle distribution information of the underwater vehicle, performing risk level assessment and obstacle avoidance path planning to generate obstacle avoidance control commands; S4: based on the obstacle avoidance control commands, controlling the underwater vehicle's power system to adjust its heading, pitch angle, and speed to achieve three-dimensional dynamic obstacle avoidance. This system addresses the shortcomings of traditional single-beam sonar in terms of stereo sensing capabilities, overcoming its inability to effectively detect obstacles in the pitch direction (such as steep seabed slopes and overhanging rock frames); it breaks through the limitations of existing sonar systems in large AUVs, improving installation compatibility issues caused by their large size, high water resistance, and poor mobility; it eliminates the dynamic response lag problem of rotating scanning sonars, enhancing real-time response to sudden obstacles (such as drifting mines and schools of fish); and it optimizes obstacle avoidance algorithms, ensuring real-time system control while improving the reliability of the obstacle avoidance algorithm to meet AUV safety standards.

[0017] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention.

[0018] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0019] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart of a three-dimensional dynamic obstacle avoidance method for a large underwater vehicle based on multiple single-beam sonar arrays, as described in an embodiment of the present invention. Figure 2 This is a sensor array configuration design diagram in an embodiment of the present invention; Figure 3 This is a diagram illustrating the three-dimensional spatial arrangement of the sonar sensors in an embodiment of the present invention. Figure 4 This is a flowchart of the system's autonomous path planning and obstacle avoidance in this embodiment of the invention; Figure 5 This is a flowchart of the multi-sonar cooperative mode switching control in an embodiment of the present invention; Figure 6 This is a block diagram of the risk assessment system in an embodiment of the present invention; Figure 7This is a flowchart of the obstacle avoidance strategy in an embodiment of the present invention; Figure 8 This is a schematic diagram of sonar scanning in an embodiment of the present invention. Figure 2 ; Figure 9 This is a schematic diagram of sonar scanning in an embodiment of the present invention. Figure 3 .

[0020] In the diagram: 0 is the central sonar, 1 is sonar one, 2 is sonar two, 3 is sonar three, and 4 is sonar four. Detailed Implementation

[0021] The preferred embodiments of the present invention will be described below with reference to the accompanying drawings. It should be understood that the preferred embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0022] The embodiments of the present invention provide, as follows Figure 1 As shown, a three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays includes: S1: Collect three-dimensional spatial detection data by arranging a spatial array consisting of a central sonar and four peripheral sonars to generate three-dimensional environmental perception information; S2: Based on three-dimensional environmental perception information, it integrates data from the inertial navigation system, Doppler velocimeter, and depth sensor to generate real-time attitude and obstacle distribution information of the submersible; S3: Based on the real-time attitude and obstacle distribution information of the submersible, perform risk level assessment and obstacle avoidance path planning, and generate obstacle avoidance control commands; S4: Based on obstacle avoidance control commands, control the underwater vehicle's power system to adjust its heading, pitch angle, and speed to achieve three-dimensional dynamic obstacle avoidance.

[0023] The working principle and beneficial effects of the above technical solution are as follows: S1. Three-dimensional spatial detection data is collected by arranging a spatial array consisting of a central sonar and four peripheral sonars to generate stereo environmental perception information. This step adopts a central + cross spatial geometric configuration design, such as... Figure 2 The image shows a side view of the head of a large AUV. Q represents the head of the large AUV; the central sonar 0 is installed strictly along the longitudinal axis of the submersible, responsible for detecting the central area directly in front; the four peripheral sonars are symmetrically distributed in a cross shape, precisely deflected in four directions (up, down, left, and right) with a fixed offset angle θ. The angle θ is calculated using the formula θ=arctan[(Dd) / 2L], where D is the desired coverage diameter, d is the blind zone diameter, and L is the detection distance. Operators calculate the installation angle θ based on the specific single-beam sonar parameters and the desired coverage area to ensure that each sonar beam achieves no blind spots within an effective detection range of 10-50 meters, forming a forward-facing three-dimensional detection sector. Figure 3As shown, when arranged in the horizontal plane, the central sonar 0 and the two horizontal sonars (i.e., Sonar 1 and Sonar 3) form a horizontal detection surface. When arranged in the vertical plane, the central sonar and the upper and lower vertical sonars (i.e., Sonar 2 and Sonar 4) form a vertical detection surface, ensuring synchronous monitoring of seabed and surface obstacles. Specifically, the BB profile (horizontal detection surface) shows the beam coverage of the central sonar 0 and the left sonar (Sonar 3) and right sonar (Sonar 1); the AA profile (vertical detection surface) shows the beam coverage of the central sonar 0 and the upper sonar (Sonar 2) and lower sonar (Sonar 4). A three-dimensional view demonstrates the spatial coverage of the sonar beams: the horizontal plane covers obstacles on both sides, while the vertical plane simultaneously monitors seabed (Sonar 2) and surface obstacles (Sonar 4). The cross-section design visually presents blind-spot-free coverage within a range of 10-50m, verifying the rationality of the installation angle θ. The system maintains the streamlined characteristics of the submersible to the maximum extent by conformally integrating the sonar into the shell, thus avoiding the damage to fluid properties caused by traditional rotating scanning methods.

[0024] S2. Based on stereo environmental perception information, data from the inertial navigation system, Doppler velocimeter, and depth sensor are fused to generate real-time attitude and obstacle distribution information for the submersible. This step achieves spatiotemporal synchronous fusion processing of multi-source sensor data. The system first synchronously receives stereo environmental perception information from five sonar arrays and submersible attitude angle data, including yaw, pitch, and roll angles, provided by the inertial navigation system. Then, it fuses three-axis velocity information from the Doppler velocimeter (DVL) and real-time depth data from the depth sensor, calculating the submersible's current complete motion state through coordinate transformation and timestamp matching. Technicians perform spatiotemporal matching calculations between the spatial distribution data of obstacles and the submersible's motion state to generate real-time attitude and obstacle distribution information, including the relative positions and distances of obstacles, as well as the submersible's dynamic parameters. This fusion process ensures the accuracy of obstacle detection and the reliability of submersible state estimation.

[0025] S3. Based on the real-time attitude and obstacle distribution information of the submersible, perform risk level assessment and obstacle avoidance path planning, and generate obstacle avoidance control commands. This step establishes a four-level risk assessment mechanism, such as... Figure 6 As shown. The system classifies risks into levels I-IV based on obstacle distance: Level I warning is activated when the obstacle distance is greater than 50 meters, and the warning information is only displayed on the control panel; Level II warning is activated within 36-50 meters, and the system automatically activates all-directional sonar arrays, adjusting heading and pitch angles; Level III warning is triggered within 10-35 meters, controlling the underwater vehicle's speed to reduce to below 2 knots and enhancing the sonar sampling frequency in a specific direction; Level IV emergency braking measures are immediately implemented within 10 meters. Figure 7As shown, the AUV speed is reduced to below 2 knots, while the sonar scanning frequency in the direction of obstacles is increased to accurately measure distances. Corresponding obstacle avoidance strategies are formulated for different risk levels. Combining the target waypoint and the current obstacle distribution, a safe passage path is calculated through a path planning algorithm, and obstacle avoidance control commands containing heading adjustment angle, pitch adjustment angle and speed control parameters are generated.

[0026] S4. Based on obstacle avoidance control commands, control the underwater vehicle's power system to adjust its heading, pitch angle, and speed, and achieve three-dimensional dynamic obstacle avoidance by changing the pitch angle and yaw angle.

[0027] In another embodiment, step S1 includes: S11: Install the central sonar along the longitudinal axis of the submersible to obtain detection data of the central area in front; S12: Install the four peripheral sonars at preset offset angles in the four directions of up, down, left, and right to form a three-dimensional detection coverage; S13: Control each sonar interval to work continuously, avoid mutual interference, and collect distance data of obstacles within a predetermined range ahead; S14: Integrate the detection data from various sonars to construct a three-dimensional spatial obstacle distribution map ahead and generate three-dimensional environmental perception information.

[0028] The working principle and beneficial effects of the above technical solution are as follows: S11, the central sonar 0 is installed along the longitudinal axis of the submersible to obtain detection data of the central area in front. For example... Figure 2 As shown, the central sonar 0 is installed with strict axial alignment, ensuring that the main beam axis of its acoustic transducer is perfectly aligned with the longitudinal axis of the submersible. Technicians use precise mechanical positioning devices to ensure the installation accuracy of the central sonar 0, enabling it to accurately detect the central area directly in front of the submersible. This sonar is responsible for acquiring information on major obstacles along the straight navigation path, with a detection range covering a cone-shaped area 10-50 meters ahead. The sonar emits sound pulses of a specific frequency and calculates the distance and orientation of obstacles by receiving the echo signals.

[0029] S12. Install the four peripheral sonars at preset offset angles in four directions: upward (Sonar 2), downward (Sonar 4), left (Sonar 3), and right (Sonar 1), forming a three-dimensional detection coverage. The four peripheral sonars are precisely installed according to the pre-calculated offset angle θ, such as... Figure 3As shown, the diagram is a view of the AUV's head, mirrored horizontally. The upper sonar deflects upwards by an angle θ to detect surface obstacles and overhanging structures; the lower sonar deflects downwards by an angle θ to specifically monitor seabed topography and bottom obstacles; the left and right sonars deflect in their respective directions by an angle θ to detect lateral obstacles. The installation angle of each sonar is precisely calculated using the formula θ = arctan[(Dd) / 2L], where D represents the desired detection coverage diameter, d represents the diameter of the central blind zone, and L is the set detection distance. This arrangement ensures that the detection sectors of the five sonars form a seamless, three-dimensional coverage network in space.

[0030] S13. Control each sonar to operate continuously at intervals to avoid mutual interference and collect distance data of obstacles within a predetermined range ahead. The system adopts a time-division multiplexing operating mode to control the transmission sequence of each sonar. The sonar controller activates each sonar unit sequentially according to a preset time interval, ensuring that only one sonar is in transmission state at any given time, avoiding mutual interference between the sound wave signals of different sonars. A typical working cycle is: after the central sonar transmits and receives, the four peripheral sonars (upper, right, lower, and left) are activated sequentially, with the working interval of each sonar set to 50-100 milliseconds. The data collected by each sonar includes obstacle distance, echo intensity, and azimuth information, covering the complete three-dimensional space within the predetermined range ahead.

[0031] S14. Integrate the detection data from each sonar to construct a three-dimensional obstacle distribution map in front, generating stereo environmental perception information. After collecting the raw detection data from five sonars, the data processing unit first performs coordinate transformation to convert the local coordinate system of each sonar to the coordinate system of the submersible. Then, spatial interpolation algorithms are used to fill the gaps between sonar detection sectors, constructing a continuous three-dimensional obstacle distribution model. The system stores the processed data in raster form, with each raster cell recording the probability of obstacle presence and distance information at that spatial location. The final generated stereo environmental perception information includes the position, size, and distribution characteristics of all obstacles in the three-dimensional space ahead, providing a complete environmental cognitive basis for subsequent obstacle avoidance decisions.

[0032] In another embodiment, step S2 includes: S21: Simultaneously receive three-dimensional environmental perception information and submarine attitude angle data provided by the inertial navigation system; S22: Combines the speed information from the Doppler velocimeter with the depth data from the depth sensor to calculate the current motion state of the submersible; S23: Spatiotemporally match the spatial distribution of obstacles with the motion state of the submersible to generate real-time attitude and obstacle distribution information containing the relative positions of obstacles and the dynamic parameters of the submersible.

[0033] The working principle and beneficial effects of the above technical solution are as follows: S21, synchronously receive stereo environment perception information and attitude angle data of the submersible provided by the inertial navigation system. The system establishes a strict data synchronization mechanism to ensure that the stereo environment perception information from the sonar array and the attitude data from the inertial navigation system are consistent in time reference. The attitude angle data provided by the inertial navigation system includes yaw angle (the rotation angle of the submersible around the vertical axis), pitch angle (the rotation angle around the horizontal axis), and roll angle (the rotation angle around the longitudinal axis). The data acquisition module marks the data of each sensor with a unified timestamp and uses a hardware synchronization triggering method to ensure the time synchronization of data acquisition, eliminating positioning errors caused by time delay.

[0034] S22. The system fuses velocity information from the Doppler velocimeter and depth data from the depth sensor to calculate the current motion state of the submersible. The Doppler velocimeter (DVL) provides the submersible's three-axis velocity components relative to the seabed, including forward velocity, lateral velocity, and vertical velocity. The depth sensor provides accurate water depth information and the rate of change of vertical position. The data fusion algorithm uses a Kalman filter to optimally estimate the multi-source velocity and position information, eliminating measurement noise and bias from each sensor through two steps: state prediction and observation update. The system calculates the submersible's six-degree-of-freedom motion state, including three translational velocity components and three angular velocity components, providing a kinematic basis for accurate obstacle avoidance decisions.

[0035] S23. The spatial distribution of obstacles is spatiotemporally matched with the underwater vehicle's motion state to generate real-time attitude and obstacle distribution information containing the relative positions of obstacles and the underwater vehicle's dynamic parameters. This step achieves accurate spatiotemporal registration in a dynamic environment. The system predicts the underwater vehicle's future trajectory based on its current motion state, while also considering the relative motion of obstacles. A coordinate transformation matrix is ​​used to convert the obstacle positions in a fixed coordinate system to dynamic coordinates relative to the underwater vehicle. The spatiotemporal matching algorithm considers the time delays in data acquisition, processing, and transmission, and performs time compensation corrections to the obstacle positions. The final generated real-time information includes the distance and azimuth of each obstacle relative to the underwater vehicle, as well as the underwater vehicle's own position, attitude, and velocity, ensuring that the obstacle avoidance algorithm makes decisions based on the latest and most accurate situational information.

[0036] In another embodiment, step S3 includes: S31: Divide risks into multiple levels based on the distance to obstacles, with different levels corresponding to different early warning and control strategies; S32: Develop corresponding obstacle avoidance strategies for different risk levels, including early warning display, heading adjustment, speed control and emergency braking; S33: Combine the target waypoint and the current obstacle distribution to calculate a safe passage path and generate obstacle avoidance control commands that include heading adjustment angle, pitch adjustment angle and speed control parameters.

[0037] The working principle and beneficial effects of the above technical solution are as follows: S31, the risk is divided into multiple levels based on the distance to obstacles, and different levels correspond to different early warning and control strategies. The system establishes a four-level risk assessment system based on distance, such as... Figure 6 As shown. Level I risk corresponds to obstacles greater than 50 meters away, which the system considers a safe distance, displaying only a warning message on the console without taking any control action; Level II risk targets obstacles between 36 and 50 meters away, in which case the system activates the entire sonar array and prepares for course adjustment; Level III risk addresses emergencies between 10 and 35 meters away, triggering speed reduction and enhanced scanning; Level IV risk handles extremely dangerous situations within 10 meters, immediately executing emergency braking. Each risk level has clearly defined judgment thresholds and corresponding response time requirements, ensuring the system can implement differentiated safety strategies based on the threat level.

[0038] S32. Develop corresponding obstacle avoidance strategies for different risk levels, including early warning display, heading adjustment, speed control, and emergency braking. For example... Figure 7 As shown, the system is configured with a specific obstacle avoidance strategy combination for each risk level. The warning display strategy includes visual warning lights, audible alarms, and information display on the user interface; the heading adjustment strategy controls the yaw angle and thrusters to achieve path deflection in the horizontal plane, with an adjustment range of ±90 degrees; the speed control strategy dynamically adjusts the propulsion power according to the risk level, gradually reducing from normal cruising speed (above 6 knots) to emergency obstacle avoidance speed (below 2 knots); the emergency braking strategy activates the thrust reversers to bring the submersible to a stop in the shortest possible time. These strategies can be combined, and the system automatically selects the optimal strategy combination based on real-time risk assessment results.

[0039] S33. Based on the target waypoint and the current obstacle distribution, calculate a safe passage path and generate obstacle avoidance control commands that include heading adjustment angles, pitch adjustment angles, and speed control parameters. Calculate the current flight path based on the target point and the user's own coordinates, and detect obstacles on the path in real time.

[0040] In another embodiment, step S13 includes: S131: Activate part of the sonar for basic detection during cruise. S132: Activate all sonar arrays in complex terrain environments; S133: In emergency obstacle avoidance mode, enhance the sampling frequency of sonar in a specific direction; S134: Real-time monitoring of the working status of each sonar, triggering an alarm and switching to the backup obstacle avoidance mode when a fault occurs.

[0041] The working principle and beneficial effects of the above technical solution are as follows: S131, during cruise mode, partial sonar is activated for basic detection. When the submersible is in normal cruise mode, the system adopts an energy-saving selective sonar operating mode. For example... Figure 4 As shown, after system startup, the initial target point is set, and the AUV switches to autopilot mode. It automatically navigates according to the planned path, activating the forward and downward obstacle avoidance sonars in an intermittent continuous operating mode to avoid mutual interference and continuously detect obstacles ahead and below, ensuring safe navigation. In the basic detection mode, only the central sonar and the downward sonar are activated. The central sonar monitors obstacles in the main channel ahead, while the downward sonar specifically detects changes in seabed topography. This configuration meets basic safe navigation requirements while significantly reducing system power consumption and acoustic noise. The two sonars operate in an intermittent mode with a working cycle set to 200 milliseconds, maximizing battery life while ensuring detection effectiveness. The cruise mode is suitable for navigation in open waters with good sea conditions and relatively simple terrain.

[0042] S132. Activate full sonar array operation in complex terrain environments. When the submersible enters canyons, reef areas, or other complex terrain environments, the system automatically switches to full array operation mode. For example... Figure 5 As shown, the system dynamically adjusts the sonar operating mode according to the AUV's movement status, activating all five sonars to form a three-dimensional detection network covering the entire area ahead. In the complex environment mode, the operating frequency is increased to 10 Hz, and the sonar duty cycle is shortened to 100 milliseconds, ensuring timely detection of rapidly changing terrain and obstacles from multiple directions. The system automatically determines whether to switch modes using a terrain complexity assessment algorithm, evaluating parameters including the rate of change of seabed slope, obstacle density, and water depth variation. Although this mode consumes more power, it provides the most comprehensive environmental awareness.

[0043] S133. In emergency obstacle avoidance mode, enhance the sampling frequency of sonar in a specific direction. When the system detects an emergency obstacle avoidance situation, immediately activate the directional enhancement scanning mode. For example... Figure 8 , Figure 9 As shown, after identifying the main threat direction of an obstacle, the system increases the sampling frequency of the sonar in that direction to 20-30 Hz, 2-3 times higher than the normal mode. For example, when a large obstacle is detected in front, the central sonar enters a high-frequency sampling mode, while the left and right sonars assist in detecting possible detour routes. Enhanced sampling provides more accurate distance measurements and faster obstacle movement trend analysis, offering high-quality data support for emergency obstacle avoidance decisions. This mode typically lasts for 10-30 seconds until the threat is eliminated or successfully avoided.

[0044] S134. Real-time monitoring of the operating status of each sonar, triggering an alarm and switching to backup obstacle avoidance mode when a fault occurs. The system has established a comprehensive sonar health monitoring mechanism, continuously checking key parameters such as the operating voltage, transmission power, receiving sensitivity, and signal quality of each sonar.

[0045] In another embodiment, the offset angle θ of the four peripheral sonars is determined by the following formula: ; Where D represents the desired coverage diameter, d represents the blind zone diameter, and L represents the detection distance.

[0046] The working principle and beneficial effects of the above technical solution are as follows: the offset angle θ of the four peripheral sonars is determined by the formula θ=arctan[(Dd) / 2L]. Figure 2 The sensor array configuration shown in the diagram requires engineers to accurately calculate the offset angle during installation. The desired coverage diameter D is determined based on the size of the submersible; for large AUVs exceeding 15 meters in length, it is typically 20 meters. The blind zone diameter d is the detection dead zone caused by the transducer's near-field effect, approximately 1-2 meters. The detection range L is taken as the median of the sonar's effective range of 10-50 meters, specifically 30 meters.

[0047] Substituting the parameters into the formula, when D=20 meters, d=2 meters, and L=30 meters, θ=arctan[(20-2) / (2×30)]=arctan(0.3)≈16.7 degrees. The four peripheral sonars deflect 16.7 degrees upward, downward, leftward, and rightward respectively, forming a three-dimensional detection network together with the central sonar.

[0048] In another embodiment, the risk levels include: A Level 1 warning is issued when the distance to an obstacle is greater than 50 meters. A level two warning is issued when the distance to the obstacle is between 36 and 50 meters. A Level 3 warning is issued when the distance to an obstacle is between 10 and 35 meters. When the distance to the obstacle is within 10 meters, it is a level four emergency braking.

[0049] The working principle and beneficial effects of the above technical solution are as follows: the risk level classification is determined based on the braking characteristics and reaction time of large AUVs. For example... Figure 6 The risk assessment system diagram shown indicates that the first-level warning is set at a distance of 50 meters or more. At this distance, an AUV traveling at 6 knots has more than 16 seconds of reaction time and only needs to be alerted by the yellow marker on the interface.

[0050] A Level II warning corresponds to a distance of 36-50 meters, with a reaction time of 12-16 seconds. For example... Figure 5 As shown, the system automatically activates all five sonars, and the operator can adjust the course to avoid them.

[0051] The Level 3 early warning system covers a danger zone of 10-35 meters, with a reaction time of only 3-12 seconds. The thruster power is reduced to below 2 knots, and the obstacle-oriented sonar is upgraded to a 20Hz sampling rate.

[0052] Level 4 emergency braking is triggered within 10 meters, the thrusters reverse to brake, the audible and visual alarms are activated, and the system awaits operator intervention.

[0053] In another embodiment, step S33 includes: S331: When an obstacle is detected ahead, the direction in which the obstacle is less than a preset value is selected as the deflection direction by comparing the sonar data in the left and right directions; S332: When an obstacle is detected below, adjust the submersible's buoyancy system and calculate the pitch angle adjustment required to ascend. S333: When encountering a large obstacle and unable to confirm a safe passage, control the underwater vehicle to rotate its yaw angle to no more than 90-θ degrees, and use multiple sonars to detect and scan the area ahead to find a passage path. S334: Based on the selected obstacle avoidance direction and path, calculate the heading adjustment angle α, pitch adjustment angle β, and speed control value v, and generate obstacle avoidance control commands.

[0054] The working principle and beneficial effects of the above technical solution are as follows: Step S331, when detecting obstacles ahead, compare left and right sonar data to select the deflection direction. For example... Figure 8 As shown, the left sonar (i.e., sonar 3) and the right sonar (i.e., sonar 1) are operating simultaneously.

[0055] Step S332: When detecting obstacles below, the cruise depth of the AUV is changed by the control system, and the pitch angle is adjusted to make the AUV move upward to avoid the obstacles below.

[0056] Step S333: When encountering large obstacles, perform yaw angle rotation scanning for path finding.

[0057] like Figure 9 As shown, when facing a large lateral obstacle, the AUV slows down to 2 knots and slowly rotates its bow. Since the peripheral sonar is already offset by θ degrees, it only needs to rotate 90-θ degrees to scan the 180-degree range ahead.

[0058] Step S334: Calculate control parameters and generate instructions. After the path is determined, calculate the heading angle α, pitch angle β, and velocity v. These three parameters are sent to the actuators via the control bus.

[0059] In another embodiment, in step S331, the heading adjustment angle α is calculated using the following formula: ; Where W represents the submersible's half-width plus safety margin, and D represents the distance to the obstacle.

[0060] The working principle and beneficial effects of the above technical solution are as follows: heading adjustment angle α = arctan(W / D).

[0061] The submersible's half-width is approximately 1 meter, plus a safety margin of 0.8 meters, so W = 1.8 meters. When the sonar measures the obstacle's distance to D = 20 meters, α = arctan(1.8 / 20) ≈ 5.1 degrees. The submersible adjusts its course laterally by 5.1 degrees to ensure safe passage. The closer the distance, the larger the required turning angle.

[0062] In another embodiment, in step S332, the pitch angle adjustment β is calculated using the following formula: ; Where h represents the required ascent height, v represents the current speed, and t represents the estimated time required to pass the obstacle.

[0063] The working principle and beneficial effects of the above technical solution are as follows: Pitch angle adjustment β = arcsin(h / vt).

[0064] The sonar reading below indicates a required ascent height h = 3 meters to avoid the reef. The current speed v = 1 m / s (2 knots), and the obstacle is 30 meters long, requiring t = 30 seconds to pass. Substituting these values, we get β = arcsin(3 / 30) ≈ 5.7 degrees. The submersible maintains a 5.7-degree upward pitch angle, adjusting buoyancy to ascend 3 meters within 30 seconds to clear the obstacle. The system monitors the ascent effect in real time and dynamically fine-tunes the angle.

[0065] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from the spirit and scope of this invention.

Claims

1. A three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays, characterized in that, include: S1: Collect three-dimensional spatial detection data by arranging a spatial array consisting of a central sonar and four peripheral sonars to generate three-dimensional environmental perception information; S2: Based on three-dimensional environmental perception information, it integrates data from the inertial navigation system, Doppler velocimeter, and depth sensor to generate real-time attitude and obstacle distribution information of the submersible; S3: Based on the real-time attitude and obstacle distribution information of the submersible, perform risk level assessment and obstacle avoidance path planning, and generate obstacle avoidance control commands; S4: Based on obstacle avoidance control commands, control the underwater vehicle's power system to adjust its heading, pitch angle, and speed to achieve three-dimensional dynamic obstacle avoidance.

2. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 1, characterized in that, Step S1 includes: S11: Install the central sonar along the longitudinal axis of the submersible to obtain detection data of the central area in front; S12: Install the four peripheral sonars at preset offset angles in the four directions of up, down, left, and right to form a three-dimensional detection coverage; S13: Control each sonar interval to work continuously, avoid mutual interference, and collect distance data of obstacles within a predetermined range ahead; S14: Integrate the detection data from various sonars to construct a three-dimensional spatial obstacle distribution map ahead and generate three-dimensional environmental perception information.

3. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 1, characterized in that, Step S2 includes: S21: Simultaneously receive three-dimensional environmental perception information and submarine attitude angle data provided by the inertial navigation system; S22: Combines the speed information from the Doppler velocimeter with the depth data from the depth sensor to calculate the current motion state of the submersible; S23: Spatiotemporally match the spatial distribution of obstacles with the motion state of the submersible to generate real-time attitude and obstacle distribution information containing the relative positions of obstacles and the dynamic parameters of the submersible.

4. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 1, characterized in that, Step S3 includes: S31: Divide risks into multiple levels based on the distance to obstacles, with different levels corresponding to different early warning and control strategies; S32: Develop corresponding obstacle avoidance strategies for different risk levels, including early warning display, heading adjustment, speed control and emergency braking; S33: Combine the target waypoint and the current obstacle distribution to calculate a safe passage path and generate obstacle avoidance control commands that include heading adjustment angle, pitch adjustment angle and speed control parameters.

5. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 2, characterized in that, Step S13 includes: S131: Activate part of the sonar for basic detection during cruise. S132: Activate all sonar arrays in complex terrain environments; S133: In emergency obstacle avoidance mode, enhance the sampling frequency of sonar in a specific direction; S134: Real-time monitoring of the working status of each sonar, triggering an alarm and switching to the backup obstacle avoidance mode when a fault occurs.

6. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 2, characterized in that, The offset angle θ of the four peripheral sonars is determined by the following formula: ； Where D represents the desired coverage diameter, d represents the blind zone diameter, and L represents the detection distance.

7. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 4, characterized in that, Risk levels include: A Level 1 warning is issued when the distance to an obstacle is greater than 50 meters. A level two warning is issued when the distance to the obstacle is between 36 and 50 meters. A Level 3 warning is issued when the distance to an obstacle is between 10 and 35 meters. When the distance to the obstacle is within 10 meters, it is a level four emergency braking.

8. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 4, characterized in that, Step S33 includes: S331: When an obstacle is detected ahead, the direction in which the obstacle is less than a preset value is selected as the deflection direction by comparing the sonar data in the left and right directions; S332: When an obstacle is detected below, adjust the submersible's buoyancy system and calculate the pitch angle adjustment required to ascend. S333: When encountering a large obstacle and unable to confirm a safe passage, control the underwater vehicle to rotate its yaw angle to no more than 90-θ degrees, and use multiple sonars to detect and scan the area ahead to find a passage path. S334: Based on the selected obstacle avoidance direction and path, calculate the heading adjustment angle α, pitch adjustment angle β, and speed control value v, and generate obstacle avoidance control commands.

9. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 8, characterized in that, In step S331, the heading adjustment angle α is calculated using the following formula: ； Where W represents the submersible's half-width plus safety margin, and D represents the distance to the obstacle.

10. The three-dimensional dynamic obstacle avoidance method for large underwater vehicles based on multiple single-beam sonar arrays according to claim 8, characterized in that, In step S332, the pitch angle adjustment β is calculated using the following formula: ； Where h represents the required ascent height, v represents the current speed, and t represents the estimated time required to pass the obstacle.

Images