An autonomous underwater combined navigation integrated real-time navigation system

Abstract

The present invention relates to the field of underwater navigation technology, and more specifically to an autonomous underwater combined navigation integrated real-time navigation system, comprising a geomagnetic and acoustic data acquisition module, a heading dynamic correction module, a fluid resistance compensation module, and an autonomous navigation decision module. The system comprises: a geomagnetic and acoustic data acquisition module for synchronously acquiring geomagnetic gradient vector data and acoustic array propagation delay data of an underwater carrier; a heading dynamic correction module for generating a heading correction factor and outputting a motion parameter set; a fluid resistance compensation module for compensating for eddy current effects on the heading angle and generating interference-resistant three-dimensional trajectory data; and an autonomous navigation decision module for dynamically adjusting thruster control parameters and generating real-time navigation instruction packets. The present invention achieves high-precision, autonomous, and interference-resistant navigation of an underwater carrier in complex environments through the coordinated integration of dual-source data fusion, heading dynamic correction, and thruster adaptive control.

Description

Technical Field

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

[0002] In tasks such as deep-sea resource exploration, underwater search and rescue, and marine ecological monitoring, underwater carriers often need to independently complete long-term, 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, geomagnetism is easily affected by local magnetic anomalies, and acoustic channels are significantly affected by water layer structure and noise. A single data source has the problems of poor stability and uncontrollable errors.

[0003] In addition, since the underwater propulsion system is greatly affected by nonlinear factors such as fluid resistance and eddy disturbance, changes in the carrier's attitude often cause 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 build an autonomous underwater combined navigation integrated real-time navigation system to solve the above problems. Summary of the Invention

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

[0005] 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 module; wherein:

[0006] Geomagnetic and acoustic data acquisition module: used to synchronously collect geomagnetic gradient vector data and acoustic array propagation delay data of underwater carriers, and output dual-source verification data packets with time stamps;

[0007] Heading dynamic correction module: used to receive dual-source calibration data packets, 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 motion parameter set containing the corrected heading angle;

[0008] Fluid resistance compensation module: Based on the carrier posture data in the motion parameter set and combined with the real-time water depth pressure value, it calculates the equivalent fluid resistance coefficient to compensate for the eddy current effect on the heading angle, thereby generating anti-interference three-dimensional trajectory data;

[0009] Autonomous navigation decision 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 package containing obstacle avoidance points.

[0010] 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:

[0011] Geomagnetic sensing unit: Fixedly arranged inside the underwater carrier, it uses three sets of three-axis vector magnetometers, installed at the front, middle and rear of the underwater carrier respectively. The three sets of magnetometers are spaced apart in space with a spacing of no less than 0.5 meters. The sampling frequency of each magnetometer is 100Hz, which is used to obtain geomagnetic gradient vector data covering the entire ship in real time;

[0012] Acoustic receiving unit: Arranged on the surface of the underwater carrier shell, it uses six hydrophones evenly arranged in a circular pattern on the equatorial plane of the carrier shell. The angle between the hydrophones is 60 degrees, the array radius is 0.4 meters, and the measurement bandwidth is 1kHz-25kHz. It is used to collect the propagation delay data of the reference sound source signal at each point in the array;

[0013] Synchronous control unit: Connects the geomagnetic sensing unit and the acoustic receiving unit. It has a built-in synchronous clock chip with a time accuracy of 0.1ms. It is used to control the start trigger of geomagnetic and acoustic signal acquisition. It also marks the same timestamp on each frame of geomagnetic vector and acoustic propagation delay data to ensure the consistency of the sampling time of the dual-source acquisition data.

[0014] Data fusion output unit: 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 a unified data frame structure, and form a dual-source verification data packet with a unified timestamp.

[0015] Optionally, the heading dynamic correction module includes a correlation analysis unit, a heading factor calculation unit and a correction parameter output unit; wherein:

[0016] Correlation analysis unit: used to receive the dual-source verification data packets with time stamps output by the geomagnetic and acoustic data acquisition modules, extract the geomagnetic gradient vector component in the longitudinal 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 the sliding time window as the correlation measure of the current heading state;

[0017] Heading factor calculation unit: calculates the heading correction factor based on the correlation coefficient output by the correlation analysis unit and the current navigation attitude state;

[0018] Correction parameter output unit: Based on the heading correction factor, the original heading angle data is corrected in real time, and the output includes a motion parameter set including the corrected heading angle, attitude angle, navigation speed and timestamp.

[0019] Optionally, the correlation analysis unit includes:

[0020] Geomagnetic component extraction subunit: receives dual-source verification data packets with timestamps, extracts the magnetic field component in the longitudinal direction of the carrier from the three-axis geomagnetic gradient vector data, and defines it as the longitudinal magnetic field intensity sequence;

[0021] Acoustic delay extraction subunit: This subunit determines two acoustic receiving sensors located opposite each other in the longitudinal direction according to the carrier's navigation direction, receives their propagation delay data, and calculates the difference between the two sensors to form an acoustic propagation delay difference sequence in the longitudinal direction.

[0022] Correlation coefficient calculation subunit: used to scan the extracted longitudinal magnetic field intensity sequence and acoustic propagation delay difference sequence step by step with a sliding time window of fixed length, and calculate the correlation coefficient R of the two sequences in each time window respectively.

[0023] Optionally, the heading factor calculation unit includes:

[0024] Correlation threshold judgment subunit: receives the correlation coefficient R output by the correlation analysis unit and compares it with the preset correlation threshold Compare and if R is lower than the threshold, it is determined that there is an error in the current heading angle;

[0025] Heading deviation quantification subunit: When it is determined that there is an error in the current heading angle, the specific heading deviation angle is calculated based on the difference between the current correlation coefficient value and the threshold value. ;

[0026] Heading correction factor generation subunit: Based on the calculated heading deviation angle , combined with the current original measured heading angle , generating the final heading correction factor , the specific calculation formula is: .

[0027] Optionally, the correction parameter output unit includes:

[0028] Heading data fusion subunit: receives the heading correction factor output by the heading factor calculation unit, fuses it with the original heading angle data in real time, and obtains the corrected real-time heading angle data. The formula is: , where: The real-time heading angle after fusion correction; is the fusion weight factor;

[0029] Attitude parameter update subunit: real-time heading angle based on fusion correction ,Combined with the current pitch angle, roll angle and carrier speed obtained synchronously, the current real-time attitude data of the carrier is updated to form a complete motion data set.

[0030] 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; wherein:

[0031] 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 receive the water depth pressure value through the preset water depth pressure sensor, and then calculate the equivalent fluid resistance coefficient according to the fluid dynamics model ;

[0032] Eddy current effect compensation unit: Based on the equivalent fluid drag coefficient and combined with real-time heading angle data, it calculates the eddy current effect compensation correction value of the heading angle. It 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.

[0033] Trajectory data generation unit: used to receive the compensated heading angle output by the eddy current effect compensation unit, combine it with the real-time pitch angle, roll angle, speed and water depth values ​​of the carrier, and integrate it to form an interference-resistant real-time three-dimensional trajectory data set.

[0034] Optionally, the eddy current effect compensation unit includes:

[0035] Eddy current influence quantification subunit: based on equivalent fluid resistance coefficient , combined with the real-time navigation speed, the quantitative value of the heading angle affected by the eddy current is calculated using the formula: , where: is the quantitative value of the heading angle affected by the eddy current; is the eddy current effect constant, determined by the hydrodynamic calibration experiment; V is the movement speed; is the equivalent fluid resistance coefficient;

[0036] Heading compensation calculation subunit: used to quantify the influence of eddy current on the heading angle , combined with the sign relationship determined by the eddy current interference direction, the heading angle eddy current effect compensation correction is calculated , whose expression is: ;

