Autonomous underwater integrated navigation integrated real-time navigation system

Abstract

The invention relates to the technical field of underwater navigation, in particular to an autonomous underwater integrated navigation integrated real-time navigation system which comprises a geomagnetic and acoustic data acquisition module, a course dynamic correction module, a fluid resistance compensation module and an autonomous navigation decision module. Wherein the geomagnetic and acoustic data acquisition module is used for synchronously acquiring geomagnetic gradient vector data and acoustic array propagation delay data of an underwater carrier; the course dynamic correction module is used for generating course correction factors and outputting a motion parameter set; the fluid resistance compensation module is used for performing eddy current effect compensation on the course angle and generating anti-interference three-dimensional trajectory data; and the autonomous navigation decision module is used for dynamically adjusting propeller control parameters and generating a real-time navigation instruction packet. According to the invention, high-precision, autonomous and anti-interference navigation of the underwater carrier in a complex environment is realized through cooperation of double-source data fusion, course dynamic correction and propeller adaptive control.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater navigation, and particularly to an autonomous underwater integrated navigation real-time navigation system. Background Art

[0002] In tasks such as deep-sea resource exploration, underwater search and rescue, and marine ecological monitoring, underwater vehicles often need to independently complete long-term and autonomous navigation tasks in areas without external positioning signal coverage; to achieve precise navigation, existing technologies mostly rely on a single sensor data source, such as geomagnetic or acoustic signals, for path estimation. However, in complex underwater environments, the geomagnetism is easily interfered by local magnetic anomalies, and the acoustic channel is significantly affected by the water layer structure and noise. The single data source has problems of poor stability and uncontrollable errors.

[0003] In addition, due to the large influence of non-linear factors such as fluid resistance and vortex perturbation on the underwater propulsion system, the attitude change of the vehicle often causes heading drift, resulting in a significant increase in the risk of navigation path deviation and obstacle avoidance failure; therefore, there is an urgent need to construct an autonomous underwater integrated navigation real-time navigation system to solve the above problems. Summary of the Invention

[0004] Based on the above purpose, the present invention provides an autonomous underwater integrated navigation real-time navigation system.

[0005] An autonomous underwater integrated navigation real-time navigation system includes a geomagnetic and acoustic data acquisition module, a heading dynamic correction module, a fluid resistance compensation module, and an autonomous navigation decision-making module; wherein: The geomagnetic and acoustic data acquisition module: is used to synchronously acquire the geomagnetic gradient vector data and the acoustic array propagation delay data of the underwater vehicle, and output a dual-source verification data packet with a time stamp; The heading dynamic correction module: is used to receive the dual-source verification data packet, generate a heading correction factor based on the axial component of the geomagnetic gradient vector and the correlation of the acoustic propagation delay, and output a set of motion parameters including the corrected heading angle; The fluid resistance compensation module: based on the vehicle attitude data in the set of motion parameters, combines the real-time water depth pressure value to calculate the equivalent fluid resistance coefficient, so as to compensate for the vortex effect on the heading angle, thereby generating anti-interference three-dimensional trajectory data; The autonomous navigation decision-making module: is used to dynamically adjust the thruster control parameters according to the deviation value between the anti-interference three-dimensional trajectory data and the preset path, and generate a real-time navigation instruction packet including obstacle avoidance points.

[0006] Optionally, the geomagnetic and acoustic data acquisition module includes a geomagnetic sensing unit, an acoustic receiving unit, a synchronization control unit, and a data fusion output unit; wherein: Geomagnetic sensing unit: Fixed inside the underwater vehicle, it uses three sets of three-axis vector magnetometers, which are respectively installed at the front, middle and tail of the underwater vehicle. The three magnetometers are arranged at intervals in space, and the interval is not less than 0.5 meters. The sampling frequency of each magnetometer is 100Hz, which is used to obtain real-time geomagnetic gradient vector data covering the entire ship's hull; Acoustic receiving unit: Arranged on the surface of the underwater vehicle's outer shell, it uses six hydrophones evenly arranged in a ring on the equatorial plane of the vehicle's outer shell. The included angle between the hydrophones is 60°, the array radius is 0.4 meters, and the measurement bandwidth is 1kHz–25kHz, which is used to collect the propagation delay data of the reference sound source signal at each point in the array; Synchronization control unit: Connects the geomagnetic sensing unit and the acoustic receiving unit, and is built-in with a synchronization clock chip with a time accuracy of 0.1ms, which is used to control the unified start trigger of geomagnetic and acoustic signal acquisition, and respectively mark the same time stamp for each frame of geomagnetic vector and acoustic propagation delay data to ensure the consistency of the sampling time of the dual-source acquisition data; Data fusion output unit: Used to receive the time-stamped geomagnetic gradient vector data and acoustic propagation delay data output by the synchronization control unit, encapsulate the two types of data according to the unified data frame structure, and form a dual-source verification data packet with a unified time stamp.

