Automatic navigation system of deep-sea unmanned vehicle and control method

Abstract

The invention provides an automatic navigation system of a deep-sea unmanned vehicle and a control method, the method adopts a three-section navigation strategy, and uses an ultra-short baseline positioning system USBL to monitor the real-time distance between the vehicle and a deep-sea space station in real time and obtain the real-time position of the vehicle; entering a coarse positioning stage when the distance is determined, acquiring the current position of the vehicle by adopting an inertial navigation system, correcting the current position of the vehicle through the real-time position of the vehicle, and controlling an error by utilizing a course angle PID (Proportion Integration Differentiation); switching to a fine positioning stage at the moment, starting a Doppler log to obtain the real-time movement speed of the vehicle, and carrying out fusion correction on the real-time movement speed and the real-time position of the vehicle by adopting a Kalman filtering algorithm; and when the distance is determined, a docking stage is started, and high-precision docking is completed in combination with binocular vision feature recognition. According to the strategy, sensor combination and an algorithm are optimized in a staged progressive manner, error accumulation is effectively inhibited, and meanwhile unmanned autonomous control of the whole process is achieved.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of deep-sea underwater carrier navigation, and particularly relates to an automatic navigation system and control method of a deep-sea unmanned carrier. BACKGROUND

[0002] Underwater navigation technology is a bottleneck problem for the development and application of underwater vehicles. Compared with air navigation, underwater navigation has the characteristics of long working time, complex environment, few information sources, and high concealment requirement, and therefore has greater difficulty. At present, underwater vehicle navigation technology mainly includes four categories, namely, dead reckoning and inertial navigation system, acoustic navigation, geophysical navigation, and integrated navigation. However, the existing navigation technology has the following problems:

[0003] 1) It is difficult to maintain navigation accuracy with the distance, and the AUV relies on the inertial navigation DVL. The error of the far-segment current will accumulate a large drift distance, thereby reducing the accuracy. 2) The traditional hard cone or guide rail requires an attitude error of ±0.1 m, and the sea state changes easily collide, which leads to frequent damage of the hatch seal and sensor.

[0004] 3) Fixed thrust or single-mode propulsion cannot balance low energy consumption and stable attitude, and high-frequency supply tasks are often forced to be interrupted due to insufficient endurance. The above problems restrict the construction of a normal, high-frequency, and low-cost logistics system for deep-sea space stations. SUMMARY

[0005] The purpose of the present application is to solve the problem of low navigation accuracy and unstable attitude leading to interrupted endurance in the prior art.

[0006] To achieve the above purpose, the technical scheme is as follows: An automatic navigation control method of a deep-sea unmanned carrier includes the following steps: S1: using an ultra-short baseline positioning system USBL to monitor the real-time distance D between the carrier and the deep-sea space station and obtain the real-time position of the carrier ; S2: when the distance is less than 5 m, entering a coarse positioning stage, using an inertial navigation system INS to obtain the current position of the carrier , and correcting the current position of the carrier through the real-time position of the carrier , comparing the corrected current position of the carrier with the position of the deep-sea space station, obtaining the deviation of the target heading angle and the current actual heading angle , dynamically eliminating the deviation through the heading angle PID , and controlling the distance error between the carrier and the space station within ±5 m; S3: when the distance switches to the fine positioning phase, the Doppler log DVL is started to obtain the real-time motion speed of the carrier, and the Kalman filtering algorithm is used to combine the real-time motion speed of the carrier and the real-time position of the carrier obtained by the ultra-short baseline positioning system USBL fusion correction is performed; S4: when the distance is started, the real-time heading and attitude are adjusted by using binocular vision feature recognition, and the fuzzy PID control algorithm is used to realize high-precision docking of the carrier and the deep sea space station.

[0007] Further, step S1 is specifically as follows: S101, the ultra-short baseline positioning system USBL transmits a detection sound wave to the deep sea space station transponder, and records the round-trip propagation time of the sound wave , and calculates the real-time distance between the carrier and the deep sea space station ; S102, according to the time difference of receiving of different transducers of the ultra-short baseline positioning system USBL, the azimuth and the pitch angle of the carrier relative to the space station are calculated , based on the coordinate system established with the space station as the reference origin, the spherical coordinates are converted into three-dimensional rectangular coordinates by using the coordinate conversion formula: , to obtain the real-time position of the carrier