[0037] Heading angle compensation correction subunit: used to compensate the eddy current effect correction amount The real-time heading angle after fusion correction Superposition is performed to obtain the real-time corrected heading angle after eddy current effect compensation .

[0038] Optionally, the autonomous navigation decision module includes a trajectory deviation analysis unit, a propeller parameter adjustment unit, and a navigation instruction generation unit; wherein:

[0039] Trajectory Deviation Analysis Unit: This unit receives the anti-interference 3D trajectory dataset output by the fluid resistance compensation module, compares it with the pre-set target path trajectory, and calculates the deviation values ​​in 3D space, including heading angle deviation, position deviation, and speed deviation, according to the system timestamp.

[0040] Propeller parameter adjustment unit: used to dynamically adjust the thrust size, direction angle and multi-propeller distribution ratio of the propeller according to the deviation value output by the trajectory deviation analysis unit using the PID control algorithm to continuously correct the navigation direction and speed;

[0041] Navigation instruction generation unit: Based on the thruster adjustment results and the position information of the 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.

[0042] Optionally, the thruster parameter adjustment unit includes:

[0043] Heading PID adjustment subunit: used to receive the heading angle deviation output by the trajectory deviation analysis unit, recorded as , using PID control algorithm to calculate the heading correction in real time ;

[0044] Speed ​​PID regulation subunit: used to receive position deviation and speed deviation in speed direction, recorded as and , also uses PID control algorithm to calculate the thrust correction value of the propeller in real time ;

[0045] Allocation ratio optimization subunit: used to correct the control amount according to the heading and speed correction control amount , dynamically optimize the thrust distribution ratio and direction angle of each propeller so that the total synthetic thrust and synthetic torque meet the desired navigation direction and speed correction requirements; the thrust distribution ratio optimization follows the following constraints:

[0046] ;

[0047] Where, The thrust assigned to the i-th thruster; is the current direction angle of the i-th thruster; is the vertical distance from the i-th thruster to the center of gravity; is the number of thrusters.

[0048] Beneficial effects of the present invention:

[0049] The present invention constructs a navigation perception system that integrates geomagnetic and acoustic dual sources, and utilizes a time synchronization mechanism and a correlation analysis model to achieve high-precision dynamic correction of heading angle errors, overcoming the problem of poor stability of a single sensor in complex underwater environments. At the same time, combined with the fluid resistance and eddy current compensation mechanism, it performs real-time compensation for the carrier's posture and external interference, effectively improving the accuracy of heading estimation.

[0050] The present invention adaptively adjusts the thruster parameters through the PID control algorithm, combined with the optimization of multi-thruster thrust distribution, to achieve dynamic linkage control of the thrust size and direction of the thrusters under the drive of trajectory deviation, thereby improving the path adaptability and navigation robustness of the underwater carrier in changing sea areas, and meeting the requirements of high-precision, autonomous, and anti-interference underwater navigation applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only for the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0052] Figure 1 is a schematic diagram of a navigation system according to an embodiment of the present invention;

[0053] Figure 2 Schematic diagram of a heading dynamic correction module according to an embodiment of the present invention. DETAILED DESCRIPTION

[0054] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments. It is also noted that, to provide a more detailed description, the following embodiments are best and preferred embodiments, and those skilled in the art may employ alternative methods for implementing certain known technologies. Furthermore, the accompanying drawings are intended only to provide a more detailed description of the embodiments and are not intended to limit the present invention.

[0055] like Figure 1-Figure 2 As shown, 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 module; wherein:

[0056] Geomagnetic and acoustic data acquisition module: used to synchronously collect geomagnetic gradient vector data and acoustic array propagation delay data of underwater carriers, and output dual-source verification data packets with time stamps;

[0057] Heading dynamic correction module: used to receive dual-source calibration data packets, 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 motion parameter set containing the corrected heading angle;