[0007] Optionally, the heading dynamic correction module includes a correlation analysis unit, a heading factor calculation unit and a correction parameter output unit; among them: Correlation analysis unit: Used to receive the time-stamped dual-source verification data packet output by the geomagnetic and acoustic data acquisition module, extract the geomagnetic gradient vector component in the longitudinal axis direction of the vehicle and the acoustic array propagation delay data in the corresponding direction, and calculate the correlation coefficient between the two based on the sliding time window as the association measure of the current heading state; Heading factor calculation unit: Calculate the heading correction factor according to the correlation coefficient output by the correlation analysis unit and in combination with the current navigation attitude state; Correction parameter output unit: Based on the heading correction factor, real-time correct the original heading angle data, and output a set of motion parameters including the corrected heading angle, attitude angle, navigation speed and time stamp.

[0008] Optionally, the correlation analysis unit includes: Geomagnetic component extraction sub-unit: Receive the time-stamped dual-source verification data packet, extract the magnetic field component in the longitudinal axis direction of the vehicle from the three-axis geomagnetic gradient vector data, and define it as the longitudinal axis magnetic field intensity sequence; Acoustic delay extraction sub-unit: Determine two acoustically receiving sensors arranged opposite to each other in the longitudinal axis direction according to the vehicle's navigation direction, receive their propagation delay data and calculate the difference between the two sensors to form an acoustic propagation delay difference sequence in the longitudinal axis direction; Correlation coefficient calculation sub-unit: It is used to gradually scan the extracted longitudinal axis magnetic field intensity sequence and acoustic propagation time delay difference sequence with a sliding time window of a fixed duration, and calculate the correlation coefficient R of the two sequences in each time window respectively.

[0009] Optionally, the heading factor calculation unit includes: Correlation threshold judgment sub-unit: Receives the correlation coefficient R output by the correlation analysis unit, and compares it with the preset correlation threshold If R is lower than the threshold, it is determined that there is an error in the current heading angle; Heading deviation quantization sub-unit: When it is determined that there is an error in the current heading angle, according to the difference between the current correlation coefficient value and the threshold, calculate the specific heading deviation angle ; Heading correction factor generation sub-unit: According to the calculated heading deviation angle , combined with the current original measured heading angle , generate the final heading correction factor , the specific calculation formula is: .

[0010] Optionally, the correction parameter output unit includes: Heading data fusion sub-unit: Receives the heading correction factor output by the heading factor calculation unit, and fuses it with the original heading angle data in real time to obtain the corrected real-time heading angle data. The formula is: , where: is the real-time heading angle after fusion and correction; is the fusion weight factor; Attitude parameter update sub-unit: Based on the real-time heading angle after fusion and correction , combined with the currently obtained pitch angle, roll angle and carrier speed, update the current real-time attitude data of the carrier to form a complete motion data set.

[0011] Optionally, the fluid resistance compensation module includes a fluid resistance coefficient calculation unit, an eddy current effect compensation unit and a trajectory data generation unit; among them: Fluid resistance coefficient calculation unit: It is used to receive the motion parameter set output by the heading dynamic correction module, extract the real-time heading angle, pitch angle, roll angle and motion speed from it, and at the same time receive the water depth pressure value through a preset water depth pressure sensor, and then calculate the equivalent fluid resistance coefficient according to the hydrodynamic model ; Eddy current effect compensation unit: Based on the equivalent fluid resistance coefficient, combined with the real-time heading angle data, calculate the eddy current effect compensation correction amount of the heading angle, which is used to compensate for the deviation of the heading angle data caused by the eddy current interference around the carrier shell, and output the compensated heading angle; Trajectory data generation unit: It is used to receive the compensated heading angle output by the eddy current effect compensation unit, and combine the real-time pitch angle, roll angle, speed and water depth values of the carrier to integrate and form an anti-interference real-time three-dimensional trajectory data set.

[0012] Optionally, the eddy current effect compensation unit includes: Eddy current influence quantization sub-unit: Based on the equivalent fluid resistance coefficient , combined with the real-time navigation speed, calculate the quantization value of the influence of the eddy current on the heading angle. The formula is: , where: is the quantization value of the influence of the eddy current on the heading angle; is the eddy current effect influence constant, which is determined by the hydrodynamic calibration experiment; V is the motion speed; is the equivalent fluid resistance coefficient; Heading compensation amount calculation sub-unit: It is used to calculate the heading angle eddy current effect compensation correction amount according to the calculated quantization value of the influence of the eddy current on the heading angle , in combination with the sign relationship determined by the eddy current interference direction. Its expression is: ; Heading angle compensation correction sub-unit: It is used to superimpose the eddy current effect compensation correction amount with the fused and corrected real-time heading angle to obtain the real-time corrected heading angle after eddy current effect compensation.

