| CPC G01C 21/165 (2013.01) [B63B 79/10 (2020.01); B63G 8/001 (2013.01); G01C 21/20 (2013.01); G01S 19/49 (2013.01); B63B 2213/00 (2013.01); B63G 2008/004 (2013.01)] | 4 Claims |
|
1. A navigation and positioning system for an underwater glider, comprising a micro-electro-mechanical system inertial measurement unit (MEMS-IMU), a global positioning system (GPS) receiving module, a triaxial magnetometer, a Doppler velocimeter (DVL), and an integrated navigation hardware processing system, wherein
the MEMS-IMU integrates a triaxial accelerometer and a triaxial gyroscope, and triaxial acceleration and angular rate information are output for obtaining navigation information comprising an attitude, a velocity, and a location of the underwater glider by using an inertial navigation algorithm;
the triaxial magnetometer is configured to correct a heading information error of the underwater glider, and the DVL is configured to determine a motion state of the underwater glider;
the GPS receiving module is combined with the MEMS-IMU to form an integrated navigation system for a correction of GPS/inertial navigation system (INS) location and velocity errors in an up floating error correction operation, and the GPS receiving module is configured to correct velocity and location errors of the underwater glider by using an integrated navigation filter algorithm; and
the integrated navigation hardware processing system is configured to receive, process, and perform a clock synchronization on signals from the triaxial magnetometer, the GPS receiving module, the MEMS-IMU, and the DVL, and perform an algorithm calculation of integrated navigation multi-sensor information; and
wherein further, the navigation and positioning system has an underwater working state and an up floating error correction state;
if a navigation and positioning error is large, the underwater glider stops working in real time and switches from the underwater working state to the up floating error correction state; and
when velocity and location errors of a GPS/INS integrated navigation system are smaller than specified thresholds, the underwater glider switches from the up floating error correction state to the underwater working state and continues to works,
wherein for up floating error correction, signals output by the GPS receiving module and the MEMS-IMU are used for performing a navigation and a positioning of the underwater glider by using the integrated navigation system for the correction of the GPS/INS location and the velocity errors, wherein an H∞ Kalman filter algorithm based on an adaptive multiple fading factor is used as the integrated navigation filter algorithm; and
a state prediction covariance matrix of the H∞ Kalman filter algorithm is Pk|k-1=SkΦk|k-1Pk-1Φk|5−1TSkT+Qk-1,
an H∞ filter gain matrix is Kk=Pk|k-1HkT(HkPk|k-1HkT+I)−1, and
an H∞ filter state optimal covariance matrix is
 wherein k and k-1 represent a current moment and a previous moment respectively, Φk|k-1 is a state-transition matrix, Hk is an observation matrix, Qk-1 is a system noise covariance, Sk is an adaptive multiple fading factor matrix, Lk is an estimate of a linear combination of system state variables, λi is a unit matrix,
 and γ is an adaptive threshold, and
wherein the adaptive multiple fading factor matrix Sk=diag (s1, s2, s3 . . . , sn) is calculated by using the following formula:
 wherein λi is an ith observation element of the observation matrix Hk jii (k) is an ith diagonal element of a matrix Jk=Φk|k-1Pk-1Φk|k-1T εi is a threshold of a Chi-square test, vi (k) is an ith diagonal element of an innovation matrix Vk and bii (k) is an ith diagonal element of a matrix Bk=HkQk-1HkT+Rk
wherein the adaptive threshold is γ=ƒ·γa, wherein
 γa=ρ(LkTLk(βk−1+HkTHk)−1) Trace( ) represents a matrix trace operation, and ρ( ) represents a spectral radius of a matrix.
|