[0058] Fluid resistance compensation module: Based on the carrier posture data in the motion parameter set and combined with the real-time water depth pressure value, it calculates the equivalent fluid resistance coefficient to compensate for the eddy current effect on the heading angle, thereby generating anti-interference three-dimensional trajectory data;

[0059] Autonomous navigation decision 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 package containing obstacle avoidance points.

[0060] 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:

[0061] Geomagnetic sensing unit: Fixedly arranged inside the underwater carrier, it uses three sets of three-axis vector magnetometers, installed at the front, middle and rear of the underwater carrier respectively. The three sets of magnetometers are spaced apart in space with a spacing of no less than 0.5 meters. The sampling frequency of each magnetometer is 100Hz, which is used to obtain geomagnetic gradient vector data covering the entire ship in real time;

[0062] Acoustic receiving unit: Arranged on the surface of the underwater carrier shell, it uses six hydrophones evenly arranged in a circular pattern on the equatorial plane of the carrier shell. The angle between the hydrophones is 60 degrees, the array radius is 0.4 meters, and the measurement bandwidth is 1kHz-25kHz. It is used to collect the propagation delay data of the reference sound source signal at each point in the array;

[0063] Synchronous control unit: Connects the geomagnetic sensing unit and the acoustic receiving unit. It has a built-in synchronous clock chip with a time accuracy of 0.1ms. It is used to control the start trigger of geomagnetic and acoustic signal acquisition. It also marks the same timestamp on each frame of geomagnetic vector and acoustic propagation delay data to ensure the consistency of the sampling time of the dual-source acquisition data.

[0064] Data fusion output unit: 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 a unified data frame structure, and form a dual-source verification data packet with a unified timestamp; the above unit adopts three groups of spatially spaced three-axis magnetometers in the geomagnetic sensing unit and configures an equally spaced annular acoustic array in the acoustic receiving unit, so that the system can fully cover the circumferential magnetic field changes and sound wave propagation delays of the underwater carrier; combined with high-precision synchronous clock control, it significantly improves the accuracy of the time and space correspondence of the dual-source data, providing high-reliability input for heading correction and anti-interference navigation.

[0065] The heading dynamic correction module includes a correlation analysis unit, a heading factor calculation unit and a correction parameter output unit; wherein:

[0066] Correlation analysis unit: used to receive the dual-source verification data packets with time stamps output by the geomagnetic and acoustic data acquisition modules, extract the geomagnetic gradient vector component in the longitudinal 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 the sliding time window as the correlation measure of the current heading state;

[0067] Heading factor calculation unit: Based on the correlation coefficient output by the correlation analysis unit and the current navigation attitude state, the heading correction factor is calculated to dynamically adjust the original heading angle error and compensate for the impact of external magnetic disturbances or abnormal sound propagation on heading recognition;

[0068] Correction parameter output unit: Based on the heading correction factor, the original heading angle data is corrected in real time, and a motion parameter set including the corrected heading angle, attitude angle (pitch angle and roll angle), navigation speed and timestamp is output, and the parameter set is sent to the fluid resistance compensation module; the above unit uses the time-synchronized geomagnetic gradient vector and acoustic propagation delay data for correlation analysis and constructs a dynamic heading correction factor, which can effectively correct the heading error caused by geomagnetic interference or underwater acoustic channel anomalies, thereby improving the heading recognition accuracy of underwater carriers in complex waters.

[0069] The correlation analysis unit includes:

[0070] Geomagnetic component extraction subunit: receives dual-source verification data packets with timestamps, extracts the magnetic field component in the longitudinal direction of the carrier from the three-axis geomagnetic gradient vector data, and defines it as the longitudinal magnetic field intensity sequence;

[0071] Acoustic delay extraction subunit: This subunit determines two acoustic receiving sensors located opposite each other in the longitudinal direction according to the carrier's navigation direction, receives their propagation delay data, and calculates the difference between the two sensors to form an acoustic propagation delay difference sequence in the longitudinal direction.