[0013] Optionally, the autonomous navigation decision module includes a trajectory deviation analysis unit, a thruster parameter adjustment unit and a navigation instruction generation unit; among them: Trajectory deviation analysis unit: It is used to receive the anti-interference three-dimensional trajectory data set output by the fluid resistance compensation module, combine the pre-set target path trajectory, and perform one-to-one comparison according to the system timestamp to calculate the deviation values in the three-dimensional space, including heading angle deviation, position deviation and speed deviation; Thruster parameter adjustment unit: It is used to dynamically adjust the thrust magnitude, direction angle and multi-thruster distribution ratio of the thruster according to the deviation values output by the trajectory deviation analysis unit to continuously correct the navigation direction and speed; Navigation instruction generation unit: Based on the thruster adjustment result and the obstacle avoidance point position information in the three-dimensional space, generate a structured real-time navigation instruction packet, specifically including the thruster thrust instruction, direction instruction, speed instruction at the current moment, as well as the spatial coordinates and priority ranking of the obstacles to be avoided.

[0014] Optionally, the thruster parameter adjustment unit includes: Heading PID adjustment sub-unit: used to receive the heading angle deviation output by the trajectory deviation analysis unit, denoted as , and uses the PID control algorithm to calculate the heading correction amount in real time ; Speed PID adjustment sub-unit: used to receive the position deviation and speed deviation in the speed direction, denoted as and respectively, and also uses the PID control algorithm to calculate the thruster thrust correction amount in real time ; Allocation ratio optimization sub-unit: used to dynamically optimize the thrust allocation ratio and direction angle of each thruster according to the heading correction control amount and the speed correction control amount , so that the total synthetic thrust and synthetic torque meet the expected navigation direction and speed correction requirements; the constraints followed by the thrust allocation ratio optimization include: ; ; where is the thrust allocated to the i-th thruster; is the current direction angle of the i-th thruster; is the perpendicular distance from the i-th thruster to the center of gravity; is the number of thrusters.

[0015] Advantages of the present invention: In the present invention, by constructing a navigation perception system that fuses geomagnetism and acoustics, and using a time synchronization mechanism and a correlation analysis model, high-precision dynamic correction of the heading angle error is achieved, overcoming the problem of poor stability of a single sensor in a complex underwater environment; at the same time, combined with a fluid resistance and eddy current compensation mechanism, real-time compensation of the carrier attitude and external interference is performed, effectively improving the accuracy of heading estimation.

[0016] In the present invention, through the adaptive adjustment of the thruster parameters by the PID control algorithm, combined with the optimization of the thrust distribution of multiple thrusters, dynamic linkage control of the magnitude and direction of the thruster thrust is achieved under the drive of the trajectory deviation, improving the path adaptability and navigation robustness of the underwater vehicle in a changing sea area, and meeting the requirements of high-precision, autonomous, and anti-interference underwater navigation applications. Description of the drawings

[0017] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings in the following description are only those of the present invention. For those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative efforts.

[0018] Figure 1 Schematic diagram of the navigation system according to an embodiment of the present invention; Figure 2 Schematic diagram of the heading dynamic correction module according to an embodiment of the present invention. Detailed implementation manners

[0019] The present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments. At the same time, it should be noted here that in order to make the embodiments more detailed, the following embodiments are the best and preferred embodiments. For some well-known technologies, those skilled in the art can also adopt other alternative methods for implementation; moreover, the accompanying drawings are only for more specific description of the embodiments, and are not intended to specifically limit the present invention.

[0020] As Figure 1 - Figure 2 shown, an autonomous underwater integrated real-time navigation system includes a geomagnetic and acoustic data acquisition module, a heading dynamic correction module, a fluid resistance compensation module, and an autonomous navigation decision-making module; wherein: Geomagnetic and acoustic data acquisition module: used to synchronously acquire the geomagnetic gradient vector data and the acoustic array propagation delay data of the underwater vehicle, and output a dual-source verification data packet with a time stamp; Heading dynamic correction module: used to receive the dual-source verification data packet, generate a heading correction factor according to the correlation between the axial component of the geomagnetic gradient vector and the acoustic propagation delay, and output a set of motion parameters including the corrected heading angle; Fluid resistance compensation module: based on the carrier attitude data in the set of motion parameters, calculate the equivalent fluid resistance coefficient in combination with the real-time water depth pressure value, so as to compensate the eddy current effect on the heading angle, and thus generate anti-interference three-dimensional trajectory data; Autonomous navigation decision-making module: used to dynamically adjust the thruster control parameters according to the deviation value between the anti-interference three-dimensional trajectory data and the preset path, and generate a real-time navigation instruction packet including obstacle avoidance points.

