Autonomous hole passing method for underwater submersible

Abstract

The invention provides an autonomous hole passing method for an underwater submersible, which comprises a submersible equipment main body, and a sensor and a distance measuring sonar which are arranged on the submersible equipment main body, the sensor and the distance measuring sonar are in signal connection with the submersible equipment main body, the sensor is used for acquiring attitude data of the submersible equipment main body, and the distance measuring sonar is used for measuring the attitude data of the submersible equipment main body. The distance measurement sonar is provided with a first distance measurement sonar and a second distance measurement sonar. According to the autonomous hole passing method for the underwater vehicle, the attitude of the ROV is adjusted by using a data fusion algorithm, so that the navigation direction of the ROV is almost consistent with the axial direction of a narrow channel (through hole) in front of the ROV, and the autonomous hole passing of the ROV is realized by adjusting the attitude and the course in real time. The ROV submersible equipment main body can be maintained near the central axis of the narrow channel (via hole) in the driving process, it is guaranteed that the ROV can smoothly pass through the narrow channel (via hole), and the function that the underwater operation submersible equipment main body adjusts the posture in real time and autonomously passes through the hole is achieved.

Description

Technical Field

[0001] The invention relates to the field of underwater submersible equipment, and in particular to an autonomous through-hole method for underwater submersible equipment. Background Art

[0002] As we all know, the hole-passing technology of underwater remotely operated vehicles (ROVs) is one of the key technologies in underwater engineering, marine exploration, underwater archaeology and other fields. At present, when underwater diving equipment needs to pass through narrow underwater spaces, that is, through holes, it is mainly through remote human operation modes such as remote control and remote commands. Due to the complex structure of the underwater environment, especially the narrow space, it is difficult to obtain the equipment status and environmental parameters intuitively. During construction, it relies on a variety of sensors carried by the equipment, such as sonar positioning, positioning system (GPS), etc., which are also easily affected by factors such as signal interference or signal interruption. If the operator observes the driving conditions of the equipment and limited image information, and then manually adjusts the speed and direction of the equipment and controls the accuracy of the impact action The cost is not high either. If manual operation is performed for a long time, the operator may become fatigued and lose concentration, which may lead to equipment failure and other unexpected situations that affect the construction. Therefore, factors such as low visibility, high pressure, temperature changes and water flow in the underwater environment pose challenges to the through-hole technology of underwater equipment. In addition, the propagation speed and attenuation characteristics of underwater sound waves in water are different from those in the air, which affects the performance of the sonar. Therefore, a method can be provided to combine the feedback data of the sonar with the attitude data of the IMU and use a data fusion algorithm to adjust the attitude of the ROV so that its navigation direction is consistent with the axial direction of the through-hole. By adjusting the attitude and heading in real time, the ROV can be maintained near the central axis of the through-hole during driving. The ROV can adjust its attitude in real time to autonomously pass the hole. Summary of the invention

[0003] In view of this, the purpose of the present invention is to provide a method for adjusting the attitude and heading of an underwater submersible in real time and performing autonomous hole crossing by combining sonar feedback data and sensor attitude data and using a data fusion algorithm to adjust the attitude.

[0004] In order to achieve the purpose of the present invention, the present invention provides an autonomous through-hole method for an underwater submersible, comprising a submersible equipment body and a sensor and a ranging sonar arranged on the submersible equipment body, wherein the sensor and the ranging sonar are connected to the submersible equipment body by signal, the sensor is used to obtain the posture data of the submersible equipment body, the ranging sonar is provided with a first ranging sonar and a second ranging sonar, the first ranging sonar is used for short-distance measurement, and the second ranging sonar is used for long-distance measurement, and specifically comprises the following steps: S1. Enable the ranging sonar to detect the surrounding environment; S2. Slowing down or stopping the main body of the submersible device according to the minimum distance value obtained by the second ranging sonar; S3, switching the main body of the submersible equipment to enter the attitude adjustment mode for autonomous hole crossing; S4, detecting the distance between the sonar and the surrounding environment, and determining whether the main body of the submersible equipment has passed through the hole; S5, adjusting the driving mode of the main body of the submersible equipment to end the through-hole process; The first ranging sonar is a front sonar, and at least one front sonar is provided; The second ranging sonar is a side-mounted sonar, and at least four sets of side-mounted sonars are provided; When performing step S3, the data of the sensor and the ranging sonar are combined, and the attitude and heading of the main body of the submersible equipment are adjusted according to the data fusion algorithm.