[0072] Correlation coefficient calculation subunit: used to scan the extracted longitudinal magnetic field intensity sequence and acoustic propagation delay difference sequence step by step with a sliding time window of fixed length, 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 magnetic field intensity sequence and the acoustic propagation delay difference sequence; is the magnetic field intensity of the geomagnetic gradient vector in the longitudinal direction of the carrier at the i-th sampling point; is the acoustic wave propagation delay difference between acoustic sensors in the longitudinal direction at the i-th sampling point; and are the means of the corresponding sequences in the time window; N is the number of sampling points in the sliding time window; through the above clear sequence extraction and correlation coefficient calculation steps, the dynamic correlation between the geomagnetic data and the acoustic data in the longitudinal direction is accurately captured, the heading error characteristics are effectively quantified, and stable data support is provided for the calculation of the heading correction factor.

[0073] The heading factor calculation unit includes:

[0074] Correlation threshold judgment subunit: receives the correlation coefficient R output by the correlation analysis unit and compares it with the preset correlation threshold Compare and 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;

[0075] Heading deviation quantification subunit: When it is determined that there is an error in the current heading angle, the specific heading deviation angle is calculated based on the difference between the current correlation coefficient value and the threshold value. , the calculation method is as follows: , where is the heading angle deviation value, K is the heading deviation proportional factor, which is a constant obtained by experimental calibration;

[0076] Heading correction factor generation subunit: Based on the calculated heading deviation angle , combined with the current original measured heading angle , generating the final heading correction factor , the specific calculation formula is: ; The above-mentioned subunits accurately calculate the heading correction factor, effectively quantify and correct the heading deviation in real time, thereby improving the accuracy of heading angle measurement and ensuring that the underwater carrier navigation system has higher navigation accuracy and anti-interference capability in the underwater environment.

[0077] The calibration parameter output unit includes:

[0078] Heading data fusion subunit: receives the heading correction factor output by the heading factor calculation unit, fuses it with the original heading angle data in real time, and obtains the corrected real-time heading angle data. The formula is: , where: The real-time heading angle after fusion correction; is the fusion weight factor, the value range is [0,1];

[0079] Attitude parameter update subunit: real-time heading angle based on fusion correction ,Combined with the current pitch angle, roll angle and carrier speed obtained synchronously, the current real-time attitude data of the carrier is updated to form a complete motion data set.

[0080] The fluid resistance compensation module includes a fluid resistance coefficient calculation unit, an eddy current effect compensation unit and a trajectory data generation unit; wherein:

[0081] 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 receive the water depth pressure value through the preset water depth pressure sensor, and then calculate the equivalent fluid resistance coefficient according to the fluid dynamics model , the specific calculation formula is: , where is the calculated equivalent fluid resistance coefficient; is the system basic resistance coefficient, obtained by experimental calibration; is the pressure influence coefficient; p is the real-time water depth pressure value; is the coefficient of the square term of velocity; V is the movement 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;

[0082] Eddy current effect compensation unit: Based on the equivalent fluid drag coefficient and combined with real-time heading angle data, it calculates the eddy current effect compensation correction value of the heading angle. It 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.

[0083] Trajectory data generation unit: used to receive the compensated heading angle output by the eddy current effect compensation unit, combine it with the real-time pitch angle, roll angle, speed and water depth values ​​of the carrier, integrate it to form an interference-resistant real-time three-dimensional trajectory data set, and output it to the autonomous navigation decision module for use; through the combination of the above units, real-time and accurate compensation for eddy current interference during the navigation of the underwater carrier is achieved, effectively improving the stability of underwater navigation and the accuracy of trajectory tracking, and providing a reliable navigation basis for the underwater carrier in complex underwater environments.

[0084] The eddy current effect compensation unit includes:

[0085] Eddy current influence quantification subunit: based on equivalent fluid resistance coefficient , combined with the real-time navigation speed, the quantitative value of the heading angle affected by the eddy current is calculated using the formula: , where: is the quantitative value of the heading angle affected by the eddy current; is the eddy current effect constant, determined by the hydrodynamic calibration experiment; V is the movement speed; is the equivalent fluid resistance coefficient;

[0086] Heading compensation calculation subunit: used to quantify the influence of eddy current on the heading angle , combined with the sign relationship determined by the eddy current interference direction, the heading angle eddy current effect compensation correction is calculated , whose expression is: ;

[0087] Heading angle compensation correction subunit: used to compensate the eddy current effect correction amount The real-time heading angle after fusion correction Superposition is performed to obtain the real-time corrected heading angle after eddy current effect compensation , the specific calculation formula is as follows: Through the above subunits, the heading deviation of the underwater carrier caused by the eddy current effect of the outer shell can be corrected quantitatively in real time, thereby effectively enhancing the accuracy and reliability of the underwater navigation heading angle.

[0088] The autonomous navigation decision module includes a trajectory deviation analysis unit, a propeller parameter adjustment unit, and a navigation instruction generation unit; wherein:

[0089] Trajectory Deviation Analysis Unit: This unit receives the anti-interference 3D trajectory dataset output by the fluid resistance compensation module, compares it with the pre-set target path trajectory, and calculates the deviation values ​​in 3D space, including heading angle deviation, position deviation, and speed deviation, and updates the deviation sequence in real time.

[0090] Propeller parameter adjustment unit: This unit is used to dynamically adjust the thrust, direction angle, and multi-propeller distribution ratio of the propellers based on the deviation value output by the trajectory deviation analysis unit using the PID control algorithm (proportional-integral-differential control) to continuously correct the navigation direction and speed, ensuring that the vehicle gradually approaches the preset path.

[0091] Navigation instruction generation unit: Based on the thruster adjustment results and the position information of the obstacle avoidance points in three-dimensional space, a structured real-time navigation instruction package is generated, 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. The 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 the navigation action; through the above units, the autonomous navigation decision module can realize the dynamic linkage of the underwater vehicle's spatial trajectory and propulsion control, so that the vehicle can automatically correct the 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.

[0092] The thruster parameter adjustment unit includes:

[0093] Heading PID adjustment subunit: used to receive the heading angle deviation output by the trajectory deviation analysis unit, recorded as , using PID control algorithm to calculate the heading correction in real time , the formula is: , where is the heading correction control amount; is the current heading angle deviation; 、 、 They are the proportional, integral and differential coefficients of heading PID control respectively;

[0094] Speed ​​PID regulation subunit: used to receive position deviation and speed deviation in speed direction, recorded as and , also uses PID control algorithm to calculate the thrust correction value of the propeller in real time , the formula is: , where is the speed correction control amount; is the current speed deviation; 、 、 They are the proportional, integral and differential coefficients of speed PID control respectively;

[0095] Allocation ratio optimization subunit: used to correct the control amount according to the heading and speed correction control amount , dynamically optimize the thrust distribution ratio and direction angle of each propeller so that the total synthetic thrust and synthetic torque meet the desired navigation direction and speed correction requirements; the thrust distribution ratio optimization follows the following constraints:

[0096] ;

[0097] Where, The thrust assigned to the i-th thruster; is the current direction angle of the i-th thruster; is the vertical distance from the i-th thruster to the center of gravity; is the number of thrusters; through the above PID dynamic adjustment and allocation optimization steps, the thruster parameter adjustment unit can realize continuous and accurate correction of navigation direction and speed, ensure the underwater carrier to move stably and quickly along the predetermined path, and effectively improve the dynamic performance and anti-interference ability of navigation control.

[0098] The present invention encompasses any alternatives, modifications, equivalents, and solutions that fall within the spirit and scope of the present invention. To provide a thorough understanding of the present invention, specific details are described in detail below in connection with the preferred embodiments of the present invention, but those skilled in the art will be able to fully understand the present invention without these detailed descriptions. Furthermore, to avoid unnecessary confusion regarding the essence of the present invention, well-known methods, processes, procedures, components, and circuits have not been described in detail.