[0021] The geomagnetic and acoustic data acquisition module includes a geomagnetic sensing unit, an acoustic receiving unit, a synchronous control unit, and a data fusion output unit; wherein: Geomagnetic sensing unit: fixedly arranged inside the underwater vehicle, using three groups of three-axis vector magnetometers, which are respectively installed at the front, middle, and tail of the underwater vehicle. The three magnetometers are spaced apart in space, and the spacing is not less than 0.5 meters. The sampling frequency of each magnetometer is 100Hz, and is used to obtain the geomagnetic gradient vector data covering the entire ship's hull in real time; Acoustic receiving unit: arranged on the surface of the underwater vehicle's shell, using six hydrophones evenly arranged in a ring on the equatorial plane of the carrier's shell. The included angle between the hydrophones is 60°, the array radius is 0.4 meters, and the measurement bandwidth is 1kHz–25kHz, and is used to collect the propagation delay data of the reference sound source signal at each point in the array; Synchronization control unit: Connects the geomagnetic sensing unit and the acoustic receiving unit, and is built-in with a synchronization clock chip with a time accuracy of 0.1 ms. It is used to control the unified start trigger of geomagnetic and acoustic signal acquisition, and respectively label the same timestamp for each frame of geomagnetic vector and acoustic propagation delay data to ensure the consistency of the sampling time of the dual-source acquisition data; Data fusion output unit: It is used to receive the geomagnetic gradient vector data and acoustic propagation delay data with timestamps output by the synchronization control unit, encapsulate the two types of data according to the unified data frame structure, and form a dual-source verification data packet with a unified timestamp; The above units adopt three groups of three-axis magnetometers arranged at spatial intervals in the geomagnetic sensing unit, and configure an equally spaced circular acoustic array in the acoustic receiving unit, so that the system can comprehensively cover the circumferential magnetic field change and acoustic wave propagation delay of the underwater vehicle; Combined with high-precision synchronization clock control, it significantly improves the accuracy of the spatio-temporal correspondence of the dual-source data and provides a highly reliable input for heading correction and anti-interference navigation.

[0022] The heading dynamic correction module includes a correlation analysis unit, a heading factor calculation unit, and a correction parameter output unit; among them: Correlation analysis unit: It is used to receive the dual-source verification data packet with timestamp output by the geomagnetic and acoustic data acquisition module, extract the geomagnetic gradient vector component in the longitudinal axis direction of the vehicle body and the acoustic array propagation delay data in the corresponding direction, and calculate the correlation coefficient between the two based on the sliding time window as the correlation measure of the current heading state; Heading factor calculation unit: According to the correlation coefficient output by the correlation analysis unit, combined with the current navigation attitude state, calculate the heading correction factor, which is used to dynamically adjust the original heading angle error and compensate for the influence of external magnetic interference or abnormal acoustic propagation on heading recognition; Correction parameter output unit: Based on the heading correction factor, it real-time corrects the original heading angle data, outputs a set of motion parameters including the corrected heading angle, attitude angles (pitch angle and roll angle), navigation speed, and timestamp, and sends this set of parameters to the fluid resistance compensation module; The above unit can effectively correct the heading error caused by geomagnetic interference or abnormal underwater acoustic channel by using the time-synchronized geomagnetic gradient vector and acoustic propagation delay data for correlation analysis and constructing a dynamic heading correction factor, and improve the heading recognition accuracy of the underwater vehicle in complex waters.

[0023] The correlation analysis unit includes: Geomagnetic component extraction sub-unit: Receives the dual-source verification data packet with timestamp, extracts the magnetic field component in the longitudinal axis direction of the vehicle body from the three-axis geomagnetic gradient vector data, and defines it as the longitudinal axis magnetic field intensity sequence; Acoustic time delay extraction subunit: Determine two acoustical receiving sensors arranged opposite to each other in the longitudinal axis direction according to the carrier navigation direction, receive the propagation time delay data thereof, calculate the difference between the two sensors, and form an acoustical propagation time delay difference sequence in the longitudinal axis direction; Correlation coefficient calculation subunit: Used to gradually scan the extracted longitudinal axis magnetic field intensity sequence and acoustical propagation time delay difference sequence with a sliding time window of a fixed duration, and calculate the correlation coefficient R of the two sequences in each time window respectively. The specific formula is: , where R is the correlation coefficient between the longitudinal axis magnetic field intensity sequence and the acoustical propagation time delay difference sequence; is the magnetic field intensity of the geomagnetic gradient vector in the longitudinal axis direction of the carrier at the i-th sampling point; is the acoustic wave propagation time delay difference between the acoustic sensors in the longitudinal axis direction at the i-th sampling point; and are the means of the corresponding sequences within the time window respectively; N is the number of sampling points within the sliding time window; Through the above-mentioned clear sequence extraction and correlation coefficient calculation steps, the dynamic correlation between the geomagnetic data and the acoustic data in the longitudinal axis direction is accurately captured, the characteristics of the heading error are effectively quantified, and stable data support is provided for the calculation of the heading correction factor.