[0005] Preferably, the front sonar is arranged on the central axis of the front end of the ROV, and can be exposed or hidden inside the bumper. The central plane of the sonar beam is basically parallel to the ROV body frame. After the load of the ROV changes, it is necessary to ensure that the maximum deviation range of the angle with the ROV body frame is 3°-5°.

[0006] Preferably, the side-mounted sonars are disposed at the four corners of the ROV body and are arranged bilaterally symmetrically, the sonars form an angle of 30° with the travel direction of the ROV, and the maximum deviation range of the angle is 3°-5°.

[0007] Preferably, the sensor is a posture sensor, which is arranged at the center of the upper surface of the ROV body, parallel to the horizontal plane when installed, and the x-axis direction is consistent with the forward direction of the ROV.

[0008] Preferably, the S3 step includes the following sub-steps: S31, initializing the state vector of the unscented Kalman filter, including position and velocity; S32, defining a nonlinear observation model to map the state vector to observable sonar distance and attitude data of the attitude sensor; S33, according to the mean and covariance matrix of the state vector, a set of σ points are selected through the transformation of the unscented Kalman filter, each σ point is propagated through the nonlinear state transfer model to obtain the predicted σ point, and the mean and covariance of the predicted σ point are calculated as the predicted state estimate and its uncertainty; S34, using the distance data of the sonar and the attitude data of the IMU, applying the observation model to the predicted σ point to obtain the predicted observation value, and calculating the mean and covariance of the observation value for the update step; S35, using the covariance of the predicted observations and the covariance of the observation noise, calculating the Kalman gain, repeating the prediction and updating steps at each time step to achieve continuous estimation of the spatial position of the ROV; S36, detecting the distance between the sonar and the surrounding space. When the front sonar has no return result and the results of the two sets of side sonars are both less than 0.5m, it is considered that there is a narrow passage ahead, and the attitude adjustment mode is entered to prepare for passing the hole; S37, customize the size of an area where the ROV can safely pass according to the cross-sectional area of ​​the ROV body, and then calculate the minimum value of the ranging results of the four sets of side-mounted sonars when the ROV is allowed to safely pass through the cross-section in combination with the installation angle of the side-mounted sonar and the set safe passing area; S38, according to the position and speed output in step S36, first reduce the forward speed of the ROV or even stop it, then adjust the posture of the ROV so that the distance values ​​D measured by the four sets of side-mounted sonars are not less than Dm, and start to pass through the hole. During the process of passing through the hole, repeat this step and adjust the posture of the ROV in real time to keep the distance value D not less than Dm; S39, detect the distance between the sonar and the surrounding environment. When the front sonar has no return result and the result of one or more side sonars is greater than or equal to 0.5m, it is considered that the narrow passage has been passed and the open environment has been entered. The ROV enters the normal driving mode and the hole-passing process ends.

[0009] Preferably, the step S1 comprises the following sub-steps: S11, judging whether there are obstacles or narrow passages in the environment in the forward direction of the ROV body according to the distance values ​​returned by the first ranging sonar and the second ranging sonar; S12: When driving normally in an open space, if there is no return result from the front sonar and the return results from the side sonars are greater than or equal to 0.5m, it is considered that the road ahead can continue to pass; S13: When driving normally in an open space, if the front sonar returns a result, it is considered that there is an obstacle ahead, and corresponding obstacle avoidance adjustments are made according to the results measured by each sonar; S14: When driving normally in an open space, if the front sonar has no return result, and when the return result of one or more side sonars is less than 0.5m, it is considered that there is a narrow passage ahead, and the process goes to step S2.

