Multi-source information fusion accurate navigation method and system for underwater vehicle

Abstract

The invention provides an underwater vehicle multi-source information fusion accurate navigation method and system. The method comprises the following steps: firstly, acquiring inertial navigation data, DVL data and ocean current information of an underwater vehicle and observation information of a distance between the underwater vehicle and a beacon, and preprocessing the inertial navigation data, the DVL data and the ocean current information; then, establishing a state model according to the kinematics principle of the underwater vehicle, and establishing an observation model by combining various types of information; carrying out fusion processing on the state model and the observation model by adopting a self-adaptive Kalman filtering algorithm; and finally, estimating the current position of the underwater vehicle according to the fused state vector, comparing the current position with a preset target position to calculate a deviation, and correcting the deviation by adjusting a rudder angle and the rotating speed of a propeller. According to the method, multi-source information is integrated, innovation is carried out on construction of a state model and an observation model and position estimation and deviation correction, adaptive Kalman filtering is adopted, and the method has the advantages that the navigation precision is improved, the ocean current influence is considered, adaptive adjustment is realized, and real-time navigation is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle navigation, and particularly relates to a precise navigation method and system for multi-source information fusion of an underwater vehicle. Background Art

[0002] With the continuous deepening of ocean development and research, underwater vehicles play an increasingly important role in the fields of ocean scientific investigation, ocean resource exploration, military reconnaissance, etc. The navigation technology of underwater vehicles is the key to their autonomous operation. Precise navigation information can ensure that underwater vehicles accurately reach the target position, improving operation efficiency and safety.

[0003] Currently, the commonly used navigation methods for underwater vehicles mainly include inertial navigation, acoustic navigation, satellite navigation, etc. The inertial navigation system (INS) is an autonomous navigation system that calculates its position and attitude by measuring the acceleration and angular velocity of the vehicle. The inertial navigation system has the advantages of high short-term accuracy and strong autonomy. However, due to the accumulation of its errors over time, the long-term navigation accuracy will gradually decrease. Acoustic navigation uses the propagation characteristics of sound waves in water to determine the position of the vehicle. Commonly used acoustic navigation devices include Doppler velocity log (DVL), long baseline positioning system (LBL), short baseline positioning system (SBL), and ultra-short baseline positioning system (USBL), etc. The acoustic navigation system can provide relatively accurate position information, but it is greatly affected by the underwater acoustic environment, such as sound speed changes, multipath effects, etc., which will lead to an increase in measurement errors. The satellite navigation system (such as GPS) can provide high-precision global positioning information, but since satellite signals cannot penetrate seawater, underwater vehicles cannot directly use satellite navigation underwater.

[0004] In practical applications, a single navigation method often fails to meet the requirements of precise navigation of underwater vehicles. To improve navigation accuracy, a multi-source information fusion method is usually adopted to fuse data from different types of navigation sensors. The existing multi-source information fusion methods mainly include Kalman filtering, extended Kalman filtering, unscented Kalman filtering, etc. These methods have improved the navigation accuracy of underwater vehicles to a certain extent, but there are still some deficiencies. For example, the existing methods often do not fully consider the influence of ocean current information on the movement of underwater vehicles, resulting in an increase in navigation errors. In addition, when dealing with multi-source information fusion, the existing methods often adopt a fixed fusion strategy, lacking real-time evaluation and adaptive adjustment of the data quality of different navigation sensors, and unable to achieve optimal navigation performance in complex underwater environments.

[0005] Therefore, there is a need for an accurate navigation method that can make full use of multi-source acoustic information, take into account the influence of ocean current information on the movement of an underwater vehicle, and can evaluate and adaptively adjust the data quality of different navigation sensors in real time to meet the accurate navigation requirements of the underwater vehicle in large-scale deep-sea operations. Summary of the Invention

[0006] The object of the present invention is to propose an accurate navigation method and system for multi-source information fusion of an underwater vehicle, which makes full use of inertial navigation, DVL, ocean current information, and the observed information of the distance to a beacon to estimate the current position of the underwater vehicle, correct the deviation, and provide real-time and accurate navigation information for large-scale deep-sea operations.

[0007] To achieve the above object, the technical solution of the present invention is as follows:

[0008] The present invention proposes an accurate navigation method for multi-source information fusion of an underwater vehicle, including the following steps:

[0009] Data acquisition: including acquiring inertial navigation data, DVL data, ocean current information, and the observed information of the distance to a beacon of the underwater vehicle;

[0010] Data preprocessing: preprocessing the acquired various data as observed data, including removing noise and calibrating errors;

[0011] Establishing a state model: according to the kinematic principle of the underwater vehicle, establishing a state model of the underwater vehicle; the state model includes the state variables of the vehicle and the change rate of the state variables, and takes the speed and direction of the ocean current as input parameters of the state model;

[0012] Establishing an observation model: according to inertial navigation data, DVL data, ocean current information, and the observed information of the distance to a beacon, establishing an observation model of the underwater vehicle, and the observation model describes the relationship between state variables and observed data;

[0013] Multi-source information fusion: using an adaptive Kalman filtering algorithm to fuse the state model and the observation model, evaluating the data quality of different navigation sensors in real time, and adaptively adjusting the filtering gain according to the data quality;

[0014] Position estimation and deviation correction: according to the state vector after fusion processing, estimating the current position of the underwater vehicle, comparing the estimated position with a preset target position, calculating the deviation, and correcting the movement of the underwater vehicle according to the deviation.

[0015] Preferably, in the data acquisition step, the inertial navigation data includes the acceleration and angular velocity of the underwater vehicle, the DVL data includes the velocity of the underwater vehicle relative to the seabed, the ocean current information includes the velocity and direction of the ocean current, and the observation information of the distance to the beacon is obtained by an acoustic ranging device.