[0024] The heading factor calculation unit includes: Correlation threshold judgment subunit: Receive the correlation coefficient R output by the correlation analysis unit, compare it with the preset correlation threshold , if R is lower than the threshold, it is determined that there is an error in the current heading angle and subsequent correction is required; Heading deviation quantization subunit: When it is determined that there is an error in the current heading angle, calculate the specific heading deviation angle according to the difference between the current correlation coefficient value and the threshold. The calculation method is as follows: , where is the heading angle deviation value, and K is the heading deviation proportionality factor, a constant obtained through experimental calibration; Heading correction factor generation subunit: According to the calculated heading deviation angle , combined with the current original measured heading angle , generate the final heading correction factor , and the specific calculation formula is: ; The above-mentioned subunits accurately calculate the heading correction factor, quantify and correct the heading deviation in real time and effectively, improve the accuracy of the heading angle measurement, and ensure that the underwater vehicle navigation system has higher navigation accuracy and anti-interference ability in the underwater environment.

[0025] The correction parameter output unit includes: Heading data fusion sub-unit: Receives the heading correction factor output by the heading factor calculation unit, and fuses it with the original heading angle data in real time to obtain the corrected real-time heading angle data. The formula is: , where: is the real-time heading angle after fusion and correction; is the fusion weight factor, and its value range is [0, 1]; Attitude parameter update sub-unit: Based on the real-time heading angle after fusion and correction, combined with the current pitch angle, roll angle and carrier speed obtained synchronously, updates the current real-time attitude data of the carrier to form a complete motion data set.

[0026] The fluid resistance compensation module includes a fluid resistance coefficient calculation unit, an eddy current effect compensation unit and a trajectory data generation unit; among them: Fluid resistance coefficient calculation unit: Used to receive the motion parameter set output from the heading dynamic correction module, extract the real-time heading angle, pitch angle, roll angle and motion speed from it, and at the same time receive the water depth pressure value through a preset water depth pressure sensor, and then calculate the equivalent fluid resistance coefficient according to the hydrodynamic model , and the specific calculation formula is: , where, is the calculated equivalent fluid resistance coefficient; is the system base resistance coefficient, obtained by experimental calibration; is the pressure influence coefficient; p is the real-time water depth pressure value; is the velocity square term coefficient; V is the motion speed; is the pitch angle influence coefficient; is the real-time pitch angle; is the roll angle influence coefficient; is the real-time roll angle; Eddy current effect compensation unit: Based on the equivalent fluid resistance coefficient, combines the real-time heading angle data to calculate the eddy current effect compensation correction amount of the heading angle, which is used to compensate for the deviation of the heading angle data caused by the eddy current interference around the carrier shell, and outputs the compensated heading angle; Trajectory data generation unit: Used to receive the compensated heading angle output by the eddy current effect compensation unit, combines the real-time pitch angle, roll angle, speed and water depth value of the carrier, integrates to form an anti-interference real-time three-dimensional trajectory data set, and outputs it to the autonomous navigation decision-making module for use; through the combination of the above units, the real-time and accurate compensation of the eddy current interference during the underwater vehicle's navigation process is realized, effectively improving the stability of the underwater navigation and the accuracy of the trajectory tracking, and providing a reliable navigation basis for the underwater vehicle in a complex underwater environment.

[0027] The eddy current effect compensation unit includes: Eddy current influence quantification subunit: Based on the equivalent fluid resistance coefficient , combined with the real-time navigation speed, calculate the quantification value of the influence of the eddy current on the course angle. The formula is: , where: is the quantification value of the influence of the eddy current on the course angle; is the eddy current effect influence constant, determined by the hydrodynamic calibration experiment; V is the motion speed; is the equivalent fluid resistance coefficient; Course compensation amount calculation subunit: Used to determine the symbol relationship according to the calculated quantification value of the influence of the eddy current on the course angle , combined with the eddy current interference direction, calculate the course angle eddy current effect compensation correction amount , and its expression is: ; Course angle compensation correction subunit: Used to superimpose the eddy current effect compensation correction amount with the fused and corrected real-time course angle to obtain the real-time corrected course angle after eddy current effect compensation , and the specific calculation formula is as follows: ; Through the above-mentioned subunits, the course deviation caused by the eddy current effect of the underwater vehicle's shell can be corrected in real time and quantitatively, thus effectively enhancing the accuracy and reliability of the underwater navigation course angle.