[0010] Preferably, in the sub-step S32, when acquiring the attitude data returned by the attitude sensor, the unscented Kalman filter first defines a nonlinear state equation and an observation model for estimating the current attitude, tilt angle and relative motion trend of the underwater submersible equipment body.

[0011] Preferably, in the sub-step S37, the set safe passage area is set according to the cross-sectional area of ​​the fuselage and the minimum cross-sectional area of ​​the narrow passage.

[0012] Preferably, in the sub-step S33, the predicted σ point is the sampling point sigma.

[0013] The beneficial effects of the present invention are as follows: the present invention provides an autonomous through-hole method for an underwater submersible, by arranging a sensor and a ranging sonar on the main body of the submersible equipment, the sensor is used to obtain the attitude data of the main body of the submersible equipment, the ranging sonar is provided with a first ranging sonar and a second ranging sonar, the first ranging sonar is used for short-distance measurement, and the second ranging sonar is used for long-distance measurement, the feedback data of the sonar is combined with the attitude data of the attitude sensor IMU, and a data fusion algorithm is used to adjust the attitude of the ROV, so that its navigation direction and the axial direction of the narrow channel (through-hole) in front can be almost consistent, and by adjusting the attitude and heading in real time, the main body of the ROV submersible equipment can be maintained near the central axis of the narrow channel (through-hole) during driving, ensuring that the ROV can smoothly pass through the narrow channel (through-hole), and realizing the function of real-time adjustment of the attitude of the main body of the underwater operation submersible equipment for autonomous through-hole. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The above and other purposes, features and advantages of the present invention will become more clear by more specific description of the preferred embodiments of the present invention shown in the accompanying drawings. The same reference numerals indicate the same parts in all the accompanying drawings, and the accompanying drawings are not deliberately scaled to the actual size, and the focus is on illustrating the main purpose of the present invention.

[0015] Figure 1 A schematic diagram of an apparatus for an autonomous through-hole method for an underwater submersible according to the present invention; Figure 2 A basic flow chart of an autonomous through-hole method for an underwater submersible according to the present invention; Figure 3 A schematic diagram of a specific embodiment of an autonomous through-hole method for an underwater submersible according to the present invention; Figure 4 It is a schematic diagram of a preferred embodiment of an autonomous through-hole method for an underwater submersible according to the present invention.

[0016] In the figure: 1. The main body of the submersible equipment; 2. The sensor; 3. The front sonar; 4-7. The side sonar. DETAILED DESCRIPTION

[0017] To facilitate understanding of the present invention, the present invention will be described more fully below with reference to the relevant drawings.

[0018] It should be noted that when an element is considered to be "connected" to another element, it may be directly connected to another element and integrated therewith, or there may be an intermediate element at the same time. The terms "installed", "one end", "the other end" and similar expressions used herein are for illustrative purposes only.

[0019] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this document pertains. The terms used in the specification herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0020] like Figure 1-4 As shown, a preferred embodiment of the present invention provides an autonomous hole-passing method for an underwater submersible, comprising a submersible equipment body 1 and a sensor 2 and a ranging sonar arranged on the submersible equipment body. The sensor 2 and the ranging sonar are connected to the submersible equipment body by signal. The sensor 2 is used to obtain the posture data of the submersible equipment body. The ranging sonar is provided with a first ranging sonar and a second ranging sonar. The first ranging sonar is used for short-distance measurement, and the second ranging sonar is used for long-distance measurement. In this embodiment, the submersible equipment body is also referred to as ROV. The ROV hole-passing is passing through a narrow channel, which specifically includes the following steps: S1. Enable the ranging sonar to detect the surrounding environment; S2. Slowing down or stopping the main body of the submersible device according to the minimum distance value obtained by the second ranging sonar; S3, switching the main body of the submersible equipment to enter the attitude adjustment mode for autonomous hole crossing; S4, detecting the distance between the sonar and the surrounding environment, and determining whether the main body of the submersible equipment has passed through the hole; S5, adjusting the driving mode of the main body of the submersible equipment to end the through-hole process; The first range-finding sonar is a front sonar 3, and at least one front sonar 3 is provided; The second range-finding sonar is a side-mounted sonar 4, and at least four sets of the side-mounted sonar 4 are provided; When performing step S3, the data of sensor 2 and ranging sonar are combined, and the attitude and heading of the submersible equipment body 1 are adjusted according to the data fusion algorithm, so that the ROV can maintain near the central axis of the narrow channel (through-hole) during driving, ensuring that the ROV can pass through the narrow channel (through-hole) smoothly, and realizing the function of real-time attitude adjustment and autonomous through-hole of the underwater operation ROV.

