Navigation method of aircraft in water-air multi-medium environment

Abstract

The invention discloses a navigation method for an aircraft in a water-air multi-medium environment, which is based on a water-air cross-medium integrated navigation system consisting of an IMU inertial navigation device, a GNSS receiver, a magnetometer, a water depth pressure sensor and DVL equipment, and comprises the following steps: firstly, determining a state equation of the navigation system, and then determining a measurement equation of the integrated navigation system; seamless switching between an overwater navigation mode and an underwater navigation mode is realized; according to the invention, an IMU inertial navigation device is taken as a core, a GNSS receiver is combined to provide pseudo-range position observation when used on water, a magnetometer provides course information, a water depth pressure sensor provides depth observation, and DVL equipment is used underwater to provide speed observation, so that the navigation problem in a water-air multi-medium environment is mainly solved; the system has the functions of acquiring navigation information on water and underwater and realizing switching between the water mode and the underwater mode.

Description

Technical Field

[0001] The present invention belongs to the technical field of complex environment navigation, and in particular relates to a navigation method for an aircraft in a water-air multi-media environment. Background Art

[0002] With the development of science and technology, aircraft, ships, submarines and other aircraft have emerged in the fields of aviation and navigation, each playing an important role in a specific field. However, most of the existing aircraft can only operate in a single medium environment, and aircraft that can adapt to multiple medium environments is one of the future development directions.

[0003] Cross-media vehicles, that is, vehicles that can operate in two or more media (such as water and air), are valued for their strong adaptability. Technological limitations in the 20th century slowed the development of cross-media vehicles, but technological advances in the 21st century, such as energy power and material technology, have made it possible to develop cross-media vehicles. The key technologies of cross-media vehicles include shape design, propulsion, flight control, communication, and guidance and navigation. In particular, cross-media navigation technology, due to the huge difference in the physical properties of water and air, makes the navigation rules inconsistent, and the dynamic characteristics change dramatically during the cross-media process, and information transmission may be delayed or distorted. These factors have brought challenges to navigation technology.

[0004] In order to solve these problems, researchers have proposed a combined navigation solution that combines multiple technologies such as inertial navigation, satellite navigation, and acoustic navigation to improve navigation accuracy and stability. Inertial navigation is suitable for all-weather autonomous navigation, but the error diverges over time; satellite navigation has high accuracy, but the signal has difficulty penetrating the water surface; and acoustic navigation is suitable for underwater. Therefore, combining these technologies and selecting appropriate combination solutions according to different media environments is an effective way to achieve navigation in cross-media environments. Summary of the invention

[0005] The purpose of the present invention is to overcome the deficiencies of the prior art and to provide a navigation method for an aircraft in a water-air multimedia environment with inertial navigation as the core and supplemented by other navigation technologies.

[0006] In order to achieve the above-mentioned purpose, the technical solution adopted by the present invention to solve the technical problem is: a navigation method for a vehicle in a water-air multi-media environment, based on an IMU inertial navigation device as a core, a GNSS receiver for providing pseudo-range position observation on water, a magnetometer for providing heading information, a water depth pressure sensor for providing depth observation, and a DVL device for providing speed observation underwater, which is composed of a water-air cross-media integrated navigation system, comprising the following steps:

[0007] S1, determine the state equation of the navigation system X(t) = F(t)X(t) + G(t)W(t), select the attitude error, velocity error, position error, gyro drift and accelerometer drift of the IMU inertial navigation device to form a 15-dimensional state vector:

[0008] The system state transfer matrix at time t is:

[0009] Where F I (t), F C (t), F ε (t) are:

[0010]

[0011] Where α g =diag(1 / τ gx 1 / τ gy 1 / τ gz ), α a =diag(1 / τ ax 1 / τ ay 1 / τ az ), where 1 / τ represents the Markov time-correlated constant; the system noise W(t) includes the zero bias and white noise of the three-axis gyroscope and three-axis accelerometer of the IMU inertial navigation device:

