This application is the national stage entry of International Application No. PCT/CN2019/077887, filed on Mar. 12, 2019, which is based upon and claims priority to Chinese Patent Application No. 201810647904.1, filed on Jun. 22, 2018, the entire contents of which are incorporated herein by reference.
The present invention belongs to the field of navigation technologies, and relates to a navigation and positioning method of an underwater glider, and specifically to an up floating error correction method of navigation system of an underwater glider. The method enables up floating error correction of the underwater glider to still satisfy navigation requirements under the extremely bad conditions, improves robustness and adaptability of the navigation system, and finally achieves accurate positioning of the underwater glider.
An underwater glider is an important tool for ocean exploration and resource development. Motion of the underwater glider is not intensive during a working process, so use of a micro-electro-mechanical system (MEMS) inertial measurement unit (IMU) with low costs, low power consumption, and long endurance is sufficient to meet requirements of navigation and positioning. However, long-time underwater work of a glider and the interference conditions (including ocean current surges, magnetic interference, and the like) will produce large cumulative positioning error. Therefore, the glider needs to float up to the water surface, and use high-precision global positioning system (GPS) signals and a data fusion algorithm to correct errors for its positioning. However, in an up floating error correction process, under the impact of factors such as inclement weather outside, fluctuations of sea waves, and ship shielding, navigation precision of a multi-sensor data fusion algorithm is reduced, and positioning diverges or even fails.
Therefore, how to ensure stability and accuracy of up floating error correction of an underwater glider under the extremely bad conditions has become an important issue in accurate positioning of an underwater glider.
Invention objective: For a problem that a glider in an up floating error correction state may be affected by factors such as inclement weather outside, fluctuations of sea waves, and ship shielding, an objective of the present invention is to provide a navigation and positioning system for an underwater glider and an up floating error correction method, to ensure robustness and adaptability of navigation and positioning of the glider by using a multi-sensor data fusion algorithm, to overcome the problem that an underwater glider cannot continuously normally work because of reduction of navigation precision and divergence of positioning caused by a conventional integrated navigation algorithm during up floating error correction of the underwater glider under extremely severe conditions, thereby achieving accurate positioning of the underwater glider.
Technical solution: To achieve the foregoing objective, the following technical solutions are used in the present invention:
A navigation and positioning system for an underwater glider is provided, including 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: The MEMS-IMU integrates a triaxial accelerometer and a triaxial gyroscope, and triaxial acceleration and angular rate information are output for obtaining navigation information including an attitude, a velocity, and a location of an underwater glider by using an inertial navigation algorithm; the triaxial magnetometer is configured to correct a heading information error of the glider, and the DVI, is configured to determine a motion state of the underwater glider; the GPS receiving module is combined with the MEWS-PAU to form a loose integrated navigation system for correction of GPS/INS location and velocity errors in an up floating error correction operation, and is configured to correct velocity and location errors of the glider by using an integrated navigation filter algorithm; and the integrated navigation hardware processing system is configured to receive, process, and perform clock synchronization on signals from the triaxial magnetometer, the GPS receiving module, the MEMS-IMU, and the DVL, and perform algorithm calculation of integrated navigation multi-sensor information.
Preferably, the navigation and positioning system for an underwater glider has two operating states: an underwater working state and an up floating error correction state; if a navigation and positioning error is too large, the 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 glider switches from the up floating error correction state to the underwater working state and continues to work.
Preferably, for underwater operation, signals output by the MEMS-IMU and the triaxial magnetometer are used for performing navigation and positioning of the glider by using a quaternion algorithm based on complementary filtering.
Preferably, for up floating error correction, signals output by the GPS receiving module and the MEMS-IMU are used for performing navigation and positioning of the glider by using the loose integrated navigation system for correction of GPS/INS location and velocity errors to perform navigation and positioning of the glider, where an Ha) Kalman filter algorithm based on an adaptive multiple fading factor is used as the integrated navigation filter algorithm: Pk|k-1=SkΦk|k-1Pk-1Φk|5−1TSkT+Qk-1; and an H∞ filter gain matrix is Kk=Pk|k-1HkT(HkPk|k-1HkT+I)−1, and an H∞ filter state optimal covariance matrix is
where k and k-1 represent a current moment and a previous moment respectively, Φk|k-1 is a state-transition matrix, 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.
Preferably, the adaptive multiple fading factor matrix Sk=diag (s1, s2, s3 . . . , sn) is calculated by using the following formula:
Preferably, the adaptive threshold is γ=η·γa, where
γa=ρ(LkTLk(Pk−1+HkTHk)−1), Trace( ) represents a matrix trace operation, and ρ( ) represents a spectral radius of a matrix.
An up floating error correction method for a navigation and positioning system for an underwater glider is provided, including the following steps:
Beneficial effects: In the present invention, the navigation and positioning system for an underwater glider completes calculation of an attitude, a velocity, and a location of an underwater glider by using a MEMS-IMU and a triaxial magnetometer that have low costs when the glider is in an underwater working state, and performs multi-sensor data fusion by using a high-precision GPS module and the MEMS-IMU, to reduce a navigation and positioning error of the glider, when the glider is in an up floating error correction state. Switching conditions and requirements of the two states of an underwater glider can be set by software and hardware according to specific work and tasks. Because during an up floating error correction process of the glider, impact from external indeterminacy on the process is unknown, and in addition, statistic characteristics of noise and a system model are time-varying, an Ha) Kalman filter algorithm based on an adaptive multiple fading factor is used in the present invention, and the present invention is adaptive to a multi-sensor data fusion algorithm in a time-varying condition, and can ensure stability and precision of up floating error correction to satisfy requirements. The present invention overcomes the problem that an underwater glider cannot continuously normally work because of reduction of navigation precision and divergence of positioning caused by a conventional integrated navigation algorithm during up floating error correction of the underwater glider under extremely severe conditions, thereby finally achieving accurate positioning of the underwater glider.
The present invention is further described with reference to the accompanying drawings and specific embodiments:
As shown in FIG. the present invention discloses a navigation and positioning system for an underwater glider, including a micro-electro-mechanical system inertial measurement unit (MEMS-IMU), a global positioning system (GPS) receiving module, a triaxial magnetometer, a Doppler velocimeter (DVL), a digital signal processing (DSP) module, and an advanced reduced instruction set microprocessor (ARM).
The MEMS-IMU integrates a triaxial accelerometer and a triaxial gyroscope, and triaxial acceleration and angular rate information are output for obtaining navigation information including an attitude, a velocity, and a location of an underwater glider by using an inertial navigation algorithm. In addition, during up floating error correction, data fusion is performed on the navigation information and a GPS signal, to correct a navigation and positioning error.
The triaxial magnetometer is used for a complementary filter algorithm, to correct a heading information error of the glider. In addition, if a heading variation is too large, reflecting strong magnetic interference, an up floating error correction operation needs to be performed.
The DVL is configured to determine a motion state of the underwater glider. If a velocity variation is too large, reflecting that the glider is severely affected by ocean current surges, the up floating error correction operation also needs to be performed.
The GPS receiving module is used for fusion with IMU data by using the designed Hoc Kalman filter algorithm based on an adaptive multiple fading factor during the up floating error correction operation of the glider, to correct velocity and location errors of the glider.
The ARM and the DSP constitute an integrated navigation hardware processing system of the underwater glider. In an underwater working state, the ARM is configured to receive, process, and perform clock synchronization on signals from the magnetometer, the IMU, and the DVL. The DSP is used for calculation of a quaternion algorithm of inertial navigation. In an up floating error correction state, the ARM is configured to receive, process, and perform clock synchronization on signals from the GPS receiving module and the IMU. The DSP is configured to perform calculation of the H∞ Kalman filter algorithm based on an adaptive multiple fading factor.
The present invention discloses an up floating error correction method for a navigation and positioning system for an underwater glider, including the following steps:
(1) Calculate an attitude, a velocity, and a location of an underwater glider by using a quaternion algorithm based on complementary filtering if a navigation and positioning system for the underwater glider starts a MEMS-IMU and a magnetometer module when the system is in an underwater working state. For a basic quaternion algorithm, refer to the book “Inertial Navigation” edited by Professor Qin Yongyuan, and for the complementary filter theory, refer to the invention patent written by Professor Chen Xiyuan, “COMPLEMENTARY FILTERING-BASED UNDERWATER GLIDER ENERGY SAVING ALGORITHM”.
(2) Determine whether the underwater glider needs to perform an up floating error correction operation. Motion of the underwater glider is not intensive during a working process. If a heading variation is too large (strong magnetic interference), a velocity variation is too large (large ocean current surges), or it is manually determined that a navigation and positioning error of the glider is too large, the glider floats up to a water surface, to perform the up floating error correction operation.
(3) Receive, after the glider floats up to the water surface, a longitude and latitude signal, an altitude signal, and a triaxial velocity signal from a GPS receiving module, and a triaxial acceleration signal and a triaxial angular velocity signal from an IMU by using an ARM+DSP integrated navigation hardware processing system. The DSP performs data fusion on the received multi-sensor signals. On the basis of ensuring reliability and precision of the MEMS-IMU and the GPS receiving module, a designed H∞ Kalman filter algorithm based on an adaptive multiple fading factor is used. The algorithm is applied to a loose integrated navigation system for correction of GPS/INS location and velocity errors shown in
In the up floating error correction, a flowchart of an adopted H∞ Kalman filter algorithm based on an adaptive multiple fading factor is shown in
{circle around (1)}. Establish an adaptive fading factor
An integrated navigation system is considered as a linear dynamic system:
Xk=Φk|k-1Xk-1±Wk-1
Zk=Hkxk+vk-1
For a fading factor-based Kalman filter algorithm, refer to “ADAPTIVE ESTIMATION OF MULTIPLE FADING FACTORS IN KALMAN FILTER FOR NAVIGATION APPLICATIONS” by Yanrui Geng. The specific adaptive fading factor-based Kalman filter equation is written as:
Xk=Φk|k-1Xk-1
Pk|k-1=SkΦk|k-1Pk-1Φk|k-1TSkT+Qk-1
Kk=Pk|k-1HkT(HkPk|k-1HkT+R1
Vk=Zk−HkXk|k-1
Xk=Xk|k-1+KkVk
{circle around (2)}. Adaptive H∞ Kalman filter algorithm
A linear discrete system is considered:
Therefore, a constructed construction cost function J is as follows:
However, because a closed solution to an H∞ optimal estimation problem can only be obtained in some special cases, a design of H∞ suboptimal filtering is usually considered. That is, a threshold γ that is sufficiently close to γ0 is given provided that an H∞ norm of a constructed algebraic function is smaller than γ.
In a case of full rank of Φk|k-1, sufficient and necessary conditions for existence of a solution to the H∞ suboptimal problem can be given by using a Riccati inequality:
Pk−1+HkT−γ−2LkTLk>0
It should be noted that robustness of a filter is related to a selected threshold γ. If the threshold γ is smaller and closer to γ0, robustness of the filter is stronger. However, γ should not be smaller than γ0. Otherwise, there is no solution to the H∞ suboptimal problem, resulting in filter divergence. In addition, if γ approaches infinity, an H∞ filter degenerates to a conventional Kalman filter. A conventional threshold γ is usually determined according to actual working experience of a glider and cannot be changed. As a result, filtering effects are more conservative. It cannot be guaranteed that an estimated error of a system is relatively small while having high robustness. Therefore, if a value of the threshold γ is adaptive to different up floating error correction environments of the underwater glider, navigation and positioning precision of the up floating error correction can be further improved while guaranteeing robustness. Therefore, an adaptive algorithm of the threshold γ is established according to the Riccati inequality as follows:
γ2>ρ(LkTLk(Pk−1+HkTHk)−1)
Ideally, an innovation matrix of Kalman filtering is Vk, ˜N (0, HkPk|k-1HkT+Rk-1). However, impact of system indeterminacy leads to an abnormal observation variable, causing a filter to be out of order. Such a case may cause a variation of VkTVk of a sum of squares of an innovation sequence. When VkTVk is relatively large, reflecting that interference of indeterminacy has large impact on the system, in this case, robustness of the system should be increased (the threshold γ should be reduced). On the contrary, when VkTVk is relatively small, reflecting that the interference is relatively small, in this case, more attention should be paid to navigation and positioning precision of the system (the threshold γ should be increased). The adaptive threshold γ is: γ=η·γa,
Differences from a standard Kalman filter equation are as follows:
{circle around (3)}. H∞ Kalman filter algorithm based on an adaptive multiple fading factor
Algorithms in steps {circle around (1)} and {circle around (2)} are integrated to form an overall integrated navigation algorithm for up floating error correction of an underwater glider (an H∞ Kalman filter algorithm based on an adaptive multiple fading factor). A flowchart of the algorithm is shown in
i) First, perform a time update: predict a state variable Xk|k-1 of the current moment in one step according to an optimal state variable estimate Xk-1 that is filtered and output at the previous moment, and predict a state prediction covariance Pk|k-1 of the current moment according to both an optimal covariance estimation matrix Pk-1 that is filtered and output at the previous moment and a fading factor matrix Sk output by a fading loop.
ii) Second, perform a measurement update: calculate a gain matrix Kk according to the state prediction covariance Pk|k-1 of the current moment, update a system optimal state variable estimate Xk by using the gain matrix Kk and the state variable Xk|k-1 predicted in one step after receiving a new external observation variable, and then, calculate an H∞ filter state optimal covariance Pk after calculating an H∞ threshold γ according to Pk|k-1 and a time-varying factor γa.
iii) Finally, perform a fading factor update: calculate the adaptive multiple fading factor matrix Sk after updating a transition matrix of the fading loop according to the H∞ filter state optimal covariance Pk.
(4) Reduce, by using an H∞ Kalman filter algorithm based on an adaptive multiple fading factor, velocity and location errors of the integrated navigation system gradually to approach zero. If the errors are smaller than specified thresholds, indicating that the up floating error correction is completed, the glider can switch to an underwater working state.
Number | Date | Country | Kind |
---|---|---|---|
201810647904.1 | Jun 2018 | CN | national |
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/CN2019/077887 | 3/12/2019 | WO |
Publishing Document | Publishing Date | Country | Kind |
---|---|---|---|
WO2019/242336 | 12/26/2019 | WO | A |
Number | Name | Date | Kind |
---|---|---|---|
11193772 | Kinnaman | Dec 2021 | B1 |
20050251328 | Merwe et al. | Nov 2005 | A1 |
Number | Date | Country |
---|---|---|
1974318 | Jun 2007 | CN |
102519450 | Jun 2012 | CN |
202382747 | Aug 2012 | CN |
106767792 | May 2017 | CN |
109253726 | Jan 2019 | CN |
Entry |
---|
Machine Translation of CN102519450A by Chen et al. (Year: 2012). |
Jiang, C.; Zhang, S.-B.; Zhang, Q.-Z. Adaptive Estimation of Multiple Fading Factors for GPS/INS Integrated Navigation Systems. Sensors 2017, 17, 1254. (Year: 2017). |
C. M. De Angelis and J. E. Whitney, “Adaptive calibration of an autonomous underwater vehicle navigation system,” Oceans 2000 MTS/IEEE Conference and Exhibition. Conference Proceedings (Cat. No.00CH37158), Providence, RI, USA, 2000, pp. 1273-1275 vol. 2, doi: 10.1109/OCEANS.2000.881777. (Year: 2000). |
S. H. Kwak, J. B. McKeon, J. R. Clynch and R. B. McGhee, “Incorporation of global positioning system into autonomous underwater vehicle navigation,” Proceedings of the 1992 Symposium on Autonomous Underwater Vehicle Technology, Washington, DC, USA, 1992, pp. 291-297, doi: 10.1109/AUV.1992.225224. (Year: 1992). |
Yanrui Geng et al., Adaptive estimation of multiple fading factors in Kalman filter for navigation applications, GPS Solut, 2008, pp. 273-279, 12. |
Yongyuan Qin, Inertial Navigation, May 2016. |
Number | Date | Country | |
---|---|---|---|
20210041240 A1 | Feb 2021 | US |