[0028] The autonomous navigation decision-making module includes a trajectory deviation analysis unit, a thruster parameter adjustment unit, and a navigation instruction generation unit; among them: Trajectory deviation analysis unit: Used to receive the anti-interference three-dimensional trajectory data set output by the fluid resistance compensation module, combine it with the pre-set target path trajectory, perform one-to-one comparison according to the system timestamp, calculate the deviation values in the three-dimensional space, including course angle deviation, position deviation, and speed deviation, and update the deviation sequence in real time; Thruster parameter adjustment unit: Used to dynamically adjust the thrust magnitude, direction angle, and multi-thruster distribution ratio of the thruster according to the deviation values output by the trajectory deviation analysis unit by using the PID control algorithm (proportional-integral-derivative control) to continuously correct the navigation direction and speed to ensure that the vehicle gradually approaches the preset path; Navigation instruction generation unit: Based on the thruster adjustment results and the position information of obstacle avoidance points in three-dimensional space, it generates a structured real-time navigation instruction package, which specifically includes the thruster thrust instruction, direction instruction, speed instruction at the current moment, as well as the spatial coordinates and priority ranking of the obstacles to be avoided. This navigation instruction package is sent to the thruster execution layer in real time through the system data bus to ensure the synchronization and safety of navigation actions; through the above-mentioned units, the autonomous navigation decision-making module can achieve dynamic linkage of the underwater vehicle's spatial trajectory and propulsion control, enabling the vehicle to automatically correct its motion state according to the real-time trajectory deviation and obstacle information in a complex underwater environment, and finally generate an efficient navigation instruction package containing obstacle avoidance points, effectively improving the autonomous obstacle avoidance and path tracking capabilities of the underwater vehicle.

[0029] The thruster parameter adjustment unit includes: Heading PID adjustment sub-unit: It is used to receive the heading angle deviation output by the trajectory deviation analysis unit, denoted as , and uses the PID control algorithm to calculate the heading correction amount in real time , and the formula is: , where is the heading correction control amount; is the current heading angle deviation; , , are the proportional, integral, and differential coefficients of the heading PID control respectively; Speed PID adjustment sub-unit: It is used to receive the position deviation and speed deviation in the speed direction, denoted as and respectively, and also uses the PID control algorithm to calculate the thruster thrust correction amount in real time , and the formula is: , where is the speed correction control amount; is the current speed deviation; , , are the proportional, integral, and differential coefficients of the speed PID control respectively; Allocation ratio optimization sub-unit: It is used to dynamically optimize the thrust allocation ratio and direction angle of each thruster according to the heading correction control amount and the speed correction control amount , so that the total combined thrust and combined torque meet the expected navigation direction and speed correction requirements; the constraints followed by the thrust allocation ratio optimization include: ; ; where is the thrust allocated to the i-th thruster; is the current direction angle of the i-th thruster; is the perpendicular distance from the i-th thruster to the center of gravity; is the number of thrusters; Through the above PID dynamic adjustment and distribution optimization steps, the thruster parameter adjustment unit can achieve continuous and precise correction of the navigation direction and speed, ensure the stable and fast response movement of the underwater vehicle along the predetermined path, and effectively improve the dynamic performance and anti-interference ability of the navigation control.

[0030] This invention covers any alternatives, modifications, equivalent methods and solutions made on the essence and scope of this invention. For the public to have a thorough understanding of this invention, specific details are described in detail in the following preferred embodiments of this invention. However, those skilled in the art can fully understand this invention without the description of these details. In addition, well-known methods, processes, procedures, components and circuits, etc. are not described in detail to avoid unnecessary confusion to the essence of this invention.

[0031] The above is only the preferred embodiment of this invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of this invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of this invention.

Claims

1. An autonomous underwater integrated real-time navigation system for combined navigation, characterized in that It includes a geomagnetic and acoustic data acquisition module, a heading dynamic correction module, a fluid resistance compensation module, and an autonomous navigation decision-making module; among which: The geomagnetic and acoustic data acquisition module: is used to synchronously acquire the geomagnetic gradient vector data and the acoustic array propagation delay data of the underwater vehicle, and output a dual-source verification data packet with a timestamp; The heading dynamic correction module: is used to receive the dual-source verification data packet, generate a heading correction factor based on the correlation between the axial component of the geomagnetic gradient vector and the acoustic propagation delay, and output a set of motion parameters including the corrected heading angle; The fluid resistance compensation module: based on the carrier attitude data in the set of motion parameters, combines the real-time water depth pressure value to calculate the equivalent fluid resistance coefficient, compensates the heading angle for the eddy current effect, and thus generates anti-interference three-dimensional trajectory data; The autonomous navigation decision-making module: is used to dynamically adjust the thruster control parameters according to the deviation value between the anti-interference three-dimensional trajectory data and the preset path, and generate a real-time navigation instruction packet including obstacle avoidance points.