[0012] W(t)=[w gεx w gεy w gεz w aεx w aεy w aεz η gx η gy η gz η ax η ay η az ] T ,

[0013] The system noise allocation matrix is:

[0014] S2, then determine the measurement equation of the integrated navigation system; use the position information and speed information of the GNSS receiver, the heading information of the magnetometer and the depth information obtained by the water depth pressure sensor when underwater as observation values, and the measurement equation is as follows: Z(t) = H(t)X(t) + R(t); discuss the composition of the measurement equation in two states: above water and underwater;

[0015] S21, when the vehicle is on the water, the GNSS receiver and magnetometer provide measurement information. In the formula They are the speed of INS solution and the speed of GNSS solution respectively. are the position solved by INS and the position provided by GNSS, h b is the magnetic field measurement vector, e is the north unit vector, and the measurement matrix is where h n is the real magnetic field vector;

[0016] S22: When the vehicle is underwater, the GNSS receiver data is invalid. The system uses the underwater depth information and the magnetometer information that can still be used normally underwater as measurement. In the formula They are the height calculated by INS and the height calculated by water depth pressure sensor, and the measurement matrix becomes If we consider introducing DVL equipment for underwater velocity observation, we have In the formula are the speed calculated by INS and the speed measured by DVL equipment respectively. The measurement matrix at this time is:

[0017] S3, with a preset specific state threshold as the switching condition, determines whether the switching condition is met by monitoring the navigation information obtained by the inertial sensor of the IMU inertial navigation device and the motion state of the carrier, and switches from the water navigation mode to the underwater navigation mode, or from the underwater navigation mode to the water navigation mode, to achieve seamless switching between the water and underwater navigation modes.

[0018] Further, the switching process from the surface navigation mode to the underwater navigation mode is as follows: determine whether the vehicle is in a descending state based on speed detection, acceleration detection or / and attitude detection, and use this state as a pre-judgment condition for mode switching; otherwise enter the surface navigation mode; when the vehicle has a continuous downward speed or acceleration in the vertical direction and is in a diving attitude, the pre-judgment condition is met and the vehicle is in a descending state; monitor the changes in external pressure to which the water depth pressure sensor is sensitive as a confirmation switching condition; otherwise enter the surface navigation mode: if the vehicle does not meet the pre-judgment condition and the confirmation switching condition during operation, the surface navigation mode is maintained; if yes, enter the underwater navigation mode: when the water depth pressure sensor contacts the water surface, the pressure value changes dramatically, confirming that the switching condition is met, and the navigation system quickly shields the external GNSS or UWB signal to enter the underwater navigation mode.

[0019] Further, the switching process from the underwater navigation mode to the surface navigation mode is: judging whether the vehicle is in a floating state according to speed detection, acceleration detection and / or attitude detection; otherwise, entering the underwater navigation mode, and if so, entering the surface navigation state; combining with pressure detection, judging whether the pressure is atmospheric pressure for a period of time; otherwise, entering the underwater navigation mode, and if so, entering the surface navigation mode.

[0020] Furthermore, when switching, the aircraft first uses a high-pass filter and a low-pass filter to process the original data measured by the three-axis gyroscope and the three-axis accelerometer, and then uses an adaptive Mahony complementary filtering method to compensate for the impact acceleration of the inertial data measured by the inertial sensor, and applies the adaptive PI parameter adjustment method to the complementary filter to realize the adaptive Mahony complementary filtering:

[0021] The quaternion of the adaptive Mahony complementary filter can be written as Where P(·) is the rotation vector, δ is the new information, and w is the gyroscope measurement value. is the accelerometer measurement value, e is the error between the predicted value and the measured value, k P With k i is the control parameter of the PI regulator, and d represents the quaternion attitude matrix.