[0016] Preferably, in the data preprocessing step:

[0017] For the inertial navigation data, the accelerometer and gyroscope are calibrated, and a filtering algorithm is used to remove noise

[0018] For the DVL data, Doppler frequency shift correction and error compensation are performed;

[0019] For the ocean current information, smoothing processing and error estimation are performed;

[0020] For the observation information of the distance to the beacon, multipath effect compensation and error correction are performed.

[0021] Preferably, the establishment of the state model specifically includes the following steps:

[0022] Define the state vector of the underwater vehicle's motion :

[0023]

[0024] Wherein, represents the position of the underwater vehicle in the Cartesian coordinate system; are respectively the underwater vehicle in the Cartesian coordinate system , , velocities in the directions; are successively the roll angle, pitch angle and heading angle of the underwater vehicle; the superscript T represents the transpose;

[0025] Define the control vector of the underwater vehicle's motion :

[0026]

[0027] Wherein, are respectively the ocean current in , , velocity components in the directions;

[0028] Define the process noise vector used to describe the uncertainty of the state model as , and the noise covariance matrix is ;

[0029] Then the state model of the underwater vehicle is expressed as:

[0030]

[0031] Among them, is a non-linear state transition function.

[0032] Preferably, the non-linear function is calculated as follows:

[0033] According to the kinematic principle of the underwater vehicle, the change rate of the state vector is described by the following dynamic equation :

[0034]

[0035]

[0036]

[0037]

[0038] Among them, is the acceleration of the underwater vehicle, measured by the inertial navigation system; , and are the change rates of the ocean current velocity in the , , directions respectively; is the angular velocity of the vehicle, measured by the gyroscope in the inertial navigation system; the above the parameter represents its first-order derivative with respect to time.

[0039] Preferably, the establishment of the observation model specifically includes the following steps:

[0040] Establish an observation equation based on DVL data:

[0041]

[0042] Among them, is the velocity vector measured by DVL, are the DVL-measured velocities in the , , directions respectively; is the angular deviation of the DVL installation, are respectively , , the angular deviations of the DVL installation in the is the rotation matrix; is the velocity vector of the underwater vehicle in the Cartesian coordinate system; is the DVL measurement error, is the covariance matrix of the DVL measurement error, then the error Subject to the mean of zero, the covariance matrix is The multivariate normal distribution of ;

[0043] Establish the observation equation based on the distance observation information between the beacon and the beacon:

[0044]

[0045] in, is the measured distance between the underwater vehicle and the beacon; is the distance between the underwater vehicle and the beacon calculated based on the positions of the underwater vehicle and the beacon; represents the position of the underwater vehicle in the Cartesian coordinate system, Indicates the location of the beacon is; Represents the acoustic ranging error, is the variance of the acoustic ranging error, then the error amount It has a mean of zero and a variance of Normal distribution ;

[0046] Establish the observation equation based on inertial navigation data:

[0047]

[0048] in, is the acceleration vector measured by the inertial navigation system, The inertial navigation system is measured 、 、 acceleration in direction; is the angular velocity vector measured by the inertial navigation system, The inertial navigation system is measured 、 、 Angular velocity in direction; and are the actual underwater vehicle acceleration vector and angular velocity vector respectively; and They are respectively the acceleration error and angular velocity error of inertial measurement, and are the covariance matrices of acceleration error and angular velocity measurement error respectively, then the error amount Subject to the mean of zero, the covariance matrix is The multivariate normal distribution of , error amount obeys a multivariate normal distribution with a mean of zero and a covariance matrix of ; ;

[0049] Establish an observation equation based on ocean current information:

[0050]

[0051] where is the ocean current velocity vector measured by the ocean current sensor, respectively represent the ocean current velocities measured by the ocean current sensor in the , , directions; is the actual ocean current velocity vector, is , , direction of the actual ocean current velocity vector; is the measurement error of the ocean current sensor, denoted as is the covariance matrix of the measurement error of the ocean current sensor, then the error quantity obeys a multivariate normal distribution with a mean of zero and a covariance matrix of ; ;

[0052] Combining the above observation equations, the observation model of the underwater vehicle is obtained:

[0053] where is the observation vector; is the state vector, is the observation function; is the observation noise vector, and the noise covariance matrix is .

[0054] Preferably, in the multi-source information fusion step, the specific steps of the adaptive Kalman filtering algorithm are as follows:

[0055] Initialization: Initialize the state vector and covariance matrix of the Kalman filter;

[0056]

[0057] where is the initial position of the underwater vehicle, is the initial velocity of the underwater vehicle, is the initial attitude of the underwater vehicle; is a Symmetric matrix, where the diagonal elements of the matrix represent the initial variances of the respective state variables;

[0058] Prediction: The state vector at the current time k predicted according to the state model and covariance matrix :

[0059] where, is the state estimate at time k - 1; is the control vector at time k; is the state transition matrix for the state evolving from time k - 1 to time k; is the covariance matrix at time k - 1; is the process noise covariance matrix;

[0060] Update: Calculate the predicted value of the observation at the current time k according to the observation model and the observation covariance matrix :

[0061] where, is the observation function describing the relationship between the state variables and the observed data; is the observation matrix; is the observation noise covariance matrix;

[0062] Calculate the filter gain: Calculate the filter gain :

[0063]

[0064] Update the state vector and covariance matrix: Update the state vector and covariance matrix according to the filter gain:

[0065] where, and represent the updated state vector and covariance matrix, is the actual observation vector, is the identity matrix;

[0066] Adaptive adjustment: Real-time evaluate the quality of data from different navigation sensors and adaptively adjust the filter gain according to the data quality.

[0067] Preferably, the real-time evaluation of the quality of data from different navigation sensors and the adaptive adjustment of the filter gain according to the data quality are as follows:

[0068] Define the evaluation index for the quality of sensor data , used to measure the The reliability of sensor data; The calculation factors include the measurement error of the sensor and the stability of the data;

[0069] When calculating the filtering gain, an adaptive adjustment factor negatively correlated with the sensor data quality evaluation index is introduced, and for the observation noise covariance matrix of the th sensor, it is adjusted as follows:

[0070]

[0071] is the adjusted observation noise covariance matrix; when is small, increases, and the weight of this sensor data in the fusion process is reduced; when is large, decreases, and the weight of this sensor data in the fusion process is increased.

[0072] Preferably, the position estimation and deviation correction steps are specifically as follows:

[0073] Based on the state vector after fusion processing, the current position of the underwater vehicle is estimated, and the position estimation is optimized by combining the ocean current influence through the following formula; where , , and respectively represent the three-dimensional position estimation values of the underwater vehicle in the Cartesian coordinate system;

[0074] The actual speed of the vehicle is expressed as:

[0075]

[0076] where, represents the speed of the underwater vehicle relative to the seabed, represents the ocean current speed;

[0077] According to the relationship between speed and position, within the time interval , the update of the vehicle position is expressed as:

[0078]

[0079] where, , , are the updated three-dimensional position estimation values of the underwater vehicle in the Cartesian coordinate system;

[0080] Estimate the position and compare it with the preset target position to calculate the deviation and the modulus of the deviation :

[0081]

[0082] ;

[0083] Adjust the rudder angle and thruster speed of the vehicle through a control law based on the deviation for deviation correction, where is the control input vector of the underwater vehicle, is the rudder angle, is the thruster thrust, , and are the proportional, integral and derivative gains respectively;

[0084] At the same time, combine the dynamic equation of the underwater vehicle to achieve closed-loop control, where is the inertia matrix, is the Coriolis force and centripetal force matrix, is the damping matrix, is the gravity and buoyancy vector, is the attitude vector of the vehicle.

[0085] The present invention provides an underwater vehicle multi-source information fusion precise navigation system, including a processor, a memory, and a computer program stored on the memory. When the processor executes the computer program, it specifically executes the steps in any one of the above-mentioned underwater vehicle multi-source information fusion precise navigation methods.

[0086] Compared with the prior art, the present invention has the following beneficial effects:

[0087] Improve navigation accuracy: By fully fusing inertial navigation, DVL, ocean current information, and the observed information of the distance to the beacon, the present invention can effectively reduce the errors of a single navigation sensor. For example, although there are noises and errors in the acquisition of inertial navigation data, through Kalman filtering and calibration processing, the data quality can be improved; DVL data is corrected by Doppler frequency shift and error compensation to reduce measurement deviation; ocean current information is smoothed and error-corrected to more accurately reflect the ocean current situation; the observed information of the distance to the beacon is compensated for multipath effects and error-corrected to reduce ranging errors. The multi-source information fusion reduces the influence of the errors of each sensor itself, thereby improving the navigation accuracy of the underwater vehicle.

[0088] Consider the influence of ocean currents: When establishing the state model, fully consider the influence of ocean current information on the movement of the underwater vehicle. The state vector Contains key information such as the position, speed, and attitude of the vehicle, and the control vector Mainly consider the ocean current information, take the speed and direction of the ocean current as the input parameters of the state model, and consider the influence of the ocean current on the vehicle speed in the dynamic equation, which can more accurately describe the motion state of the underwater vehicle and further improve the navigation accuracy.

[0089] Adaptive adjustment: Adopt the adaptive Kalman filter algorithm, which can evaluate the quality of different navigation sensor data in real time and adaptively adjust the filtering gain according to the data quality. Define the sensor data quality evaluation index , through the adaptive adjustment factor Adjust the observation noise covariance matrix . When is small, increases, and the weight of this sensor data in the fusion process is reduced; when is large, decreases, and the weight of this sensor data in the fusion process is increased. This adaptive adjustment mechanism can make full use of the advantages of each sensor in a complex underwater environment and improve the robustness and accuracy of the navigation system.

[0090] Real-time navigation: It can estimate the current position of the underwater vehicle in real time and correct the motion of the underwater vehicle according to the deviation. Obtain the state vector through multi-source information fusion, and optimize the position estimation by combining the influence of the ocean current. Compare the estimated position with the preset target position to calculate the deviation, and adjust the rudder angle and propeller speed of the vehicle through the deviation-based control law to correct the deviation, providing real-time and accurate navigation information for large-scale deep-sea operations. Brief Description of the Drawings

[0091] Figure 1 It is a flowchart of the multi-source information fusion precise navigation method for the underwater vehicle of the present invention. Detailed Embodiment

[0092] The following combines the attached Figure 1 to specifically describe the technical solution of the present invention.

[0093] The present invention proposes a multi-source information fusion precise navigation method for an underwater vehicle, including the following steps:

[0094] Data acquisition: Include collecting inertial navigation data, DVL data, ocean current information, and observation information on the distance from the beacon of the underwater vehicle;

[0095] Data preprocessing: Preprocess the collected various data as observation data, including removing noise and calibrating errors;

[0096] Establish a state model: According to the kinematic principle of the underwater vehicle, establish a state model of the underwater vehicle; the state model includes the state variables of the vehicle, the change rate of the state variables, and takes the velocity and direction of the ocean current as the input parameters of the state model;

[0097] Establish an observation model: According to the inertial navigation data, DVL data, ocean current information, and the observation information of the distance to the beacon, establish an observation model of the underwater vehicle, and the observation model describes the relationship between the state variables and the observation data;

[0098] Multi-source information fusion: Use the adaptive Kalman filter algorithm to fuse the state model and the observation model, evaluate the quality of the data of different navigation sensors in real time, and adaptively adjust the filtering gain according to the data quality;