[0099] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.

Claims

1. An autonomous underwater integrated navigation real-time navigation system, characterized in that: It includes geomagnetic and acoustic data acquisition module, heading dynamic correction module, fluid resistance compensation module and autonomous navigation decision module; among which: Geomagnetic and acoustic data acquisition module: used to synchronously collect geomagnetic gradient vector data and acoustic array propagation delay data of underwater carriers, and output dual-source verification data packets with time stamps; Heading dynamic correction module: used to receive dual-source calibration data packets, 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 motion parameter set containing the corrected heading angle; Fluid resistance compensation module: Based on the carrier posture data in the motion parameter set and combined with the real-time water depth pressure value, it calculates the equivalent fluid resistance coefficient to compensate for the eddy current effect on the heading angle, thereby generating anti-interference three-dimensional trajectory data; Autonomous navigation decision 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 package containing obstacle avoidance points.

2. The autonomous underwater combined navigation 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 synchronization control unit, and a data fusion output unit; wherein: Geomagnetic sensing unit: Fixedly arranged inside the underwater carrier, it uses three sets of three-axis vector magnetometers, installed at the front, middle and rear of the underwater carrier respectively. The three sets of magnetometers are spaced apart in space with a spacing of no less than 0.5 meters. The sampling frequency of each magnetometer is 100Hz, which is used to obtain geomagnetic gradient vector data covering the entire ship in real time; Acoustic receiving unit: Arranged on the surface of the underwater carrier shell, it uses six hydrophones evenly arranged in a circular pattern on the equatorial plane of the carrier shell. The angle between the hydrophones is 60 degrees, the array radius is 0.4 meters, and the measurement bandwidth is 1kHz-25kHz. It is used to collect the propagation delay data of the reference sound source signal at each point in the array; Synchronous control unit: Connects the geomagnetic sensing unit and the acoustic receiving unit. It has a built-in synchronous clock chip with a time accuracy of 0.1ms. It is used to control the start trigger of geomagnetic and acoustic signal acquisition. It also marks the same timestamp on 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 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 a unified data frame structure, and form a dual-source verification data packet with a unified timestamp.

3. The autonomous underwater combined navigation integrated real-time navigation system 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; wherein: Correlation analysis unit: used to receive the dual-source verification data packets with time stamps output by the geomagnetic and acoustic data acquisition modules, extract the geomagnetic gradient vector component in the longitudinal 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 the sliding time window as the correlation measure of the current heading state; Heading factor calculation unit: calculates the heading correction factor based on the correlation coefficient output by the correlation analysis unit and the current navigation attitude state; Correction parameter output unit: Based on the heading correction factor, the original heading angle data is corrected in real time, and the output includes a motion parameter set including the corrected heading angle, attitude angle, navigation speed and timestamp.

4. The autonomous underwater combined navigation integrated real-time navigation system according to claim 3, characterized in that: The correlation analysis unit includes: Geomagnetic component extraction subunit: receives dual-source verification data packets with timestamps, extracts the magnetic field component in the longitudinal direction of the carrier from the three-axis geomagnetic gradient vector data, and defines it as the longitudinal magnetic field intensity sequence; Acoustic delay extraction subunit: This subunit determines two acoustic receiving sensors located opposite each other in the longitudinal direction according to the carrier's navigation direction, receives their propagation delay data, and calculates the difference between the two sensors to form an acoustic propagation delay difference sequence in the longitudinal direction. Correlation coefficient calculation subunit: used to scan the extracted longitudinal magnetic field intensity sequence and acoustic propagation delay difference sequence step by step with a sliding time window of fixed length, and calculate the correlation coefficient R of the two sequences in each time window respectively.