[0008] Further, step S2 is specifically as follows: S201, the inertial navigation system is used to integrate the linear acceleration obtained by the accelerometer and the angular velocity obtained by the gyroscope to obtain the current position of the carrier; S202, the real-time position of the carrier is used to correct the current position of the carrier , to obtain the corrected current position of the carrier; S203, the corrected current position of the carrier is compared with the known position of the deep sea space station, to obtain the deviation of the target heading angle and the current actual heading angle , the deviation is dynamically eliminated by the heading angle PID , when it is found that the carrier is deviating from the target direction, the advancing direction is corrected; if the target point has been approached, the adjustment speed is slowed down to avoid excessive correction, and the carrier keeps effective tracking of the target point in the coarse positioning phase, so that the distance error is controlled within 5 meters.

[0009] Further, step S201 is specifically as follows: a, the gyroscope is used to collect the angular velocity of the carrier in the carrier coordinate system in real time, and the angular velocity , calculate the attitude angle : ; b, construct the attitude conversion matrix through the attitude angle, collect the linear acceleration under the carrier coordinate system in real time according to the three-axis accelerometer, project the linear acceleration under the carrier coordinate system into the geographic coordinate system by using the attitude conversion matrix, and obtain the linear acceleration of the carrier under the geographic coordinate system ; , integrate the linear acceleration under the geographic coordinate system , obtain the real-time velocity under the geographic coordinate system : ; c, integrate the velocity , obtain the current position of the carrier under the inertial coordinate system : .

[0010] Further, step S202 is specifically as follows: a, construct a position correction model, calculate the deviation between the current position and the real-time position , and the formula is as follows: , wherein reflects the difference between the current position of the inertial navigation system (INS) and the real-time position, and contains the cumulative error information of the inertial navigation; b, assign weight coefficients and to and , respectively, and obtain the corrected position by weighted fusion: : .

[0011] Further, step S3 is specifically as follows: S301, emit sound waves to the underwater through the Doppler log (DVL) and receive echoes, and calculate the carrier velocity by using the Doppler shift formula: , wherein is the sound speed in water, is the frequency difference between the emitted and received sound waves, is the frequency of the emitted sound waves, is the included angle between the sound beam and the motion direction of the carrier; according to the carrier velocity , output a three-dimensional velocity vector under the carrier coordinate system , convert the three-dimensional velocity vector into the geographic coordinate system, and obtain the velocity under the geographic coordinate system , wherein is the attitude conversion matrix; S302, obtaining the real-time position of the carrier by the ultra-short baseline positioning system (USBL) Converting to the geographic coordinate system, the position in the geographic coordinate system is obtained , is the latitude, is the longitude, is the altitude; the position in the geographic coordinate system is obtained and the position of the deep sea space station get the inertial real position information ; wherein, and is the latitude, and is the longitude, and is the altitude; S303, according to the inertial navigation real geographic system position and velocity construct the measurement equation: In the formula, is the measurement value of the system at the moment, is the system measurement matrix, is the system state quantity, is the position measurement noise vector; is the latitude obtained by the inertial navigation system, is the longitude obtained by the inertial navigation system, is the altitude obtained by the inertial navigation system, is the east speed obtained by the inertial navigation system, is the north speed obtained by the inertial navigation system, is the sky speed obtained by the inertial navigation system, is the east speed obtained by the Doppler velocity log (DVL), is the north speed obtained by the Doppler velocity log (DVL), is the north speed obtained by the Doppler velocity log (DVL); The measurement equation of Kalman filter is used to calculate the state estimation value at time k; the state estimation value is used as the optimal state parameter of the carrier at the current moment, and is directly used in the real-time navigation system to correct the positioning drift caused by the measurement error of a single sensor.

[0012] Further, step S4 is specifically as follows: S401, adjusting the heading and attitude by using binocular vision recognition, first collecting the docking interface image by the camera, extracting the target feature points of the docking interface by using the improved SURF algorithm, and inputting the target feature points into the PID control algorithm to adjust the heading and attitude; S402, fine-tune the thruster and rudder through the PID control algorithm to realize the precise docking of the carrier and the deep sea space.