[0099] Position estimation and deviation correction: According to the state vector after fusion processing, estimate the current position of the underwater vehicle, compare the estimated position with the preset target position, calculate the deviation, and correct the movement of the underwater vehicle according to the deviation.

[0100] In this embodiment, in the data acquisition step, the inertial navigation data includes the acceleration and angular velocity of the underwater vehicle, the DVL data includes the velocity of the underwater vehicle relative to the seabed, the ocean current information includes the velocity and direction of the ocean current, and the observation information of the distance to the beacon is obtained by an acoustic ranging device. The data acquisition processes of the inertial navigation system, DVL, ocean current sensor, and acoustic ranging device will be elaborated in detail below, and technical details and formulas will be given in combination with the characteristics of the underwater environment.

[0101] 1.1 Inertial navigation data acquisition

[0102] The inertial navigation system is mainly used to measure the acceleration and angular velocity of the underwater vehicle. Let the acceleration vector collected by the inertial navigation system at time be , and the angular velocity vector be ; where, T represents transpose, are respectively , , accelerations in the directions, and , , are respectively angular velocities in the

[0103] In the actual acquisition process, due to the noise and errors of the sensors, the actually collected acceleration and angular velocity are expressed as:

[0104] where, and are the bias errors of the accelerometer and gyroscope respectively, and are the noises of the accelerometer and gyroscope respectively. It is usually assumed that they are Gaussian white noises, and their covariances are and .

[0105] To improve the accuracy of data acquisition, the inertial navigation system needs to collect data at a relatively high sampling frequency and generally can be set to to , so as to capture the changes in the motion state of the vehicle in real time.

[0106] 1.2 DVL Data Acquisition

[0107] The DVL is used to measure the speed of the underwater vehicle relative to the seabed. Let the speed vector of the vehicle relative to the seabed measured by the DVL at be , where are the speeds of the vehicle relative to the seabed measured by the DVL at in the , , directions respectively.

[0108] Due to the influence of factors such as Doppler frequency shift and sound speed change on DVL measurement, the actual measured value deviates from the true value and can be expressed as:

[0109] where is the system error of the DVL, is the DVL measurement noise, and its covariance is .

[0110] The sampling frequency of the DVL is generally set to to to balance the data update frequency and measurement accuracy.

[0111] 1.3 Sea Current Information Acquisition

[0112] The sea current sensor is used to collect the speed and direction of the sea current. Let the speed vector of the sea current at be , where respectively represent the speeds of the sea current in , , directions, and the direction can be represented by the angle<X and indicates that is the deflection angle of the ocean current direction in the horizontal plane, that is, the course angle, which is usually defined as the angle between the projection of the ocean current velocity on the horizontal plane and the due north direction; indicates the angle between the ocean current direction and the horizontal plane, that is, the pitch angle, which is positive when the ocean current is upward and negative when it is downward.

[0113] The actually measured ocean current velocity and the actually measured direction 、 will be affected by sensor errors and ocean environmental noise, and can be expressed as:

[0114] where is the systematic error of the ocean current sensor, is the measurement noise of the ocean current sensor, and its covariance is ; and are the zero bias errors of the direction measurement, and are the noises of the direction measurement, and their covariances are and respectively.

[0115] The sampling frequency of the ocean current sensor can be adjusted according to the severity of the ocean current change, and is generally set to to .

[0116] 1.4 Acquisition of Observation Information on the Distance to the Beacon

[0117] The acoustic ranging device measures the distance between the underwater vehicle and the beacon by transmitting and receiving sound waves. Let the true distance between the underwater vehicle and the j-th beacon at time be , and the distance measured by the acoustic ranging device is .

[0118] Since the propagation of sound waves in water is affected by factors such as sound speed change and multipath effect, there is a deviation between the measured value and the true value, which can be expressed as:

[0119] where is the systematic error of the ranging system, is the measurement noise of the ranging system, and its covariance is .

[0120] The sampling frequency of the acoustic ranging device is generally set to to , to avoid interference between acoustic signals.

[0121] The above - collected data is transmitted to the navigation computer through a data transmission module according to a certain communication protocol (such as CAN bus, Ethernet, etc.) for subsequent processing.

[0122] In this embodiment, in the data pre - processing step:

[0123] For inertial navigation data, calibrate the accelerometer and gyroscope, and use a filtering algorithm to remove noise

[0124] For DVL data, perform Doppler frequency shift correction and error compensation;

[0125] For ocean current information, perform smoothing processing and error estimation;

[0126] For the observation information of the distance to the beacon, perform multipath effect compensation and error correction.

[0127] Data pre - processing is a key step to improve the performance of the multi - source information fusion precise navigation method for underwater vehicles. It can effectively remove noise, calibrate errors, and improve data quality. The pre - processing methods are elaborated in detail for different types of data below.

[0128] 2.1 Inertial Navigation Data Pre - processing

[0129] The acceleration and angular velocity data output by the inertial navigation system usually contain noise, zero - bias error, scale factor error, etc. To improve data quality, the Kalman filtering algorithm is used for filtering processing and calibration is performed according to calibration parameters.

[0130] 2.1.1 Calibration

[0131] Let the output of the accelerometer be , the true acceleration be , the zero - bias error of the accelerometer be , the scale factor error of the accelerometer be , then the calibration formula is:

[0132] Similarly, for the output of the gyroscope , the true angular velocity be , the zero - bias error of the gyroscope be , the scale factor error of the gyroscope be , the calibration formula is:

[0133] 2.1.2 Kalman Filtering

[0134] Kalman filter is an optimal recursive filter that estimates the system state through two steps: prediction and update. Let the state vector of the inertial navigation system at the current k-th moment be , and the observation vector be . The state transition equation and the observation equation are respectively:

[0135] where is the state transition matrix, is the control input matrix, is the control input vector, is the process noise vector, is the observation matrix, is the observation noise vector.

[0136] The specific steps of the Kalman filter are as follows:

[0137] Prediction:

[0138] where is the predicted state vector of the inertial navigation system at the current k-th moment, is the predicted state vector of the inertial navigation system at the (k - 1)-th moment; is the predicted covariance matrix at the k-th moment, is the predicted covariance matrix at the (k - 1)-th moment; is the process noise covariance matrix.

[0139] Update:

[0140] where is the Kalman gain, is the updated state vector, is the updated covariance matrix, is the observation noise covariance matrix, is the identity matrix.

[0141] 2.2 DVL Data Preprocessing

[0142] The DVL data needs to be corrected for Doppler frequency shift and error compensated. 7]

[0143] 2.2.1 Doppler Frequency Shift Correction

[0144] Let the Doppler frequency shift measured by the DVL be , the speed of sound be , the installation angle of the DVL be , and the true speed of the vehicle relative to the seabed be . Then the Doppler frequency shift correction formula is:

[0145] Among them, is the emission frequency; the true velocity can be calculated through the above formula :

[0146] 2.2.2 Error Compensation

[0147] Let the velocity measured by the DVL be , and the true velocity be , the error model can be expressed as:

[0148] Among them, is the error compensation amount, which is calculated according to the calibration parameters and error model of the DVL.

[0149] 2.3 Seafloor Current Information Preprocessing

[0150] The seafloor current information collected by the seafloor current sensor needs to be smoothed and error estimated.

[0151] 2.3.1 Smoothing

[0152] The sliding average filtering algorithm is used for smoothing. Let the measured value sequence of the seafloor current velocity be , and the filtered seafloor current velocity be , and the length of the sliding window be , then the sliding average filtering formula is:

[0153] Among them, represents the index of the current time step, is the cumulative variable index within the sliding window, used to traverse the current moment and its previous historical sampling points; represents the length of the sliding window.

[0154] 2.3.2 Error Estimation and Correction

[0155] Let the seafloor current velocity measured by the seafloor current sensor be , and the true seafloor current velocity be , the error model can be expressed as:

[0156] Among them, is the error compensation amount, which is estimated and corrected according to the calibration parameters and error model of the seafloor current sensor.

[0157] 2.4 Preprocessing of Observation Information on the Distance to the Beacon

[0158] The distance observation information between the acoustic ranging device and the beacon needs to be compensated for multipath effects and error corrected.

[0159] 2.4.1 Multipath effect compensation

[0160] Adopt a multipath suppression algorithm, such as an adaptive filtering algorithm. Let the received signal be , the desired signal be , the output of the adaptive filter be , the error signal be , then the update formula of the adaptive filtering algorithm is:

[0161] where is the weight vector of the filter, is the step size factor, represents the index of the current time step.

[0162] 2.4.2 Error correction

[0163] Let the measured distance by the acoustic ranging device be , the true distance be , the error model can be expressed as:

[0164] where is the error compensation amount, which is corrected according to the calibration parameters and error model of the acoustic ranging device.

[0165] In this embodiment, the establishment of the state model specifically includes the following steps:

[0166] Define the state vector of the underwater vehicle's motion:

[0167]

[0168] where represents the position of the underwater vehicle in the Cartesian coordinate system; are respectively the velocities of the underwater vehicle in the Cartesian coordinate system , , directions; are successively the roll angle, pitch angle and heading angle of the underwater vehicle;

[0169] Control vector Mainly consider the influence of ocean current information on the motion of the underwater vehicle, and define the control vector of the underwater vehicle's motion:

[0170]

[0171] Among them, are respectively the velocity components of the ocean current in , , directions;

[0172] Define the process noise vector used to describe the state model uncertainty as , which includes the effects of various unmodeled factors and measurement errors. It is usually assumed that is Gaussian white noise with zero mean, and its noise covariance matrix is ;

[0173] Then the state model of the underwater vehicle is expressed as:

[0174]

[0175] Among them, is a non - linear state transition function.

[0176] In this embodiment, the calculation of the non - linear function is specifically as follows:

[0177] According to the kinematic principle of the underwater vehicle, the rate of change of the state vector is described by the following dynamic equation :

[0178]

[0179]

[0180] In the underwater environment, the ocean current will have a significant impact on the speed of the vehicle; in order to more accurately describe the movement of the vehicle, the effect of the ocean current needs to be considered; the actual speed update formula of the vehicle in , , directions is:

[0181]

[0182] The attitude update of the vehicle can be described by the quaternion method or the Euler angle method. Here, the Euler angle method is adopted, and the relationship between the rate of change of the attitude angle and the angular velocity is:

[0183]

[0184] Among them, is the acceleration of the underwater vehicle, measured by the inertial navigation system; , and are the rates of change of the ocean current velocity in , , directions respectively; is the angular velocity of the vehicle, measured by the gyroscope in the inertial navigation system; the above the parameter represents its first derivative with respect to time.

[0185] In this embodiment, the establishment of the observation model specifically includes the following steps:

[0186] Establish an observation equation based on DVL data:

[0187]

[0188] Among them, is the velocity vector measured by DVL, are the DVL measured velocities in , , directions respectively; is the angle deviation of DVL installation (in practical applications, the measurement of DVL may be affected by factors such as installation errors and sound speed changes), are , , the angle deviations of DVL installation in directions respectively, is the rotation matrix; is the velocity vector of the underwater vehicle in the Cartesian coordinate system; is the DVL measurement error quantity, which can be modeled as Gaussian white noise with zero mean, denoted as is the covariance matrix of the DVL measurement error, then the error quantity follows a multivariate normal distribution with mean zero and covariance matrix ;