[0021] refer to Figure 1-4As shown, in a further preferred embodiment, the front sonar 3 is arranged on the central axis of the front end of the ROV, and can be exposed or hidden inside the anti-collision bar. The central plane of the sonar beam is basically parallel to the ROV body frame. After the load of the ROV changes, it is necessary to ensure that the maximum deviation range of the angle with the ROV body frame is 3°-5°. Specifically, in this embodiment, the maximum deviation value of the angle with the ROV body frame is 5°.

[0022] refer to Figure 1-4 As shown, in a further preferred embodiment, the side sonar 4 is set at the four corners of the ROV body and is arranged symmetrically. The sonar forms an angle of 30° with the travel direction of the ROV, and the maximum deviation range of the angle is 3°-5°. Specifically, in this embodiment, the maximum deviation value of the angle is 5°.

[0023] refer to Figure 1-4 As shown, in a preferred embodiment, the sensor 2 is a posture sensor, which is arranged at the center of the upper surface of the ROV body, parallel to the horizontal plane when installed, and the x-axis direction is consistent with the forward direction of the ROV.

[0024] refer to Figure 1-4 As shown, in a preferred embodiment, during the posture adjustment stage, when the ROV determines that there is a narrow space ahead, the program switches the ROV mode from the normal driving mode to the posture adjustment mode to perform posture adjustment before passing through the hole; The S3 step includes the following sub-steps: S31, initializing the state vector of the unscented Kalman filter (UKF), including position and velocity; S32. Define a nonlinear observation model to map the state vector to the observable sonar distance and attitude data of the attitude sensor; because the ROV motion is nonlinear, when obtaining the attitude data returned by the IMU, the UKF (unscented Kalman filter) first defines a nonlinear state equation and observation model to estimate the current attitude, tilt angle, relative motion trend, etc. of the ROV; S33, according to the mean and covariance matrix of the state vector, a set of σ points are selected through the transformation of the unscented Kalman filter, each σ point is propagated through the nonlinear state transfer model to obtain the predicted σ point, and the mean and covariance of the predicted σ point are calculated as the predicted state estimate and its uncertainty; S34, using the distance data of the sonar and the attitude data of the IMU, applying the observation model to the predicted σ point to obtain the predicted observation value, and calculating the mean and covariance of the observation value for the update step; S35, using the covariance of the predicted observations and the covariance of the observation noise, calculating the Kalman gain, repeating the prediction and updating steps at each time step to achieve continuous estimation of the spatial position of the ROV; S36, detecting the distance between the sonar and the surrounding space. When there is no return result from the front sonar 3, and the results of the two sets of side sonars 4 are both less than 0.5m, it is considered that there is a narrow passage ahead, and the attitude adjustment mode is entered to prepare for passing the hole; S37, customize the size of an area where the ROV can pass safely according to the cross-sectional area of ​​the ROV fuselage, and then calculate the minimum value Dm of the ranging results of the four sets of side-mounted sonars 4 when the ROV is allowed to pass safely through the cross-section in combination with the installation angle of the side-mounted sonar 4 and the set safe passing area; S38, according to the position and speed output in step S36, first lower the forward speed of the ROV or even stop advancing, then adjust the posture of the ROV so that the distance values ​​D measured by the four sets of side-mounted sonars 4 are not less than Dm, and start passing through the hole. In the process of passing through the hole, repeat this step, and adjust the posture of the ROV in real time to keep the distance value D not less than Dm; collect the distance data of the surrounding space through the sonar matrix, calculate the four target distances through the feedback data of the side-mounted sonar 4 matrix, and then calculate a passage space (similar to a rectangular cross section) located in front of the ROV that is nearly parallel to the ROV body frame (angle ≤5°) in combination with the installation angle and installation position, and adjust the posture until the passage cross section is slightly wider than the cross section of the ROV, and it is considered that the current state can be safely advanced and start passing through the narrow passage (through the hole); S39, detecting the distance between the sonar and the surrounding environment space. When there is a no-return result from the front sonar 3, and the result of one or more side sonars is greater than or equal to 0.5m, it is considered that the narrow passage has been passed and the open environment has been entered. The ROV enters the normal driving mode and the hole-passing process ends; The embodiment of the present invention provides a more efficient solution by collecting the data of the sonar matrix and the attitude data of the attitude measurement unit (IMU), including acceleration, angular velocity and possible magnetic field data, and performing data fusion.