[0013] An automatic navigation system of a deep-sea unmanned carrier comprises: An inertial navigation system (INS) is composed of an inertial measurement unit of a three-axis rate gyroscope, a three-axis accelerometer and a microcomputer, which is used to acquire angular velocity and linear acceleration in a carrier coordinate system in real time, and output position, velocity and heading data in a geographic coordinate system through coordinate conversion; An ultra-short baseline positioning system (USBL) communicates with a surface platform through acoustic signals to acquire the distance between the carrier and the deep-sea space station and the real-time position of the carrier in real time; A Doppler velocity log (DVL) is used to measure the real-time motion speed of the carrier and fuse the data of the ultra-short baseline positioning system (USBL) to correct cumulative errors; A camera is installed at the front end or the bottom of the carrier and is used for image acquisition when docking a close-range target; A controller is integrated with a PID control algorithm and is used to receive multi-source data of the inertial navigation system (INS), the Doppler velocity log (DVL), the ultra-short baseline positioning system (USBL) and the camera and generate control instructions of the thruster and the rudder; A thruster and rudder combination realizes power output and heading adjustment of the carrier according to the control instructions.

[0014] The present application has the following advantages: The present application divides the underwater autonomous navigation process into three stages: coarse positioning (> 500m), fine positioning (50m-500m) and docking positioning (< 50m) by introducing a phased sensor fusion navigation mechanism and a controller switching strategy. Each stage corresponds to a different sensor combination and fusion algorithm. The system can automatically complete navigation system switching and attitude adjustment according to the real-time distance of the target and the quality of the sensor signal. The present application not only effectively overcomes the error accumulation of the inertial navigation system, improves the underwater long-distance navigation accuracy, but also improves the medium and short distance target recognition ability through multi-source information fusion, and realizes centimeter-level micro-error docking in the docking stage. The method has the advantages of high automation, strong environmental adaptability and high robustness, and improves the efficiency of unmanned carrier operation of the deep-sea space station. BRIEF DESCRIPTION OF DRAWINGS

[0015] Figure 1 It is the automatic navigation control method flow chart of the unmanned carrier provided by the present application; Figure 2 It is the automatic cruise flexible coarse positioning stage diagram provided by the present application; Figure 3 It is the automatic cruise flexible fine positioning stage diagram provided by the present application; Figure 4The application provides an automatic cruise flexible docking phase diagram. DETAILED DESCRIPTION

[0016] In order to make the purpose, technical scheme and advantages of the present application more clear, the present application is further described in detail below in combination with embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and do not limit the present application.

[0017] The application principle of the present application is further described below in combination with the drawings and specific embodiments.

[0018] An automatic navigation system of a deep-sea unmanned carrier includes an inertial navigation system (INS), a Doppler log DVL, an ultra-short baseline positioning system USBL, a controller, a camera, a propeller and a rudder.

[0019] The inertial navigation system is used to obtain the position, speed and heading of the carrier. The inertial navigation system is composed of three rate gyros, three linear accelerometers and a microcomputer. The three gyroscopes and three accelerometers are usually assembled together to form an inertial measurement unit (IMU). The gyroscopes are used to collect the rotation angular velocity of the carrier in the carrier coordinate system in real time. The linear accelerometers are used to realize the coordinate conversion between the carrier coordinate system and the geographic coordinate system.

[0020] The Doppler log (DVL) is used to measure the real-time motion speed of the carrier, and the data of the ultra-short baseline positioning system USBL are fused to correct the cumulative error.

[0021] The ultra-short baseline positioning system (USBL) communicates with the water surface ship or platform through underwater acoustic signals, and measures the distance and bearing of the carrier.

[0022] The controller integrates the PID control algorithm, is used to receive the multi-source data of the inertial navigation system INS, the Doppler log DVL, the ultra-short baseline positioning system USBL and the camera, and generates the control instructions of the propeller and the rudder.

[0023] The camera is installed at the front end or the bottom of the carrier, and is used for image acquisition when docking a close-range target.

[0024] The propeller and the rudder are combined, and realize the power output and the heading adjustment of the carrier according to the control instructions.