[0189] Establish an observation equation based on the observed information of the distance to the beacon:

[0190]

[0191] Among them, is the measured distance between the underwater vehicle and the beacon; is the distance between the underwater vehicle and the beacon calculated based on the positions of the underwater vehicle and the beacon; represents the position of the underwater vehicle in the Cartesian coordinate system, represents the position of the beacon as; represents the acoustic ranging error quantity, which can be modeled as Gaussian white noise with zero mean, denoted as is the variance of the acoustic ranging error, then the error quantity obeys a normal distribution with a mean of zero and a variance of ; ;

[0192] Establish an observation equation based on inertial navigation data:

[0193]

[0194]

[0195] where is the acceleration vector measured by the inertial navigation system, are respectively the accelerations measured by the inertial navigation system in the , , directions; is the angular velocity vector measured by the inertial navigation system, are respectively the angular velocities measured by the inertial navigation system in the , , directions; and are respectively the actual underwater vehicle acceleration vector and angular velocity vector; and are respectively the acceleration error and angular velocity error measured by the inertial navigation. Denote and as the covariance matrices of the acceleration error and angular velocity measurement error respectively. Then the error quantity obeys a multivariate normal distribution with a mean of zero and a covariance matrix of ; the error quantity obeys a multivariate normal distribution with a mean of zero and a covariance matrix of ; ;

[0196] Establish an observation equation based on ocean current information:

[0197]

[0198] where is the ocean current velocity vector measured by the ocean current sensor, respectively represent the ocean current velocities measured by the ocean current sensor in the , , directions; is the actual ocean current velocity vector, is , , direction actual ocean current velocity vector; is the measurement error of the ocean current sensor, which can be modeled as Gaussian white noise with zero mean, denoted as the covariance matrix of the measurement error of the ocean current sensor, then the error quantity follows a multivariate normal distribution with a mean of zero and a covariance matrix of ; ;

[0199] By integrating the above observation equations, the observation model of the underwater vehicle is obtained:

[0200] where, is the observation vector; is the state vector, is the observation function; is the observation noise vector, and the noise covariance matrix is .

[0201] In this embodiment, in the multi-source information fusion step, the specific steps of the adaptive Kalman filter algorithm are as follows:

[0202] Initialization: Initialize the state vector and the covariance matrix of the Kalman filter. The state vector contains information such as the position, velocity, and attitude of the underwater vehicle at the initial moment, and can be set according to the initial measurement data or prior knowledge. The covariance matrix represents the uncertainty of the state vector and reflects the credibility of the initial estimate;

[0203]

[0204] where, is the initial position of the underwater vehicle, is the initial velocity of the underwater vehicle, is the initial attitude of the underwater vehicle; is a symmetric matrix, and the diagonal elements of the matrix represent the initial variances of the respective state variables;

[0205] Prediction: Predict the state vector and the covariance matrix of the current k-th moment according to the state model:

[0206] where, is the state transition function, is the state estimate at the (k - 1)-th moment; is the control vector at the k-th moment, which includes the velocity and direction of the ocean current; is the state transition matrix for the state evolving from time k-1 to time k. It describes the change relationship of the state variables between adjacent times and can be obtained by linearizing the state transition function ; is the covariance matrix at time k-1; is the process noise covariance matrix, reflecting the uncertainty of the state model;

[0207] Update: Calculate the predicted value of the observation at the current time k according to the observation model and the observation covariance matrix :

[0208] where is the observation function describing the relationship between the state variables and the observation data; is the observation matrix, which can be obtained by linearizing the observation function ; is the observation noise covariance matrix, reflecting the uncertainty of the observation data;

[0209] Calculate the filtering gain: Calculate the filtering gain :

[0210]

[0211] The filtering gain is used to balance the weights of the predicted value and the observed value in the state update;

[0212] Update the state vector and covariance matrix: Update the state vector and covariance matrix according to the filtering gain:

[0213] where and represent the updated state vector and covariance matrix, is the actual observation vector, is the identity matrix;

[0214] Adaptive adjustment: Real-time evaluate the quality of data from different navigation sensors and adaptively adjust the filtering gain according to the data quality.

[0215] In this embodiment, the real-time evaluation of the quality of data from different navigation sensors and the adaptive adjustment of the filtering gain according to the data quality are as follows:

[0216] Define the sensor data quality evaluation index , which is used to measure the reliability of the data of the th sensor; The calculation factors include the measurement error of the sensor and the stability of the data;

[0217] When calculating the filtering gain, an adaptive adjustment factor negatively correlated with the sensor data quality evaluation index is introduced to adjust the observation noise covariance matrix of the th sensor: where is the adjusted observation noise covariance matrix; when is small, increases, reducing the weight of the data of this sensor in the fusion process; when is large, decreases, increasing the weight of the data of this sensor in the fusion process.

[0218]

[0219] Through this adaptive adjustment mechanism, the advantages of each sensor can be fully utilized in a complex underwater environment, improving the robustness and accuracy of the navigation system. When is small, increases, reducing the weight of the data of this sensor in the fusion process; when is large, when is large, decreases, increasing the weight of the data of this sensor in the fusion process.

[0220] Through this adaptive adjustment mechanism, the advantages of each sensor can be fully utilized in a complex underwater environment, improving the robustness and accuracy of the navigation system.

[0221] In this embodiment, the position estimation and deviation correction steps are specifically as follows:

[0222] After multi-source information fusion processing, the fused state vector is obtained. The state vector contains the position information of the underwater vehicle, expressed as the quantity where , and respectively represent the three-dimensional position estimation values of the underwater vehicle in the Cartesian coordinate system.

[0223] In practical applications, considering the complexity of the underwater environment, the position estimation can be further optimized by considering the influence of ocean currents; denote as the ocean current velocity, as the velocity of the underwater vehicle relative to the seabed, then the actual velocity of the vehicle is:

[0224]

[0225] According to the relationship between velocity and position, within the time interval , the update of the vehicle's position is expressed as:

[0226]

[0227] where, , , The three-dimensional position estimation values of the updated underwater vehicle in the Cartesian coordinate system;

[0228] Compare the estimated position with the preset target position to calculate the deviation and the modulus of the deviation :

[0229]

[0230] ;

[0231] The magnitude of the deviation reflects the degree of deviation between the current position of the underwater vehicle and the target position; Based on the calculated deviation correct the movement of the underwater vehicle so that the vehicle approaches the target position. The correction process can be achieved by adjusting the rudder angle and the propeller speed of the vehicle.

[0232] Establish a control law based on the deviation using proportional-integral-derivative (PID) control , and adjust the rudder angle and the propeller speed of the vehicle through the control law based on the deviation for deviation correction, where is the control input vector of the underwater vehicle, is the rudder angle, is the propeller thrust, , and are the proportional, integral, and derivative gains respectively;

[0233] In practical applications, the dynamic characteristics of the underwater vehicle also need to be considered. Combine the dynamic equation of the underwater vehicle to implement closed-loop control, where is the inertia matrix, is the Coriolis force and centripetal force matrix, is the damping matrix, is the gravity and buoyancy vector, is the attitude vector of the vehicle.

[0234] By solving the above dynamic equation, the motion response of the vehicle under the action of the control input can be obtained, so as to realize the correction of the vehicle position. At the same time, during the entire correction process, position estimation and deviation calculation need to be continuously performed to form a closed-loop control to ensure that the underwater vehicle can accurately reach the target position.

[0235] The present invention provides an underwater vehicle multi-source information fusion precise navigation system, including a processor, a memory and a computer program stored on the memory. When the processor executes the computer program, it specifically executes the steps in any one of the above-mentioned underwater vehicle multi-source information fusion precise navigation methods.

[0236] The above are the preferred embodiments of the present invention. All changes made according to the technical solution of the present invention and whose functional effects do not exceed the scope of the technical solution of the present invention belong to the protection scope of the present invention.

Claims

1. An accurate navigation method for multi-source information fusion of an underwater vehicle, characterized in that, It includes the following steps: Data acquisition: It includes acquiring the inertial navigation data, DVL data, sea current information of the underwater vehicle, and the observation information of the distance to the beacon; Data preprocessing: Preprocess the various acquired data to be used as observation data, including removing noise and calibrating errors; Establish a state model: According to the kinematic principle of the underwater vehicle, establish a state model of the underwater vehicle; the state model includes the state variables of the vehicle and the change rates of the state variables, and takes the speed and direction of the sea current as input parameters of the state model; Establish an observation model: According to the inertial navigation data, DVL data, sea current information, and the observation information of the distance to the beacon, establish an observation model of the underwater vehicle, and the observation model describes the relationship between the state variables and the observation data; Multi-source information fusion: Use the adaptive Kalman filtering algorithm to perform fusion processing on the state model and the observation model, evaluate the quality of different navigation sensor data in real time, and adaptively adjust the filtering gain according to the data quality; Position estimation and deviation correction: According to the state vector after fusion processing, estimate the current position of the underwater vehicle, compare the estimated position with the preset target position, calculate the deviation, and correct the movement of the underwater vehicle according to the deviation.

2. The precise navigation method for multi-source information fusion of an underwater vehicle according to claim 1, wherein In the data acquisition step, the inertial navigation data includes the acceleration and angular velocity of the underwater vehicle, the DVL data includes the speed of the underwater vehicle relative to the seabed, the sea current information includes the speed and direction of the sea current, and the observation information of the distance to the beacon is obtained through an acoustic ranging device.

3. The multi-source information fusion precise navigation method for an underwater vehicle according to claim 1, wherein In the data preprocessing step: For the inertial navigation data, calibrate the accelerometer and gyroscope, and use a filtering algorithm to remove noise; For the DVL data, perform Doppler frequency shift correction and error compensation; For the sea current information, perform smoothing processing and error estimation; For the observation information of the distance to the beacon, perform multipath effect compensation and error correction.

4. The underwater vehicle multi-source information fusion precise navigation method according to claim 1, characterized in that The establishment of the state model specifically includes the following steps: Define the state vector of the underwater vehicle's motion : wherein, represents the position of the underwater vehicle in the Cartesian coordinate system; are respectively the velocities of the underwater vehicle in the Cartesian coordinate system in the directions; are successively the roll angle, pitch angle and heading angle of the underwater vehicle; the superscript T represents transpose; Define the control vector for the motion of the underwater vehicle : Among them, are respectively the velocity components of the ocean current in the directions. Define the process noise vector used to describe the state model uncertainty as , and the noise covariance matrix as ; Then the state model of the underwater vehicle is expressed as: Among them, is a non-linear state transition function.

5. The precise navigation method for multi-source information fusion of an underwater vehicle according to claim 4, characterized in that, Nonlinear function The calculation is as follows: According to the kinematic principle of the underwater vehicle, the rate of change of the state vector is described by the following dynamic equation : Among them, is the acceleration of the underwater vehicle, which is measured by the inertial navigation system; , and are respectively the change rates of the ocean current velocity in the direction; is the angular velocity of the vehicle, which is measured by the gyroscope in the inertial navigation system; the above the parameter represents its first-order derivative with respect to time.