5. The autonomous underwater combined navigation integrated real-time navigation system according to claim 4, characterized in that: The heading factor calculation unit includes: Correlation threshold judgment subunit: receives the correlation coefficient R output by the correlation analysis unit and compares it with the preset correlation threshold Compare and if R is lower than the threshold, it is determined that there is an error in the current heading angle; Heading deviation quantification subunit: When it is determined that there is an error in the current heading angle, the specific heading deviation angle is calculated based on the difference between the current correlation coefficient value and the threshold value. ; Heading correction factor generation subunit: Based on the calculated heading deviation angle , combined with the current original measured heading angle , generate the final heading correction factor , the specific calculation formula is: .

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

7. The autonomous underwater combined navigation integrated real-time navigation system 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; wherein: 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 receive the water depth pressure value through the preset water depth pressure sensor, and then calculate the equivalent fluid resistance coefficient according to the fluid dynamics model ; Eddy current effect compensation unit: Based on the equivalent fluid drag coefficient and combined with real-time heading angle data, it calculates the eddy current effect compensation correction value of the heading angle. It 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, combine it with the real-time pitch angle, roll angle, speed and water depth values ​​of the carrier, and integrate it to form an interference-resistant real-time three-dimensional trajectory data set.

8. The autonomous underwater combined navigation integrated real-time navigation system according to claim 7, characterized in that: The eddy current effect compensation unit includes: Eddy current influence quantification subunit: based on equivalent fluid resistance coefficient , combined with the real-time navigation speed, the quantitative value of the heading angle affected by the eddy current is calculated using the formula: , where: is the quantitative value of the heading angle affected by the eddy current; is the eddy current effect constant, determined by the hydrodynamic calibration experiment; V is the movement speed; is the equivalent fluid resistance coefficient; Heading compensation calculation subunit: used to quantify the influence of eddy current on the heading angle , combined with the sign relationship determined by the eddy current interference direction, the heading angle eddy current effect compensation correction is calculated , whose expression is: ; Heading angle compensation correction subunit: used to compensate the eddy current effect correction amount The real-time heading angle after fusion correction Superposition is performed to obtain the real-time corrected heading angle after eddy current effect compensation .

9. The autonomous underwater combined navigation integrated real-time navigation system according to claim 1, characterized in that: The autonomous navigation decision module includes a trajectory deviation analysis unit, a propeller parameter adjustment unit and a navigation instruction generation unit; wherein: Trajectory Deviation Analysis Unit: This unit receives the anti-interference 3D trajectory dataset output by the fluid resistance compensation module, compares it with the pre-set target path trajectory, and calculates the deviation values ​​in 3D space, including heading angle deviation, position deviation, and speed deviation, according to the system timestamp. Propeller parameter adjustment unit: used to dynamically adjust the thrust size, direction angle and multi-propeller distribution ratio of the propeller according to the deviation value output by the trajectory deviation analysis unit using the PID control algorithm to continuously correct the navigation direction and speed; Navigation instruction generation unit: Based on the thruster adjustment results and the position information of the 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.

10. The autonomous underwater combined navigation integrated real-time navigation system according to claim 9, characterized in that: The thruster parameter adjustment unit includes: Heading PID adjustment subunit: used to receive the heading deviation angle output by the trajectory deviation analysis unit, recorded as , using PID control algorithm to calculate the heading correction in real time ; Speed ​​PID regulation subunit: used to receive position deviation and speed deviation in speed direction, recorded as and , also uses PID control algorithm to calculate the thrust correction value of the propeller in real time ; Allocation ratio optimization subunit: used to adjust the heading and thrust correction , dynamically optimize the thrust distribution ratio and direction angle of each propeller so that the total synthetic thrust and synthetic torque meet the desired navigation direction and speed correction requirements; the thrust distribution ratio optimization follows the following constraints: ； Where, The thrust assigned to the i-th thruster; is the current direction angle of the i-th thruster; is the vertical distance from the i-th thruster to the center of gravity; is the number of thrusters.

Images