2. An autonomous underwater integrated real-time navigation system according to claim 1, characterized in that The geomagnetic and acoustic data acquisition module includes a geomagnetic sensing unit, an acoustic receiving unit, a synchronous control unit, and a data fusion output unit; among which: The geomagnetic sensing unit: is fixedly arranged inside the underwater vehicle, uses three groups of three-axis vector magnetometers, which are respectively installed at the front, middle, and tail of the underwater vehicle. The three magnetometers are spaced apart in space, and the spacing is not less than 0.5 meters. The sampling frequency of each magnetometer is 100Hz, and is used to obtain the geomagnetic gradient vector data covering the entire hull in real time; The acoustic receiving unit: is arranged on the surface of the underwater vehicle's outer shell, uses six hydrophones to be evenly arranged in a ring on the equatorial plane of the carrier's outer shell. The included angle between the hydrophones is 60°, the array radius is 0.4 meters, and the measurement bandwidth is 1kHz–25kHz, and is used to collect the propagation delay data of the reference sound source signal at each point in the array; The synchronous control unit: connects the geomagnetic sensing unit and the acoustic receiving unit, and is built-in with a synchronous clock chip with a time accuracy of 0.1ms, and is used to control the unified start trigger of the geomagnetic and acoustic signal acquisitions, and respectively mark the same timestamp for each frame of geomagnetic vector and acoustic propagation delay data, ensuring the consistency of the dual-source acquisition data in the sampling time; The data fusion output unit: is used to receive the geomagnetic gradient vector data and the acoustic propagation delay data with timestamps output by the synchronous control unit, encapsulate the two types of data according to the unified data frame structure, and form a dual-source verification data packet with a unified timestamp.

3. An autonomous underwater integrated real-time navigation system for combined navigation according to claim 1, characterized in that, The heading dynamic correction module includes a correlation analysis unit, a heading factor calculation unit, and a correction parameter output unit; among which: The correlation analysis unit: is used to receive the dual-source verification data packet with a timestamp output by the geomagnetic and acoustic data acquisition module, extract the geomagnetic gradient vector component in the longitudinal axis direction of the carrier and the acoustic array propagation delay data in the corresponding direction, and calculate the correlation coefficient between the two based on a sliding time window as the correlation metric of the current heading state; The heading factor calculation unit: calculates the heading correction factor according to the correlation coefficient output by the correlation analysis unit and combines the current navigation attitude state; Calibration parameter output unit: Based on the heading correction factor, it corrects the original heading angle data in real time and outputs a set of motion parameters including the corrected heading angle, attitude angle, navigation speed, and timestamp.

4. An autonomous underwater integrated real-time navigation system according to claim 3, characterized in that, The correlation analysis unit includes: Geomagnetic component extraction sub-unit: Receives the dual-source verification data packet with timestamp, extracts the magnetic field component in the longitudinal axis direction of the carrier from the three-axis geomagnetic gradient vector data, and defines it as the longitudinal axis magnetic field intensity sequence; Acoustic time delay extraction sub-unit: Determines two acoustically receiving sensors arranged oppositely in the longitudinal axis direction according to the navigation direction of the carrier, receives their propagation time delay data and calculates the difference between the two sensors to form an acoustic propagation time delay difference sequence in the longitudinal axis direction; Correlation coefficient calculation sub-unit: Used to gradually scan the extracted longitudinal axis magnetic field intensity sequence and acoustic propagation time delay difference sequence with a sliding time window of fixed duration, and calculate the correlation coefficient R of the two sequences in each time window respectively.

5. An autonomous underwater integrated real-time navigation system for combined navigation according to claim 4, characterized in that, The heading factor calculation unit includes: Relevant threshold judgment subunit: Receive the correlation coefficient R output by the correlation analysis unit and compare it with the preset correlation threshold for comparison. If R is lower than the threshold, it is determined that there is an error in the current heading angle; Course deviation quantification subunit: When it is determined that there is an error in the current course angle, a specific course deviation angle is calculated according to the difference between the current correlation coefficient value and the threshold value ; Heading correction factor generation subunit: According to the calculated heading deviation angle , combined with the current original measured heading angle , generate the final heading correction factor . The specific calculation formula is as follows: .