6. The precise navigation method for multi-source information fusion of an underwater vehicle according to claim 5, characterized in that The establishment of the observation model specifically includes the following steps: Establish an observation equation based on the DVL data: Among them, is the velocity vector measured by DVL, are respectively the DVL measured velocities in the directions; is the angle deviation of DVL installation, are respectively the angle deviations of DVL installation in the directions; is the rotation matrix; is the velocity vector of the underwater vehicle in the Cartesian coordinate system; is the DVL measurement error quantity, denoted as the covariance matrix of the DVL measurement error, then the error quantity obeys a multivariate normal distribution with a mean of zero and a covariance matrix of ; ; Establish an observation equation based on the observation information of the distance to the beacon: Among them, is the measured distance between the underwater vehicle and the beacon; is the distance between the underwater vehicle and the beacon calculated based on the positions of the underwater vehicle and the beacon; represents the position of the underwater vehicle in the Cartesian coordinate system, represents the position of the beacon as; represents the acoustic ranging error amount, denoted as is the variance of the acoustic ranging error, then the error amount follows a normal distribution with a mean of zero and a variance of ; ; Establish an observation equation based on the inertial navigation data: Among them, is the acceleration vector measured by the inertial navigation system, are respectively the accelerations measured by the inertial navigation system in the directions of; is the angular velocity vector measured by the inertial navigation system, are respectively the angular velocities measured by the inertial navigation system in the directions of; and are respectively the actual acceleration vector and angular velocity vector of the underwater vehicle; and are respectively the acceleration error and angular velocity error measured by the inertial navigation. Denote and as the covariance matrices of the acceleration error and angular velocity measurement error respectively. Then the error quantity follows a multivariate normal distribution with mean zero and covariance matrix , and the error quantity follows a multivariate normal distribution with mean zero and covariance matrix ; ;​ Establish an observation equation based on the sea current information: Among them, is the ocean current velocity vector measured by the ocean current sensor, respectively represent the ocean current velocities measured by the ocean current sensor in the directions; is the actual ocean current velocity vector, is the actual ocean current velocity vector in the direction; is the measurement error of the ocean current sensor, denoted as the covariance matrix of the measurement error of the ocean current sensor, then the error quantity obeys a multivariate normal distribution with a mean of zero and a covariance matrix of ; ; Integrate the above various observation equations to obtain the observation model of the underwater vehicle: Among them, is the observation vector; is the state vector, is the observation function; is the observation noise vector, and the noise covariance matrix is .

7. The precise navigation method for multi-source information fusion of an underwater vehicle according to claim 6, characterized in that In the multi-source information fusion step, the specific steps of the adaptive Kalman filtering algorithm are as follows: Initialization: Initialize the state vector of the Kalman filter and the covariance matrix ; Among them, is the initial position of the underwater vehicle, is the initial velocity of the underwater vehicle, is the initial attitude of the underwater vehicle; is a symmetric matrix, and the diagonal elements of the matrix respectively represent the initial variances of the state variables; Prediction: The state vector at the current k-th moment predicted according to the state model and covariance matrix : wherein, is the state estimate at time k-1; is the control vector at time k; is the state transition matrix for the state evolving from time k-1 to time k; is the covariance matrix at time k-1; is the process noise covariance matrix; Update: Calculate the predicted value of the current observation at time k according to the observation model and the observation covariance matrix : Among them, is an observation function that describes the relationship between state variables and observed data; is an observation matrix; is an observation noise covariance matrix; Calculate the filtering gain: Calculate the filtering gain : Update the state vector and covariance matrix: Update the state vector and covariance matrix according to the filtering gain: Among them, and represent the updated state vector and covariance matrix, is the actual observation vector, is the identity matrix; Adaptive adjustment: Evaluate the quality of different navigation sensor data in real time, and adaptively adjust the filtering gain according to the data quality.

8. The underwater vehicle multi-source information fusion precise navigation method according to claim 7, characterized in that The real-time evaluation of the quality of different navigation sensor data and the adaptive adjustment of the filtering gain according to the data quality are specifically as follows: Define the sensor data quality assessment index , which is used to measure the reliability of the th sensor data; The calculation factors of include the measurement error of the sensor and the stability of the data; When calculating the filtering gain, an adaptive adjustment factor negatively correlated with the sensor data quality evaluation index is introduced to adjust the observation noise covariance matrix of the th sensor: is the adjusted observation noise covariance matrix; when is small, increases, reducing the weight of the data from this sensor in the fusion process; when is large, decreases, increasing the weight of the data from this sensor in the fusion process.

9. The precise navigation method for multi-source information fusion of an underwater vehicle according to claim 1, characterized in that The specific steps of the position estimation and deviation correction step are as follows: According to the state vector after fusion processing Estimate the current position of the underwater vehicle, and optimize the position estimate by combining the influence of ocean currents through the following formula; where , 、 and respectively represent the three-dimensional position estimate values of the underwater vehicle in the Cartesian coordinate system; The actual speed of the vehicle Expressed as: Among them, represents the speed of the underwater vehicle relative to the seabed, represents the ocean current speed; Based on the relationship between speed and position, within the time interval , the update of the vehicle position is expressed as: Among them, , , are the three-dimensional position estimated values of the updated underwater vehicle in the Cartesian coordinate system respectively; Compare the estimated position with a preset target position to calculate the deviation and the modulus of the deviation : ； Based on the deviation-based control law Adjust the rudder angle and thruster speed of the vehicle for deviation correction, where is the control input vector of the underwater vehicle, is the rudder angle, is the thruster thrust, , and are the proportional, integral and derivative gains respectively; Combined with the underwater vehicle dynamics equation at the same time to achieve closed-loop control, where is the inertia matrix, is the Coriolis force and centripetal force matrix, is the damping matrix, is the gravity and buoyancy vector, is the attitude vector of the vehicle.

10. An underwater vehicle multi-source information fusion precise navigation system, characterized in that, It includes a processor, a memory, and a computer program stored on the memory. When the processor executes the computer program, it specifically executes the steps in the multi-source information fusion precise navigation method for an underwater vehicle according to any one of claims 1-9.

Images