[0025] refer to Figure 1-4 As shown, in a preferred embodiment, step S1 includes the following sub-steps: S11, judging whether there are obstacles or narrow passages in the environment in the forward direction of the ROV body according to the distance values ​​returned by the first ranging sonar and the second ranging sonar; S12: In the case of normal driving in an open space, if there is no return result from the front sonar 3 and the return results from the side sonar 4 are greater than or equal to 0.5m, it is considered that the road ahead can continue to pass; S13, when driving normally in an open space, if the front sonar 3 returns a result, it is considered that there is an obstacle ahead, and corresponding obstacle avoidance adjustments are made according to the results measured by each sonar; S14, when driving normally in an open space, if the front sonar 3 has no return result, and when the return result of one or more side sonars 4 is less than 0.5m, it is considered that there is a narrow passage ahead, and the process goes to step S2; The present invention provides a more efficient solution, which collects data from the sonar matrix and the attitude data of the attitude measurement unit (IMU), including acceleration, angular velocity and possible magnetic field data, and performs data fusion. Before fusion, the sonar data needs to be filtered to remove noise and multipath effects, and the IMU data needs to be calibrated to eliminate sensor bias and errors. The sonar data is first used for preliminary positioning to determine the relative position of the ROV and the surrounding environment, and to determine whether there is a narrow passage ahead.

[0026] refer to Figure 1-4 As shown, in a further preferred embodiment, in sub-step S32, when acquiring the attitude data returned by the attitude sensor, the unscented Kalman filter first defines a nonlinear state equation and an observation model to estimate the current attitude, tilt angle and relative motion trend of the underwater submersible equipment body.

[0027] refer to Figure 1-4 In a preferred embodiment, in the sub-step S37, the set safe passage area is set according to the cross-sectional area of ​​the fuselage and the minimum cross-sectional area of ​​the narrow passage.

[0028] refer to Figure 1-4 In a preferred embodiment, in sub-step S33, the predicted σ point is the sampling point sigma.

[0029] The present invention also provides an embodiment of sub-step S37: Specifically, refer to Figure 4 , use length and width to represent the cross-sectional area in centimeters. Assume that the cross-sectional area of ​​the ROV body is (L, W) (in centimeters), and then define the area that the ROV can safely pass through as (L+2, W+2). Since the installation angle is 30°, according to sin(30°)=Δd / Dm, and Δd =((L+2)-L) / 2, Δd is a reserved passing distance according to the length and width of the equipment cross section. According to the formula, the minimum value Dm of the side-mounted sonar ranging result when the ROV can safely pass through a narrow channel can be obtained.

[0030] Embodiment 1:

[0031] Specifically, when the cross-sectional area of ​​the ROV body is (49.9, 29.6), the area that the ROV can safely pass is (51.9, 31.6); substituting into the formula sin(30)=Δd / Dm, Δd=(49.9+2)-49.9) / 2, we get Δd=1, and the minimum value of the ranging result Dm=2 is obtained.

[0032] Embodiment 2:

[0033] When the cross-sectional area of ​​the ROV body is (38, 30), the area that the ROV can safely pass is (40, 32); substituting into the formula sin(30)=Δd / Dm, Δd=((38+2)-L) / 2, Δd=1, the minimum value of the ranging result Dm=2 is obtained.

[0034] Embodiment 3:

[0035] When the cross-sectional area of ​​the ROV body is (45, 40), the area that the ROV can safely pass is (47, 42); substituting into the formula sin(30)=Δd / Dm, Δd =((L+2)-L) / 2, Δd=1, and the minimum value of the ranging result Dm=2 is obtained.

[0036] Through the above three embodiments, it can be concluded that when ROV bodies of various sizes pass through narrow passages, if the length of the current environment space exceeds the length and width of the cross section of the device body by more than 1 cm, it is considered that the device can pass through the narrow passage safely.

[0037] The beneficial effects of the present invention are as follows: the present invention provides an autonomous through-hole method for an underwater submersible, by arranging a sensor and a ranging sonar on the main body of the submersible equipment, the sensor is used to obtain the attitude data of the main body of the submersible equipment, the ranging sonar is provided with a first ranging sonar and a second ranging sonar, the first ranging sonar is used for short-distance measurement, and the second ranging sonar is used for long-distance measurement, the feedback data of the sonar is combined with the attitude data of the attitude sensor IMU, and a data fusion algorithm is used to adjust the attitude of the ROV, so that its navigation direction and the axial direction of the narrow channel (through-hole) in front can be almost consistent, and by adjusting the attitude and heading in real time, the main body of the ROV submersible equipment can be maintained near the central axis of the narrow channel (through-hole) during driving, ensuring that the ROV can smoothly pass through the narrow channel (through-hole), and realizing the function of real-time adjustment of the attitude of the main body of the underwater operation submersible equipment to autonomously pass through the hole.

[0038] In the present application, unless otherwise clearly specified and limited, a first feature being “above” or “below” a second feature may mean that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, a first feature being “above”, “above”, and “above” a second feature may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being “below”, “below”, and “below” a second feature may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is lower in level than the second feature.

[0039] In the description of this specification, the description with reference to the terms "preferred embodiment", "further embodiment", "other embodiments" or "specific example" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

[0040] The above embodiments only express several implementation methods of the invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for those of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the invention, and these all belong to the protection scope of the invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. An autonomous hole-through method for an underwater submersible, comprising a submersible equipment body and a sensor and a ranging sonar arranged on the submersible equipment body, wherein the sensor and the ranging sonar are connected to the submersible equipment body by signal, wherein the sensor is used to obtain the posture data of the submersible equipment body, and the ranging sonar is provided with a first ranging sonar and a second ranging sonar, wherein the first ranging sonar is used for short-distance measurement, and the second ranging sonar is used for long-distance measurement, and wherein: The specific steps include: S1. Enable the ranging sonar to detect the surrounding environment; S2. Slowing down or stopping the main body of the submersible device according to the minimum distance value obtained by the second ranging sonar; S3, switching the main body of the submersible equipment to enter the attitude adjustment mode for autonomous hole crossing; S4, detecting the distance between the sonar and the surrounding environment, and determining whether the main body of the submersible equipment has passed through the hole; S5, adjusting the driving mode of the main body of the submersible equipment to end the through-hole process; The first ranging sonar is a front sonar, and at least one front sonar is provided; The second ranging sonar is a side-mounted sonar, and at least four sets of side-mounted sonars are provided; When performing step S3, the data of the sensor and the ranging sonar are combined, and the attitude and heading of the main body of the submersible equipment are adjusted according to the data fusion algorithm.

2. The autonomous through-hole method for underwater submersible according to claim 1, characterized in that: The front sonar is arranged on the central axis of the front end of the ROV, and can be exposed or hidden inside the anti-collision bar. The central plane of the sonar beam is basically parallel to the ROV body frame. After the load of the ROV changes, it is necessary to ensure that the maximum deviation range of the angle with the ROV body frame is 3°-5°.

3. The autonomous through-hole method for underwater submersible according to claim 1, characterized in that: The side-mounted sonars are arranged at the four corners of the ROV body and are arranged symmetrically. The sonars form an angle of 30° with the ROV's travel direction, and the maximum deviation range of the angle is 3°-5°.