[0022] Furthermore, the adaptive Mahony complementary filtering method includes prediction and correction. Prediction is to solve the predicted attitude angle using the angular velocity measurement value, and correction is to correct the predicted attitude angle using the acceleration measurement value. The present invention uses the average measurement value of the three-axis accelerometer for nearly 20 epochs to adjust the PI regulator parameters:

[0023] a) Get the three-axis gyroscope measurement value w and the three-axis accelerometer measurement value of the last epoch And the quaternion q n-1 ;

[0024] b) Using the measurements of the triaxial accelerometer over the last 20 epochs Determine the PI regulator parameter k P and k i ;

[0025] c) Measurements of the three-axis accelerometer Perform normalization;

[0026] d) Get the estimate of the Earth's gravity vector from the quaternion attitude matrix d

[0027] e) Calculate the error vector

[0028] f) The integral term I over the previous epochn-1 The error vector e of the current epoch is filtered and then the integral term I is calculated n =I n-1 +k i ·Δt·e, Δt is the sampling period;

[0029] g) Calculate the update factor δ = k P e+I n ;

[0030] h) Calculate the compensated three-axis gyroscope measurement value w'=w+δ;

[0031] i) Use the quaternion q of the last epoch n-1 and the compensated gyroscope value w' to update the quaternion:

[0032]

[0033] j) for q n Perform normalization processing and calculate the attitude angles γ and θ respectively;

[0034] Repeat the above steps to obtain the updated value of the attitude angle in real time.

[0035] Furthermore, the shock acceleration compensation and the adaptive Mahony complementary filter are integrated through the following steps:

[0036] The first step is to convert the current measurement value of the three-axis accelerometer Compared with the local Earth gravity acceleration, if it satisfies There is no external shock acceleration; is a preset acceleration threshold based on the noise level of the tri-axis accelerometer;

[0037] Step 2: If the This means that the spacecraft is in the process of crossing the medium and is subjected to a large impact acceleration. At this time, the accelerometer measurement value contains the impact acceleration component, so the attitude estimation cannot be performed directly, so it needs to be compensated to a certain extent.

[0038] The third step is to compensate the three-axis accelerometer measurement value according to the motion of the spacecraft during the cross-medium process. The compensation calculation expression is: Where β is the compensation coefficient set according to the motion of the spacecraft across the medium, which is determined by the magnitude of the impact acceleration and the measurement accuracy of the inertial sensor used;

[0039] The fourth step is to perform adaptive Mahony complementary filter integration on the compensated acceleration and the angular velocity measured by the gyroscope to calculate the real-time attitude angle.

[0040] The beneficial effects of the present invention are as follows: the navigation method of the present invention takes the IMU inertial navigation device as the core, combines the GNSS receiver for use on water to provide pseudo-range position observation, the magnetometer for providing heading information, the water depth pressure sensor for providing depth observation, and the DVL device for use underwater to provide speed observation, mainly responds to navigation problems in a water-air multi-media environment, and has the functions of obtaining navigation information on and under water and realizing switching between on and under water modes. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 The overall framework diagram of the navigation system of the present invention is shown in FIG.

[0042] Figure 2 This is a flow chart of switching from the surface to the underwater mode of the present invention;

[0043] Figure 3 This is a flow chart of the algorithm for switching from underwater to above-water mode of the present invention;

[0044] Figure 4 This is a flow chart of the adaptive Mahony complementary filtering integration method of the present invention. DETAILED DESCRIPTION

[0045] The present invention is further described as follows with reference to the accompanying drawings and embodiments:

[0046] The present invention studies the navigation problem in a cross-medium environment of water and air, and designs a cross-medium combined navigation system with inertial navigation as the core, which is combined with different other navigation technologies on the water and underwater. Specifically, a GNSS receiver is used on the water to provide pseudo-range position observation, a DVL device is used underwater to provide speed observation, and a water depth pressure sensor is used to provide depth observation. At the same time, a magnetometer is used throughout the process to provide heading information, and a mode switching algorithm is used to complete the switching of the combined navigation modes on the water and underwater. In summary, Figure 1 As shown, the specific steps are as follows.