[0025] The position relationship between the systems: the camera is generally installed at the front end or the bottom of the carrier and faces the docking direction of the target. The INS and the DVL are installed in the middle or the bottom of the hull and are fixed firmly with the controller; the USBL antenna is installed at the top of the carrier and faces the water surface to receive acoustic signals. The propeller and the rudder are distributed at the tail or the two sides, so as to facilitate the position movement and control.

[0026] like Figures 1-4 As shown, the present invention discloses an automatic navigation control method for a deep-sea unmanned vehicle, employing a three-stage navigation strategy. The navigation mode and control algorithm are dynamically switched based on the real-time distance between the vehicle and the deep-sea space station. In the coarse positioning stage (distance > 500m, i.e., the real-time distance between the vehicle and the deep-sea space station during guidance), inertial navigation and sonar beacon fusion positioning are used, achieving ±5m accuracy through PID control of the heading angle. In the fine positioning stage (500m to 50m), the system switches to Doppler log (DVL) and ultra-short baseline (USBL) sonar collaborative positioning, using an extended Kalman filter (EKF) to further improve accuracy. When the distance is ≤ 50m, the docking stage is initiated, combining binocular visual feature recognition to achieve fully autonomous unmanned control throughout the process.

[0027] An automatic navigation control method for a deep-sea unmanned vehicle includes the following steps: S1: Real-time monitoring of the distance between the launch vehicle and the deep-sea space station. The details are as follows: The sound waves were transmitted to the transponder of the deep-sea space station using the Ultra-Short Baseline Positioning System (USBL), and the round-trip time of the sound waves was recorded. It also calculates the real-time distance between the launch vehicle and the deep-sea space station. Based on the reception time difference of different transducers in the Ultra-Short Baseline Positioning System (USBL), the azimuth angle of the launch vehicle relative to the space station is calculated. and pitch angle A coordinate system established with the space station's location as the reference origin, using the same coordinate transformation formula, is used to create spherical coordinates. Convert to 3D Cartesian coordinates: To obtain the final position observation values It reflects the real-time position of the launch vehicle in the carrier coordinate system.

[0028] S2: When the distance Then, in the coarse positioning stage, an inertial navigation system and ultra-short baseline (USBL) sonar are used for coordinated positioning, and a positioning accuracy of ±5m is achieved through a heading angle PID control algorithm; the details are as follows: S201. The inertial navigation system calculates the vehicle's current position by integrating the linear acceleration obtained from the accelerometer and the rotational angular velocity obtained from the gyroscope twice. Based on the rotational angular velocity measured by the three gyroscopes and the linear acceleration measured by the three accelerometers, the vehicle's current position is calculated using the rotation angle and linear acceleration as follows:

[0029] a. Utilize a gyroscope to collect the angular velocity of the carrier's rotation in the carrier coordinate system in real time, and based on the rotation angular velocity... Calculate attitude angle : ; b. Construct an attitude transformation matrix based on attitude angles (this technique is relatively mature and will not be elaborated here) to achieve coordinate transformation between the vehicle coordinate system and the geographic coordinate system. Linear acceleration in the vehicle coordinate system is collected in real time using a triaxial accelerometer. The attitude transformation matrix is ​​then used to convert the linear acceleration... Projecting this onto a geographic coordinate system yields the linear acceleration of the vehicle in that coordinate system. linear acceleration in geographic coordinate system Integrate to obtain the real-time velocity in the geographic coordinate system. : ;

[0030] c. Regarding speed Integrate to obtain the current position of the launch vehicle in the inertial coordinate system. : The obtained position This is the current position of the launch vehicle.

[0031] S202, Using the real-time distance between the launch vehicle and the deep-sea space station from step S1. This is used to correct the current position calculated by the vehicle's inertial navigation system, playing a real-time correction role and ensuring navigation accuracy during the coarse positioning stage. Specifically:

[0032] a. Construct a position correction model and calculate the current position. With real-time location deviation The formula is: ,in It reflects the difference between the current position of the inertial navigation system and the external reference position, and includes the cumulative error information of the inertial navigation; b. Based on the reliability of the two types of location information, and Assign weight coefficients and (satisfy The corrected position is obtained through weighted fusion. : When real-time location High reliability (e.g., stable signal, small error) increases (like ), to enhance the correction effect; When real-time location When reliability is low (e.g., when the signal is interfered with or has large errors), reduce... (like This reduces its impact on inertial navigation positioning and avoids introducing new errors.

[0033] S203. Utilizing PID (Proportional-Integral-Derivative) control for heading control, the control system determines whether the current motion is stable and adjusts the thrust and heading angle of the thrusters accordingly. The corrected position of the launch vehicle is compared with the known position of the deep-sea space station to obtain the deviation between the target heading angle and the current actual heading angle. Dynamic elimination of heading angle PID Deviation: When the launch vehicle is found to be deviating from the target direction, the direction of travel is corrected; if it has already approached the target point, the adjustment speed is slowed down to avoid over-correction. During the coarse positioning phase, the launch vehicle maintains effective tracking of the target point and ultimately controls the distance error within 5 meters.

[0034] S3: When The system then switches to the fine positioning stage, initiating coordinated positioning using a Doppler log (DVL) and ultra-short baseline (USBL) sonar, and employing a Kalman filter algorithm for data fusion and correction.

[0035] S301. The vehicle speed is calculated using the Doppler frequency shift formula by transmitting sound waves underwater via a Doppler log (DVL) and receiving the echoes. in, The speed of sound in water, The frequency difference between the transmitted and received sound waves, To transmit sound waves at a frequency, The angle between the sound beam and the direction of the launch vehicle's motion; ultimately determined by the launch vehicle's speed. Output the three-dimensional velocity vector in the carrier coordinate system. This reflects the real-time velocity of the carrier in the x, y, and z axes. The three-dimensional velocity vector in the carrier coordinate system... The attitude transformation matrix is ​​used to convert the coordinates to the geographic coordinate system to obtain the velocity in the geographic coordinate system. ; This is the attitude transformation matrix.

[0036] S302, Obtain the real-time position of the vehicle from the Ultra-Short Baseline Positioning System (USBL) Convert to a geographic coordinate system to obtain the location in the geographic coordinate system. , Latitude Longitude Elevation; location in geographic coordinate system and the location of the deep-sea space station Obtain the true position information of inertia ;in, and Latitude and Longitude and For height.

[0037] S303. The data is fused using an Extended Kalman Filter (EKF). While the velocity information provided by the DVL is stable, it fails to reflect the actual position; the position information provided by the USBL, although accurate, has a low update frequency and is susceptible to interference. Therefore, an Extended Kalman Filter (EKF) is needed to combine them, eliminating their respective shortcomings and improving overall positioning stability and accuracy.

[0038] S303, Based on location information and speed Construct the measurement equation: In the formula, For the system The measured value at time, For the system measurement matrix, For system state variables, This is the noise vector for position measurement. To obtain latitude for the inertial navigation system, The longitude obtained by the inertial navigation system. The altitude obtained by the inertial navigation system. The speed obtained by the inertial navigation system. The north velocity obtained by the inertial navigation system. The speed obtained by the inertial navigation system The speed obtained by the Doppler log (DVL) The north velocity obtained by the Doppler log (DVL) The north velocity was obtained by the Doppler log (DVL).

[0039] The state estimate at time k can then be calculated using the measurement equations of the Kalman filter. , For the inaccurate angle of the data platform, For the speed error of the carrier, For the positional error of the carrier, For random constant drift of the gyroscope, The state estimation value is used as the optimal state parameter of the carrier at the current time, and is directly used in the real-time navigation system to provide high-precision position (latitude, longitude, and height) and motion state (speed and angle) reference for the carrier, correct the positioning drift caused by single sensor measurement error, and ensure that the carrier accurately knows its position and motion trend in the underwater environment, thereby preparing for the next stage of docking.

[0040] S4: when the docking stage is started, the heading and attitude are adjusted in real time by using binocular vision feature recognition, and high-precision docking of ±2 cm is achieved by using a fuzzy PID control algorithm.

[0041] (1) The specific method of adjusting the heading and attitude by binocular vision feature recognition is as follows: a. Image acquisition: The docking interface image is acquired by a camera.

[0042] b. Feature extraction: The target feature points of the docking interface are extracted by improving the SURF algorithm. The principle of the improved SURF algorithm is as follows:

[0043] In the docking stage, the binocular camera acquires the docking interface image. To quickly locate the edge of the docking interface, this embodiment selects the algorithm framework in the public paper "SURF: Speeded-Up Robust Features" (ECCV 2006), and uses the threshold adjustment and direction consistency screening commonly used in underwater images to extract the target feature points of the docking interface. The coordinates of the obtained target feature points of the docking interface are subjected to simple geometric calculation to obtain the horizontal offset, angle error, and forward distance error of the docking interface in the image. The three error values are input into the fuzzy PID control algorithm, which drives the thruster to output thrust and synchronously adjusts the rudder angle to achieve docking.

[0044] c. Feedback output: According to the extracted target feature points, the input is sent to the submersible control system for real-time adjustment of the heading and attitude.

[0045] (2) The controller uses intelligent control algorithms such as fuzzy PID to fine-tune the thrusters and rudders. According to the visual feedback, the heading and attitude are adjusted in real time to make the carrier align with the docking interface in the correct direction and approach smoothly. Once the docking interface is aligned, the carrier automatically performs the end docking action.

[0046] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. Any modifications, equivalent replacements, and improvements made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. An automatic navigation control method for a deep-sea unmanned vehicle, characterized in that, Includes the following steps: S1: Utilize the Ultra-Short Baseline Positioning System (USBL) to monitor the real-time distance D between the launch vehicle and the deep-sea space station, and obtain the real-time position of the launch vehicle. ; S2: When the distance Then, in the coarse positioning stage, the inertial navigation system (INS) is used to obtain the current position of the launch vehicle. And through the real-time location of the carrier Correct the current position of the launch vehicle The corrected current position of the launch vehicle is compared with the position of the deep-sea space station to obtain the deviation between the target heading angle and the current actual heading angle. Dynamic elimination of heading angle PID The deviation will be controlled within ±5m of the distance error between the launch vehicle and the space station; S3: When The system switches to the fine positioning phase, activates the Doppler log (DVL) to acquire the real-time velocity of the launch vehicle, and uses a Kalman filter algorithm to combine the real-time velocity of the launch vehicle with the real-time position of the launch vehicle obtained by the ultra-short baseline positioning system (USBL). Perform fusion correction; S4: When the distance During the docking phase, the vehicle uses binocular vision features to adjust its course and attitude in real time, and achieves high-precision docking with the deep-sea space station through a fuzzy PID control algorithm.

2. The automatic navigation control method for a deep-sea unmanned vehicle according to claim 1, characterized in that, Step S1 is detailed as follows: S101. Transmit sound waves to the deep-sea space station transponder using the Ultra-Short Baseline Positioning System (USBL) and record the round-trip time of the sound waves. It also calculates the real-time distance between the launch vehicle and the deep-sea space station. ; S102. Calculate the azimuth angle of the launch vehicle relative to the space station based on the receiving time difference of different transducers in the Ultra-Short Baseline Positioning System (USBL). and pitch angle A coordinate system established with the space station's location as the reference origin, using the same coordinate transformation formula, is used to create spherical coordinates. Convert to 3D Cartesian coordinates: To obtain the real-time location of the launch vehicle .

3. The automatic navigation control method for a deep-sea unmanned vehicle according to claim 1, characterized in that, Step S2 is detailed as follows: S201. Integrate the linear acceleration obtained by the accelerometer and the rotational angular velocity obtained by the gyroscope using the inertial navigation system to obtain the current position of the carrier. S202, via the real-time location of the carrier Correct the current position of the launch vehicle This yields the corrected current position of the launch vehicle. S203. Compare the corrected current position of the launch vehicle with the known position of the deep-sea space station to obtain the deviation between the target heading angle and the current actual heading angle. Dynamic elimination of heading angle PID Deviation detection: When it is detected that the carrier is deviating from the target direction, the direction of travel is corrected; ultimately, the distance error is controlled within 5 meters.

4. The automatic navigation control method for a deep-sea unmanned vehicle according to claim 3, characterized in that, Step S201 is as follows: a. Utilize a gyroscope to collect the angular velocity of the carrier's rotation in the carrier coordinate system in real time, and based on the rotation angular velocity... Calculate attitude angle : ; b. Construct an attitude transformation matrix based on attitude angles. Using the linear acceleration collected in real-time from the triaxial accelerometer in the carrier coordinate system, convert the linear acceleration into linear acceleration using the attitude transformation matrix. Projecting this onto a geographic coordinate system yields the linear acceleration of the vehicle in that coordinate system. linear acceleration in geographic coordinate system Integrate to obtain the real-time velocity in the geographic coordinate system. : ; c. Regarding speed Integrate to obtain the current position of the launch vehicle in the inertial coordinate system. : .

5. The automatic navigation control method for a deep-sea unmanned vehicle according to claim 3, characterized in that, Step S202 is as follows: a. Construct a position correction model and calculate the current position. With real-time location deviation The formula is: ,in It reflects the difference between the current position and the real-time position of the inertial navigation system (INS), and includes the cumulative error information of inertial navigation; b. and Assign weight coefficients and The corrected position is obtained through weighted fusion. : 。 6. The automatic navigation control method for a deep-sea unmanned vehicle according to claim 1, characterized in that, Step S3 is as follows: S301. The vehicle speed is calculated using the Doppler frequency shift formula by transmitting sound waves underwater via a Doppler log (DVL) and receiving the echoes. in, The speed of sound in water, The frequency difference between the transmitted and received sound waves, To transmit sound wave frequencies, The angle between the sound beam and the direction of the vehicle's motion; based on the vehicle's speed. Output the three-dimensional velocity vector in the carrier coordinate system , the three-dimensional velocity vector Transform to a geographic coordinate system to obtain the velocity in the geographic coordinate system. ,in This is the attitude transformation matrix; S302, Obtain the real-time position of the vehicle from the Ultra-Short Baseline Positioning System (USBL) Convert to a geographic coordinate system to obtain the location in the geographic coordinate system. , Latitude Longitude Elevation; location in geographic coordinate system and the location of the deep-sea space station Obtain the true position information of inertia ;in, and Latitude and Longitude and For height; S303, based on the inertial navigation real geographic system location and speed Construct the measurement equation: In the formula, For the system The measured value at time, For the system measurement matrix, For system state variables, For position measurement noise vector; To obtain latitude for the inertial navigation system, The longitude obtained by the inertial navigation system. The altitude obtained by the inertial navigation system. The speed obtained by the inertial navigation system. The north velocity obtained by the inertial navigation system. The speed obtained by the inertial navigation system The speed obtained by the Doppler log (DVL) The north velocity obtained by the Doppler log (DVL) The north velocity obtained by the Doppler log (DVL); The state estimate at time k is then calculated using the measurement equation of Kalman filtering. This state estimate serves as the optimal state parameter of the vehicle at the current moment and is directly used in the real-time navigation system to correct the positioning drift caused by the measurement error of a single sensor.

7. The automatic navigation control method for a deep-sea unmanned vehicle according to claim 1, characterized in that, Step S4 is as follows: S401. Adjusting heading and attitude using binocular vision recognition: First, the camera acquires images of the interface, and the improved SURF algorithm is used to extract target feature points of the interface. The target feature points are then input into the PID control algorithm to adjust the heading and attitude. S402: By using a PID control algorithm to fine-tune the thrusters and servos, the launch vehicle can achieve precise docking with the deep sea.

8. An automatic navigation system for a deep-sea unmanned vehicle, characterized in that, include: The inertial navigation system (INS) consists of an inertial measurement unit composed of a three-axis rate gyroscope, a three-axis accelerometer, and a microcomputer. It is used to acquire angular velocity and linear acceleration in the carrier coordinate system in real time, and output position, velocity, and heading data in the geographic coordinate system through coordinate transformation. The Ultra-Short Baseline Positioning System (USBL) communicates with the surface platform via acoustic signals to obtain the distance between the launch vehicle and the deep-sea space station and the real-time position of the launch vehicle. The Doppler log (DVL) is used to measure the real-time velocity of the vehicle and is fused with data from the ultra-short baseline positioning system (USBL) to correct for accumulated errors. A camera, mounted at the front or bottom of the vehicle, is used for image acquisition during close-range target docking. The controller integrates a PID control algorithm to receive multi-source data from the inertial navigation system (INS), Doppler log (DVL), ultra-short baseline positioning system (USBL), and camera, and to generate control commands for the thrusters and servos. The combination of thrusters and servo motors enables the vehicle to output power and adjust its heading according to control commands.

Images