4. The autonomous through-hole method for underwater submersible according to claim 1, characterized in that: The sensor is a posture sensor, which is arranged at the center of the upper surface of the ROV body, parallel to the horizontal plane when installed, and the x-axis direction is consistent with the forward direction of the ROV.

5. The autonomous through-hole method for underwater submersible according to claim 1, characterized in that: The S3 step includes the following sub-steps: S31, initializing the state vector of the unscented Kalman filter, including position and velocity; S32, defining a nonlinear observation model to map the state vector to observable sonar distance and attitude data of the attitude sensor; S33, according to the mean and covariance matrix of the state vector, a set of σ points are selected through the transformation of the unscented Kalman filter, each σ point is propagated through the nonlinear state transfer model to obtain the predicted σ point, and the mean and covariance of the predicted σ point are calculated as the predicted state estimate and its uncertainty; S34, using the distance data of the sonar and the attitude data of the IMU, applying the observation model to the predicted σ point to obtain the predicted observation value, and calculating the mean and covariance of the observation value for the update step; S35, using the covariance of the predicted observations and the covariance of the observation noise, calculating the Kalman gain, repeating the prediction and updating steps at each time step to achieve continuous estimation of the spatial position of the ROV; S36, detecting the distance between the sonar and the surrounding space. When the front sonar has no return result and the results of the two sets of side sonars are both less than 0.5m, it is considered that there is a narrow passage ahead, and the attitude adjustment mode is entered to prepare for passing the hole; S37, customize the size of an area where the ROV can safely pass according to the cross-sectional area of ​​the ROV body, and then calculate the minimum value of the ranging results of the four sets of side-mounted sonars when the ROV is allowed to safely pass through the cross-section in combination with the installation angle of the side-mounted sonar and the set safe passing area; S38, according to the position and speed output in step S36, first reduce the forward speed of the ROV or even stop it, then adjust the posture of the ROV so that the distance values ​​D measured by the four sets of side-mounted sonars are not less than Dm, and start to pass through the hole. During the process of passing through the hole, repeat this step and adjust the posture of the ROV in real time to keep the distance value D not less than Dm; S39, detect the distance between the sonar and the surrounding environment. When the front sonar has no return result and the result of one or more side sonars is greater than or equal to 0.5m, it is considered that the narrow passage has been passed and the open environment has been entered. The ROV enters the normal driving mode and the hole-passing process ends.

6. The autonomous through-hole method for underwater submersible according to claim 1, characterized in that: The step S1 comprises the following sub-steps: S11, judging whether there are obstacles or narrow passages in the environment in the forward direction of the ROV body according to the distance values ​​returned by the first ranging sonar and the second ranging sonar; S12: When driving normally in an open space, if there is no return result from the front sonar and the return results from the side sonars are greater than or equal to 0.5m, it is considered that the road ahead can continue to pass; S13: When driving normally in an open space, if the front sonar returns a result, it is considered that there is an obstacle ahead, and corresponding obstacle avoidance adjustments are made according to the results measured by each sonar; S14: When driving normally in an open space, if the front sonar has no return result, and when the return result of one or more side sonars is less than 0.5m, it is considered that there is a narrow passage ahead, and the process goes to step S2.

7. The autonomous through-hole method for an underwater submersible according to claim 5, characterized in that: In the sub-step S32, when acquiring the attitude data returned by the attitude sensor, the unscented Kalman filter first defines a nonlinear state equation and an observation model for estimating the current attitude, tilt angle and relative motion trend of the underwater submersible equipment body.

8. The autonomous through-hole method for an underwater submersible according to claim 5, characterized in that: In the sub-step S37, the set safe passage area is set according to the cross-sectional area of ​​the fuselage and the minimum cross-sectional area of ​​the narrow passage.

9. The autonomous through-hole method for an underwater submersible according to claim 5, characterized in that: In the sub-step S33, the predicted σ point is the sampling point sigma.

Images