[0047] The present invention adopts a loosely combined data fusion method, uses navigation error parameters as state variables, and designs an error state Kalman filter.

[0048] S1, first determine the state equation of the navigation system X(t)=F(t)X(t)+G(t)W(t).

[0049] The present invention selects the attitude error, velocity error, position error, gyro drift and triaxial accelerometer drift of the IMU inertial navigation device as state variables, which together constitute a 15-dimensional state vector:

[0050]

[0051] The system state transfer matrix at time t is: Where F I(t), F C (t), F ε (t) are:

[0052]

[0053] Where α g =diag(1 / τ gx 1 / τ gy 1 / τ gz ), α a =diag(1 / τ ax 1 / τ ay 1 / τ az ), where 1 / τ represents the Markov time correlation constant.

[0054] The system noise W(t) includes the zero bias and white noise of the three-axis gyroscope and three-axis accelerometer of the IMU inertial navigation device, that is: W(t) = [w gεx w gεy w gεz w aεx w aεy w aεz η gx η gy η gz η ax η ay η az ] T ,

[0055] The system noise allocation matrix is:

[0056] S2, then determine the measurement equation of the integrated navigation system.

[0057] The water-to-air cross-medium integrated navigation system of the present invention mainly uses the position information and speed information of the GNSS receiver, the heading information of the magnetometer and the depth information obtained by the water depth pressure sensor when underwater as observation values. The measurement equation is as follows: Z(t)=H(t)X(t)+R(t); the composition of the measurement equation is discussed in two states: above water and underwater.

[0058] S21, when the carrier, i.e. the aircraft, is on the water, the GNSS receiver and magnetometer provide measurement information. In the formula are the speeds solved by INS and GNSS respectively, are the position solved by INS and the position provided by GNSS, h b is the magnetic field measurement vector, e is the north unit vector, and the measurement matrix is where h n is the real magnetic field vector.

[0059] S22, when the vehicle is underwater, the GNSS receiver data fails, the system uses the underwater depth information and the magnetometer information that can still be used normally underwater as measurement. In the formula They are the height calculated by INS and the height calculated by water depth pressure sensor, and the measurement matrix becomes If we consider introducing DVL equipment for underwater velocity observation, we have In the formula are the speed calculated by INS and the speed measured by DVL equipment respectively. The measurement matrix at this time is:

[0060] S3, for the above-mentioned different measurement equations on the water and underwater, a mode switching algorithm needs to be designed to achieve seamless switching between the water and underwater navigation modes. The mode switching algorithm uses a preset specific state threshold as the switching condition, and determines whether the switching condition is met by monitoring the navigation information obtained by the sensor and the motion state of the carrier. If the condition is met, the navigation mode is changed in time.

[0061] The switching process from surface navigation mode to underwater navigation mode is as follows: Figure 2 The mode switching algorithm monitors the motion state of the aircraft in real time, determines whether the aircraft is in a descending state based on speed detection, acceleration detection and / or attitude detection, and uses this state as a pre-judgment condition for mode switching.

[0062] Otherwise enter water navigation mode.

[0063] When the vehicle has a continuous downward speed or acceleration in the vertical direction and is in a dive posture, the prediction condition is met. At this time, combined with pressure detection, the switching condition is confirmed by monitoring the changes in external pressure to which the water depth pressure sensor is sensitive (determining whether the pressure changes drastically).

[0064] Otherwise, enter the water navigation mode: If the pre-judgment conditions and confirmation switching conditions of the operating vehicle are not met during operation, the water navigation mode will be maintained.

[0065] If yes, enter underwater navigation mode: when the water depth pressure sensor touches the water surface, the pressure value will change dramatically, confirming that the switching conditions are met, and the navigation system will quickly shield the external GNSS or UWB signal to enter underwater navigation mode.