6. An autonomous underwater integrated navigation real-time navigation system according to claim 5, characterized in that, The calibration parameter output unit includes: Heading data fusion subunit: receives the heading correction factor output by the heading factor calculation unit, and fuses it with the original heading angle data in real time to obtain the corrected real-time heading angle data. The formula is: , where: is the real-time heading angle after fusion correction; is the fusion weight factor; Attitude parameter update subunit: Based on the real-time heading angle after fusion correction , combined with the currently obtained pitch angle, roll angle and carrier speed synchronously, update the current real-time attitude data of the carrier to form a complete motion data set.

7. An autonomous underwater integrated real-time navigation system for combined navigation according to claim 1, characterized in that The fluid resistance compensation module includes a fluid resistance coefficient calculation unit, an eddy current effect compensation unit, and a trajectory data generation unit; among them: Fluid drag coefficient calculation unit: It is used to receive the set of motion parameters output from the heading dynamic correction module, extract the real-time heading angle, pitch angle, roll angle and motion speed from it, and at the same time receive the water depth pressure value through a preset water depth pressure sensor, and then calculate the equivalent fluid drag coefficient according to the hydrodynamic model ; Eddy current effect compensation unit: Based on the equivalent fluid resistance coefficient, combines the real-time heading angle data to calculate the eddy current effect compensation correction amount of the heading angle, which is used to compensate for the deviation of the heading angle data caused by the eddy current interference around the carrier shell, and outputs the compensated heading angle; Trajectory data generation unit: Used to receive the compensated heading angle output by the eddy current effect compensation unit, combines the real-time pitch angle, roll angle, speed, and water depth values of the carrier, and integrates them to form an anti-interference real-time three-dimensional trajectory data set.

8. An autonomous underwater integrated real-time navigation system according to claim 7, characterized in that The eddy current effect compensation unit includes: Vortex influence quantification subunit: Based on the equivalent fluid resistance coefficient , combined with the real-time navigation speed, calculate the quantification value of the influence of the vortex on the heading angle. The formula is: , where: is the quantification value of the influence of the vortex on the heading angle; is the influence constant of the vortex effect, determined by the hydrodynamic calibration experiment; V is the motion speed; is the equivalent fluid resistance coefficient; Course compensation amount calculation sub-unit: used to calculate the quantization value of the course angle affected by the eddy current , combined with the sign relationship determined by the eddy current interference direction, calculate the compensation correction amount of the eddy current effect on the course angle , and its expression is: ; Heading angle compensation and correction sub-unit: used to add the eddy current effect compensation and correction amount to the real-time heading angle after fusion and correction to obtain the real-time corrected heading angle after eddy current effect compensation .

9. An autonomous underwater integrated real-time navigation system for combined navigation according to claim 1, characterized in that, The autonomous navigation decision module includes a trajectory deviation analysis unit, a thruster parameter adjustment unit, and a navigation instruction generation unit; among them: Trajectory deviation analysis unit: Used to receive the anti-interference three-dimensional trajectory data set output by the fluid resistance compensation module, combines the preset target path trajectory, and performs one-to-one comparison according to the system timestamp to calculate the deviation values in the three-dimensional space, including heading angle deviation, position deviation, and speed deviation; Thruster parameter adjustment unit: Used to dynamically adjust the thrust magnitude, direction angle, and multi-thruster distribution ratio of the thruster according to the deviation values output by the trajectory deviation analysis unit by using the PID control algorithm to continuously correct the navigation direction and speed; Navigation instruction generation unit: Based on the thruster adjustment result and the obstacle avoidance point position information in the three-dimensional space, generates a structured real-time navigation instruction packet, specifically including the thruster thrust instruction, direction instruction, speed instruction at the current moment, as well as the spatial coordinates and priority ranking of the obstacles to be avoided.

10. An autonomous underwater integrated real-time navigation system according to claim 9, characterized in that, The thruster parameter adjustment unit includes: Heading PID adjustment sub-unit: used to receive the heading angle deviation output by the trajectory deviation analysis unit, denoted as , and use the PID control algorithm to calculate the heading correction amount in real time ; Speed PID adjustment sub-unit: used to receive the position deviation and speed deviation in the speed direction, denoted as and , and also use the PID control algorithm to calculate the thruster thrust correction amount in real time ; Allocation ratio optimization subunit: used to optimize the thrust allocation ratio and direction angle of each thruster dynamically according to the course correction control amount and the speed correction control amount , so that the total resultant thrust and resultant moment meet the desired navigation direction and speed correction requirements; the constraints followed by the thrust allocation ratio optimization include: ； ; where, is the thrust allocated to the i-th thruster; is the current direction angle of the i-th thruster; is the perpendicular distance from the i-th thruster to the center of gravity; is the number of thrusters.

Images