[0066] The process of switching from underwater navigation mode to surface navigation mode is as follows: Figure 3As shown, the pre-judgment condition is to determine whether the aircraft is in a floating state, and the confirmation switching condition is to determine whether the pressure is consistent with the atmospheric pressure for a period of time. Determine whether the aircraft is in a floating state based on speed detection, acceleration detection or / and attitude detection. Otherwise, enter the underwater navigation mode, and if so, enter the aircraft floating state. Combined with pressure detection, determine whether the pressure is atmospheric pressure for a period of time. Otherwise, enter the underwater navigation mode, and if so, enter the water navigation mode.

[0067] In addition, the aircraft will be affected by the cross-medium impact acceleration during the cross-medium process, which will cause the inertial data measured by the inertial sensor to change suddenly, thereby reducing the attitude measurement accuracy. To address this problem, the present invention proposes an improved adaptive Mahony complementary filtering method, and on this basis, compensates for the impact acceleration to adapt to complex cross-medium situations.

[0068] Before performing the adaptive Mahony complementary filtering, the raw data of the gyroscope and accelerometer are processed by using a high-pass filter and a low-pass filter respectively. Subsequently, an adaptive PI (proportional integral) parameter adjustment method is applied to the complementary filter to realize the adaptive Mahony complementary filtering.

[0069] The quaternion of the adaptive Mahony complementary filter can be written as Where P(·) is the rotation vector, δ is the new information, and w is the gyroscope measurement value. is the accelerometer measurement value; e is the error between the predicted value and the measured value, k P With k i is the control parameter of the PI regulator, and d represents the quaternion attitude matrix.

[0070] The adaptive Mahony complementary filtering algorithm includes prediction and correction. Prediction is to solve the attitude angle using the angular velocity measurement value, and correction is to correct the predicted attitude angle using the acceleration measurement value. The present invention uses the average measurement value of the three-axis accelerometer for nearly 20 epochs to adjust the PI regulator parameters. The specific design steps are as follows.

[0071] a) Get the three-axis gyroscope measurement value w and the three-axis accelerometer measurement value of the last epoch And the quaternion q n-1 .

[0072] b) Using the measurements of the triaxial accelerometer over the last 20 epochs Determine PI parameter k P and k i .

[0073] c) Measurements of the three-axis accelerometer Normalize.

[0074] d) Get the estimate of the Earth's gravity vector from the quaternion attitude matrix d

[0075] e) Calculate the error vector

[0076] f) The integral term I over the previous epoch n-1 And the error vector e of the current epoch is filtered. Then the integral term I is calculated n =I n-1 +k i ·Δt·e (Δt is the sampling period).

[0077] g) Calculate the update factor δ = k P e+I n .

[0078] h) Calculate the compensated three-axis gyroscope measurement value w'=w+δ.

[0079] i) Use the quaternion q of the last epoch n-1 and the compensated gyroscope value w' to update the quaternion:

[0080]

[0081] j) for q n After normalization, the attitude angles γ and θ are calculated respectively.

[0082] Repeat the above steps to obtain the updated value of the attitude angle in real time.

[0083] k P and k i The PI regulator parameters are determined by the gravity acceleration component measured by the accelerometer, and the external acceleration caused by various maneuvers or external impacts of the spacecraft should be eliminated before calculation. Therefore, impact acceleration compensation is of great significance to improving the accuracy of the attitude measurement system based on the micro-inertial measurement unit.

[0084] Impact acceleration compensation is the prerequisite for accurate attitude estimation of a cross-medium vehicle when crossing the water-air medium. The flowchart of the proposed integrated method of impact acceleration compensation and adaptive Mahony complementary filtering is shown in the figure below. Figure 4 shown.

[0085] The first step is to convert the current measurement value of the three-axis accelerometer Compared with the local Earth gravity acceleration, if it satisfies There is no external shock acceleration; is a preset acceleration threshold value based on the noise level of the triaxial accelerometer. Therefore, the triaxial accelerometer measurement only reflects the attitude change of the aircraft under static or uniform conditions. The present invention uses the average value of the accelerometer measurement to replace the instantaneous measurement value to reduce the influence of the accelerometer measurement noise caused by various operations before the system is started.

[0086] Step 2: If the This means that the spacecraft is in the process of crossing the medium and is subjected to a large impact acceleration. At this time, the accelerometer measurement value contains the impact acceleration component, so the attitude estimation cannot be performed directly, and therefore it needs to be compensated to a certain extent.

[0087] The third step is to compensate the three-axis accelerometer measurement value according to the motion of the spacecraft during the cross-medium process. The compensation calculation expression is: Where β is the compensation coefficient set according to the motion of the vehicle across the medium, which is determined by the magnitude of the impact acceleration and the measurement accuracy of the inertial sensor used.

[0088] The fourth step is to perform adaptive Mahony complementary filter integration on the compensated acceleration and the angular velocity measured by the gyroscope to calculate the real-time attitude angle.

[0089] The fifth step is to use the above quaternion to update the attitude angle.

[0090] The above embodiments are only illustrative of the principles and effects of the present invention, as well as some embodiments of its application. For those skilled in the art, several modifications and improvements may be made without departing from the creative concept of the present invention, and all of these belong to the protection scope of the present invention.

Claims

1. A navigation method for an aircraft in a water-air multimedia environment, characterized by: The water-air cross-medium integrated navigation system based on IMU inertial navigation devices, GNSS receivers, magnetometers, water depth pressure sensors and DVL equipment includes the following steps: S1, determine the state equation of the navigation system X(t) = F(t)X(t) + G(t)W(t), select the attitude error, velocity error, position error, gyro drift and three-axis accelerometer drift of the IMU inertial navigation device to form a 15-dimensional state vector: The system state transfer matrix at time t is: F I (t), F C (t), F ε (t) are: Where α g =diag(1 / τ gx 1 / τ gy 1 / τ gz ), α a =diag(1 / τ ax 1 / τ ay 1 / τ az ), where 1 / τ represents the Markov time-correlation constant; The system noise W(t) includes the zero bias and white noise of the three-axis gyroscope and three-axis accelerometer of the IMU inertial navigation device: W(t)=[w gεx w gεy w gεz w aεx w aεy w aεz or gx or gy or gz or ax or ay or az ] T , The system noise allocation matrix is: S2, determine the measurement equation of the integrated navigation system: Z(t)=H(t)X(t)+R(t); S21, when the vehicle is on the water, the GNSS receiver and magnetometer provide measurement information. In the formula They are the speed of INS solution and the speed of GNSS solution respectively. are the position solved by INS and the position provided by GNSS, h b is the magnetic field measurement vector, e is the north unit vector, and the measurement matrix is where h n is the real magnetic field vector; S22, when the vehicle is underwater, there is In the formula are the height of INS and the height of water depth pressure sensor respectively, and the measurement matrix is The introduction of DVL velocity observation underwater has In the formula are the speed calculated by INS and the speed measured by DVL respectively, and the measurement matrix is S3, with a preset specific state threshold as the switching condition, determines whether the switching condition is met by monitoring the navigation information obtained by the inertial sensor of the IMU inertial navigation device and the motion state of the vehicle, and switches from the water navigation mode to the underwater navigation mode, or from the underwater navigation mode to the water navigation mode.

2. The navigation method of an aircraft in a water-air multimedia environment according to claim 1, characterized in that: The switching process from the surface navigation mode to the underwater navigation mode is as follows: Determining whether the aircraft is in a descending state based on speed detection, acceleration detection and / or attitude detection; Otherwise, it enters the water navigation mode; when the vehicle has a continuous downward speed or acceleration in the vertical direction and is in a dive posture, the vehicle is in a descending state; the switching condition is confirmed by monitoring the changes in the external pressure sensitive to the water depth pressure sensor; otherwise, it enters the water navigation mode; if yes, it enters the underwater navigation mode.

3. The navigation method of an aircraft in a water-air multimedia environment according to claim 2, characterized in that: The switching process from the underwater navigation mode to the surface navigation mode is as follows: Determine whether the spacecraft is in a floating state based on speed detection, acceleration detection and / or attitude detection; Otherwise, it enters the underwater navigation mode, if yes, it enters the buoyancy state of the vehicle; combined with pressure detection, it is determined whether the pressure is atmospheric pressure for a period of time; otherwise, it enters the underwater navigation mode, if yes, it enters the surface navigation mode.

4. A navigation method for an aircraft in a water-air multimedia environment according to claim 2 or 3, characterized in that: When switching, the aircraft first uses a high-pass filter and a low-pass filter to process the original data measured by the three-axis gyroscope and the three-axis accelerometer, and then uses an adaptive Mahony complementary filtering method to compensate for the impact acceleration of the inertial data measured by the inertial sensor. The adaptive PI parameter adjustment method is applied to the complementary filter to realize the adaptive Mahony complementary filtering: The quaternion of the adaptive Mahony complementary filtering is Where P(·) is the rotation vector, δ is the new information, and w is the gyroscope measurement value. is the accelerometer measurement value, e is the error between the predicted value and the measured value, k P With k i is the control parameter of the PI regulator, and d represents the quaternion attitude matrix.

5. The navigation method of an aircraft in a water-air multimedia environment according to claim 4, characterized in that: The adaptive Mahony complementary filtering method uses the angular velocity measurement value to solve the predicted attitude angle, and uses the acceleration measurement value to correct the predicted attitude angle; the average measurement value of the three-axis accelerometer in the past 20 epochs is used to adjust the PI regulator parameters: a) Get the three-axis gyroscope measurement value w and the three-axis accelerometer measurement value of the last epoch And the quaternion q n-1 ; b) Using the measurements of the triaxial accelerometer over the last 20 epochs Determine the PI regulator parameter k P and k i ; c) Measurements of the three-axis accelerometer Perform normalization; d) Get the estimate of the Earth's gravity vector from the quaternion attitude matrix d e) Calculate the error vector f) The integral term I over the previous epoch n-1 The error vector e of the current epoch is filtered and then the integral term I is calculated n =I n-1 +k i ·Δt·e, Δt is the sampling period; g) Calculate the update factor δ = k P e+I n ; h) Calculate the compensated three-axis gyroscope measurement value w'=w+δ; i) Use the quaternion q of the last epoch n-1 and the compensated gyroscope value w' to update the quaternion: j) for q n Perform normalization processing and calculate the attitude angles γ and θ respectively; Repeat the above steps to obtain the updated value of the attitude angle in real time.

6. The navigation method of an aircraft in a water-air multimedia environment according to claim 5, characterized in that: The shock acceleration compensation and adaptive Mahony complementary filter integration are implemented through the following steps: The first step is to convert the current measurement value of the three-axis accelerometer Compared with the local Earth gravity acceleration, if it satisfies There is no external shock acceleration; is a preset acceleration threshold based on the noise level of the tri-axis accelerometer; Step 2: If the This means that the spacecraft is in the process of crossing the medium and is subjected to a large impact acceleration. At this time, the accelerometer measurement value needs to be compensated; The third step is to compensate the three-axis accelerometer measurements according to the motion of the spacecraft across the medium: Where β is the compensation coefficient set according to the motion of the spacecraft across the medium, which is determined by the magnitude of the impact acceleration and the measurement accuracy of the inertial sensor used; The fourth step is to perform adaptive Mahony complementary filter integration on the compensated acceleration and the angular velocity measured by the gyroscope to calculate the real-time attitude angle.

Images