A method and system for monitoring the attitude of a vehicle during navigation

Abstract

The application provides a kind of attitude monitoring method and system in aircraft navigation, in each filtering period, the high frequency energy component of current gyroscopic measurement angular velocity signal is used to determine process noise covariance matrix, according to the state vector of last time and the angular velocity obtained by current gyroscopic measurement and process noise covariance matrix, the prior state vector and prior covariance matrix of current time are predicted;Asymmetric Sigma point set is generated and measurement noise covariance matrix is determined;The asymmetric Sigma point set is substituted into measurement model, and the predicted measurement value and predicted measurement covariance are obtained by unscented transformation;Based on the predicted measurement covariance and the measurement noise covariance matrix, the Kalman gain is calculated, and the innovation composed of real measurement value and predicted measurement value is weighted gate inspection;Based on the adjusted Kalman gain and innovation, the state vector and covariance matrix are updated, and then the attitude information of current time aircraft is obtained.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of vehicle navigation, and particularly relates to a vehicle navigation attitude monitoring method and system. BACKGROUND

[0002] Vehicle attitude monitoring is a key technology in the field of navigation, guidance and control, and is crucial to the navigation safety and mission success of various vehicles such as ships, aircraft, underwater vehicles and spacecraft. The attitude information of a vehicle, usually referring to its roll angle, pitch angle and yaw angle relative to a specific reference coordinate system, is the basis for precise control and stable navigation. Currently, the mainstream technical solution for attitude monitoring is multi-sensor information fusion based on an inertial measurement unit and a magnetometer. The inertial measurement unit includes a three-axis gyroscope and a three-axis accelerometer. The gyroscope is used to measure angular velocity, and the attitude angle can be obtained by integration. However, the measurement value has inherent bias and noise, which will cause the integral error to accumulate over time, i.e. the drift phenomenon. The accelerometer can measure the gravity acceleration vector in static or uniform motion, and is used to correct the pitch angle and roll angle errors generated by the gyroscope. However, when the vehicle has a maneuvering acceleration, it is difficult to accurately separate the gravity and maneuvering acceleration components. The magnetometer corrects the yaw angle by measuring the earth's magnetic field vector. However, it is easily affected by the magnetic field of the vehicle's own components and external environment, resulting in inaccurate heading information. The Kalman filter method is widely used in vehicle attitude monitoring. However, when the vehicle switches from smooth navigation to severe maneuvering, or the sensor's noise characteristics change due to temperature, vibration and other factors, the attitude estimation may have overshoot, delay and other problems. Moreover, the sensor may have temporary abnormal values or be disturbed by impact, resulting in a biased non-Gaussian distribution of the true state error. For the processing of magnetic field interference, existing methods mostly rely on offline calibration, which cannot adapt to the magnetic field changes caused by the vehicle's motor starting, load changes or proximity to large ferromagnetic objects, making it difficult to ensure the long-term reliability of the heading angle. These factors affect the accuracy of attitude monitoring in vehicle navigation. SUMMARY

[0003] In order to improve the accuracy of attitude monitoring in vehicle navigation and thus improve the accuracy of navigation, the application provides a vehicle navigation attitude monitoring method, which comprises:

[0004] initializing a state vector and a covariance matrix of the state vector, the state vector including at least a quaternion of an attitude of the vehicle, real-time biases of a three-axis gyroscope, real-time biases of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in a body coordinate system of the vehicle; in each filtering period, determining a process noise covariance matrix by using a high-frequency energy component of a current angular velocity signal measured by the gyroscope, predicting a prior state vector and a prior covariance matrix at a current time according to a state vector at a previous time, an angular velocity measured by the gyroscope at the current time, and the process noise covariance matrix; generating an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, an asymmetry of the asymmetric Sigma point set being determined by a third moment of a historical prediction error;

[0005] determining a measurement noise covariance matrix by using statistical variances of a latest innovation sequence within a preset sliding time window, and increasing a component corresponding to a magnetic field measurement value in the measurement noise covariance matrix according to a deviation between a current magnetic field measurement norm and a local reference geomagnetic model norm;

[0006] substituting the asymmetric Sigma point set into a measurement model to obtain a predicted measurement value and a predicted measurement covariance by an unscented transformation; calculating a Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and performing a weighted gate check on innovation composed of a real measurement value and the predicted measurement value; updating the state vector and the covariance matrix based on the adjusted Kalman gain and the innovation, and thus obtaining attitude information of the vehicle at the current time.

[0007] Preferably, the determination of the process noise covariance matrix by using the high-frequency energy component of the angular velocity signal measured by the gyroscope comprises:

[0008] obtaining a three-axis angular velocity measurement value of the gyroscope at a current sampling time;

[0009] obtaining a high-frequency component of the angular velocity signal by performing vector difference between the current angular velocity measurement value and a previous angular velocity measurement value;

[0010] calculating an energy of the high-frequency component to obtain a high-frequency energy value;

[0011] multiplying the high-frequency energy value by a preset scaling coefficient matrix, and adding a basic process noise covariance matrix to obtain the process noise covariance matrix at the current time.

[0012] Preferably, the prediction of the prior state vector and the prior covariance matrix at the current time according to the state vector at the previous time, the angular velocity measured by the gyroscope at the current time, and the process noise covariance matrix comprises:

[0013] extracting an attitude quaternion and gyroscope biases from the posterior state vector at the previous time;

[0014] compensate a gyro angular velocity measurement value at a current time instant by using the gyro bias to obtain a compensated angular velocity;

[0015] integrate the compensated angular velocity by using a quaternion kinematic differential equation to obtain a predicted attitude quaternion at the current time instant;

[0016] combine the gyro bias, the accelerometer bias and the magnetic field disturbance vector at a previous time instant as predicted values at the current time instant, and combine the predicted attitude quaternion to obtain a prior state vector at the current time instant;

[0017] pass the Sigma point set at the previous time instant through the quaternion kinematic differential equation to calculate a state covariance after transmission, and add the process noise covariance matrix to obtain a prior covariance matrix at the current time instant.

[0018] Preferably, the prior state vector and the prior covariance matrix are used to generate an asymmetric Sigma point set, and an asymmetry of the asymmetric Sigma point set is determined by a third moment of a historical prediction error, and the method comprises the following steps:

[0019] calculate a vector difference between a historical predicted state vector and a posterior state vector to obtain a prediction error sequence within a sliding time window;

[0020] calculate a third central moment of the prediction error sequence to obtain a skewness coefficient vector;

[0021] decompose the prior covariance matrix at the current time instant to obtain a lower triangular matrix;

[0022] obtain a weight of a first group of Sigma points and a weight of a second group of Sigma points, wherein a difference between the weight of the first group of Sigma points and the weight of the second group of Sigma points is proportional to the skewness coefficient vector;

[0023] generate a Sigma point set asymmetrically distributed in a state space by using the prior state vector, the lower triangular matrix and the weights of the first group and the second group.

[0024] Preferably, the measurement noise covariance matrix is determined by using statistical variances of a latest innovation sequence within a preset sliding time window, and the method comprises the following steps:

[0025] obtain and calculate a difference between a real measurement value at the current time instant and a predicted measurement value to obtain an innovation vector at the current time instant;

[0026] store the innovation vector at the current time instant into a preset sliding time window with a length of M to obtain an innovation sequence;

[0027] calculate a covariance of the innovation sequence to obtain an innovation covariance matrix;

[0028] Subtracting the state prediction covariance in the measurement space obtained by the unscented transformation from the innovation covariance matrix obtains the measurement noise covariance matrix at the current time.

[0029] Preferably, the step of increasing the component corresponding to the magnetometer measurement in the measurement noise covariance matrix according to the norm deviation of the current magnetometer measurement and the norm of the local reference geomagnetic model comprises:

[0030] Calculating the L2 norm of the three-axis magnetometer measurement at the current time, and calculating the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, calculating the absolute value of the difference between the L2 norm of the magnetometer measurement and the L2 norm of the reference geomagnetic field vector to obtain the norm deviation;

[0031] Inputting the norm deviation into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; multiplying the diagonal element corresponding to the three-axis magnetometer measurement in the measurement noise covariance matrix determined by the latest innovation sequence by the noise scaling factor.

[0032] Preferably, the step of performing a weighted gate test on the innovation composed of the real measurement value and the predicted measurement value comprises:

[0033] Calculating the inverse matrix of the covariance matrix of the innovation vector, and calculating the square of the distance of the innovation using the innovation vector and the inverse matrix of the covariance matrix of the innovation vector; substituting the square of the distance into a Gaussian decay function to obtain a continuous weighting factor in the range of [0, 1];

[0034] Performing a multiplication operation on the continuous weighting factor and the unadjusted Kalman gain to obtain an adjusted Kalman gain.

[0035] The application further provides a navigation attitude monitoring system for an aircraft, comprising:

[0036] A Sigma point set generation unit is configured to initialize a state vector and a covariance matrix of the state vector, wherein the state vector at least includes a quaternion of an aircraft attitude, a real-time bias of a three-axis gyroscope, a real-time bias of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in an aircraft body coordinate system; in each filtering period, a high-frequency energy component of a current angular velocity signal measured by the gyroscope is used to determine a process noise covariance matrix, and a prior state vector and a prior covariance matrix at the current time are predicted according to a state vector at a previous time, an angular velocity measured by the gyroscope at the current time, and the process noise covariance matrix; an asymmetric Sigma point set is generated based on the prior state vector and the prior covariance matrix, and the asymmetry of the asymmetric Sigma point set is determined by a third moment of a historical prediction error.

[0037] The measurement noise covariance matrix generating unit is configured to determine the measurement noise covariance matrix by using statistical variances of the latest innovation sequence within a preset sliding time window, and increase components corresponding to the magnetometer measurement values in the measurement noise covariance matrix according to deviations of the current magnetometer measurement norm and the local reference geomagnetic model norm;

[0038] The monitoring unit is configured to substitute the asymmetric Sigma point set into a measurement model, obtain predicted measurement values and predicted measurement covariances through an unscented transformation, calculate a Kalman gain based on the predicted measurement covariances and the measurement noise covariance matrix, and perform a weighted gate test on innovation composed of real measurement values and predicted measurement values; and update a state vector and a covariance matrix based on the adjusted Kalman gain and the innovation, thereby obtaining attitude information of the vehicle at the current time.

[0039] Preferably, the process of determining the process noise covariance matrix by using high-frequency energy components of the current gyroscopic angular velocity signal comprises:

[0040] Obtaining gyroscopic three-axis angular velocity measurement values at the current sampling time;

[0041] Obtaining high-frequency components of the angular velocity signal by performing vector difference between the current angular velocity measurement values and the angular velocity measurement values at the last time;

[0042] Calculating energy of the high-frequency components to obtain a high-frequency energy value;

[0043] Multiplying the high-frequency energy value by a preset scaling coefficient matrix, and adding the high-frequency energy value to a basic process noise covariance matrix to obtain the process noise covariance matrix at the current time.

[0044] Preferably, the process of predicting the prior state vector and the prior covariance matrix at the current time based on the state vector at the last time, the current gyroscopic angular velocity obtained from the gyroscopic measurement, and the process noise covariance matrix comprises:

[0045] Extracting attitude quaternions and gyroscopic biases from the posterior state vector at the last time;

[0046] Compensating the gyroscopic angular velocity measurement values at the current time by using the gyroscopic biases to obtain compensated angular velocities;

[0047] Integrating the compensated angular velocities based on the four-element motion kinematics differential equation to obtain predicted attitude quaternions at the current time;

[0048] Combining the gyroscopic biases, the accelerometer biases and the magnetic field disturbance vectors at the last time as predicted values at the current time with the predicted attitude quaternions to obtain the prior state vector at the current time;

[0049] The state covariance after transmission is calculated by the quaternion kinematics differential equation from the Sigma point set of the last time, and the process noise covariance matrix is added to obtain the prior covariance matrix of the current time.

[0050] Preferably, the unsymmetrical Sigma point set is generated based on the prior state vector and the prior covariance matrix, and the unsymmetrical degree of the unsymmetrical Sigma point set is determined by the third moment of the historical prediction error, comprising:

[0051] In the sliding time window, the vector difference between the historical prediction state vector and the posterior state vector is calculated to obtain a prediction error sequence;

[0052] The third central moment of the prediction error sequence is calculated to obtain a skewness coefficient vector;

[0053] The prior covariance matrix of the current time is decomposed to obtain a lower triangular matrix;

[0054] The weight of the first group of Sigma points and the weight of the second group of Sigma points are obtained, wherein the difference between the weight of the first group of Sigma points and the weight of the second group of Sigma points is proportional to the skewness coefficient vector;

[0055] The prior state vector, the lower triangular matrix, and the first group and second group weights are used to generate a Sigma point set with unsymmetrical distribution in the state space.

[0056] Preferably, the measurement noise covariance matrix is determined by the statistical variance of the latest innovation sequence in a preset sliding time window, comprising:

[0057] The difference between the real measurement value of the current time and the predicted measurement value is obtained to obtain an innovation vector of the current time;

[0058] The innovation vector of the current time is stored in a preset sliding time window with a length of M to obtain an innovation sequence;

[0059] The covariance of the innovation sequence is calculated to obtain an innovation covariance matrix;

[0060] The measurement noise covariance matrix of the current time is obtained by subtracting the covariance of the state prediction in the measurement space obtained by the unscented transformation from the innovation covariance matrix.

[0061] Preferably, the components corresponding to the magnetometer measurement value in the measurement noise covariance matrix are increased according to the deviation of the norm of the current magnetometer measurement from the norm of the local reference geomagnetic model, comprising:

[0062] calculating the L2 norm of the three-axis magnetometer measurement value at the current time, calculating the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, and obtaining the norm deviation by calculating the absolute value of the difference between the L2 norm of the magnetometer measurement value and the L2 norm of the reference geomagnetic field vector;

[0063] inputting the norm deviation into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1, and multiplying the diagonal element corresponding to the three-axis magnetometer measurement value in the measurement noise covariance matrix determined by the latest innovation sequence by the noise scaling factor.

[0064] Preferably, the soft gating test on the innovation composed of the real measurement value and the predicted measurement value comprises:

[0065] calculating the inverse matrix of the covariance matrix of the innovation vector, calculating the square value of the distance of the innovation by using the innovation vector and the inverse matrix of the covariance matrix of the innovation vector, and substituting the square value of the distance into a Gaussian decay function to obtain a continuous weighting factor in the range of [0, 1];

[0066] performing multiplication operation on the continuous weighting factor and the unadjusted Kalman gain to obtain the adjusted Kalman gain.

[0067] Compared with the prior art, the application introduces the real-time bias of the three-axis accelerometer and the three-axis magnetic field disturbance vector into the state vector, more accurately separates the gravity acceleration vector and the linear acceleration of the vehicle itself, and improves the reliability of the heading angle. Moreover, the asymmetric Sigma points generated based on the third-order moment of the historical error make the filter better fit the non-Gaussian error distribution and suppress the estimation deviation caused by the asymmetric disturbance of the system. By adjusting the measurement noise covariance matrix and the process noise covariance matrix, the optimal filtering performance can be maintained when the vehicle switches between static and dynamic states and the sensor noise changes. Moreover, the soft gating test on the innovation avoids the impact of data mutation on the system. BRIEF DESCRIPTION OF DRAWINGS

[0068] Figure 1 the flowchart of embodiment one;

[0069] Figure 2 the schematic diagram of the generation of the process noise covariance matrix;

[0070] Figure 3 the schematic diagram of the comparison between the symmetric Sigma point set and the asymmetric Sigma point set;

[0071] Figure 4 the schematic diagram of increasing the component corresponding to the magnetometer measurement value in the measurement noise covariance matrix;

[0072] Figure 5 A diagram for gain adjustment factor. DETAILED DESCRIPTION

[0073] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by a person skilled in the art without creative work fall within the protection scope of the present application.

[0074] The specific embodiments, such as Figure 1 A navigation method of a vehicle according to the present application, as shown in the accompanying drawings, comprises:

[0075] S1, initializing a state vector and a covariance matrix of the state vector, the state vector at least including a quaternion of a vehicle attitude, a real-time bias of a three-axis gyroscope, a real-time bias of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in a vehicle body coordinate system; in each filtering period, determining a process noise covariance matrix by using a high-frequency energy component of a current gyroscope measured angular velocity signal, predicting a prior state vector and a prior covariance matrix at a current time according to a state vector at a previous time and a current gyroscope measured angular velocity and the process noise covariance matrix; generating an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, an asymmetry degree of the asymmetric Sigma point set being determined by a third moment of a historical prediction error;

[0076] The state of the vehicle is expressed by using the state vector, and the state vector at least includes a quaternion of an attitude, a gyroscope bias, an accelerometer bias, and a magnetic field disturbance vector. The quaternion of the attitude represents an orientation of the vehicle in a three-dimensional space, and the use of the quaternion can avoid the gimbal lock problem of Euler angles; the gyroscope bias is due to a zero-point drift of the gyroscope, and three values represent a current drift size; the accelerometer bias is used to calibrate a zero-point bias of the accelerometer in real time, and the magnetic field disturbance vector is used to represent an interference field of an internal magnetic field of the vehicle itself, such as a motor and a metal hull, by using three values. The bias of the accelerometer is introduced into the state vector, so that the gravitational acceleration vector and the linear acceleration of the vehicle itself can be separated more accurately; the magnetic field disturbance vector is introduced, the magnetic field disturbance is taken as a state variable to be estimated, the filter can compensate for local interference in real time, and the stability and reliability of the heading angle are improved.

[0077] The smoothness of the gyro angular rate signal is monitored, for example, by a high pass filter to separate the high frequency jitter component from the signal. When the vehicle is cruising in calm water, the high frequency component is small and the energy is low, the process noise covariance matrix is small, indicating a high confidence in the prediction based on the current motion. When the vehicle suddenly starts the thruster or encounters a turbulent flow, the angular rate signal is jiggled violently and the high frequency energy is high, the process noise covariance matrix is increased, i.e. the confidence in the motion model is reduced and more reliance is put on the subsequent sensor measurements for correction. The next time's attitude is calculated from the last time's optimal attitude estimate, combined with the gyro readings after bias compensation, through the quaternion kinematics equation. For other error terms in the state vector, such as bias and disturbance, the prediction value is equal to the last time's optimal estimate value since they change slowly. The last time's state uncertainty, i.e. the covariance matrix, is also passed through the kinematics model and the process noise covariance matrix calculated in the last step is added. In one embodiment, the process noise covariance matrix is generated as shown in Figure 2

[0078] Some representative sample points, i.e. Sigma points, are selected from the probability cloud representing the current predicted attitude and uncertainty for subsequent validation. The prediction error history in the recent period of time is obtained, if the filter always predicts a higher roll angle, more or heavier sample points in the direction of a lower roll angle are generated when generating the Sigma points. The asymmetry is determined by the third moment, i.e. skewness, of the historical prediction error, and the prediction bias of the non-Gaussian distribution is corrected.

[0079] S2, the measurement noise covariance matrix is determined by the statistical variance of the latest innovation sequence in a preset sliding time window, and the component corresponding to the magnetometer measurement value in the measurement noise covariance matrix is increased according to the deviation of the current magnetometer measurement norm from the local reference geomagnetic model norm;

[0080] The predicted measurement value is compared with the actual measurement value, and the difference between the two is calculated to obtain the innovation. In a sliding time window, an exemplary window is 20 sampling periods, the dispersion degree of the innovation sequence in the window is counted. If the innovation is always small and stable, it indicates that the measurement value is reliable, and the measurement noise covariance matrix is reduced. On the contrary, if the innovation changes greatly, it indicates that the measurement value noise is large, and the measurement noise covariance matrix is increased.

[0081] ​The total strength of the geomagnetic field is essentially constant and can be looked up in an internal global geomagnetic model. The vector length of the current magnetometer three-axis reading, i.e. the magnetic field norm, is computed and compared to the reference model value. When the vehicle approaches a steel structure of an underwater pipeline or a sunken ship, its own magnetic field is severely disturbed, causing the measured norm to deviate sharply from the reference value. If the deviation is detected to exceed a pre-set threshold, the values in the measurement noise covariance matrix belonging to the magnetometer channel are multiplied by a penalty factor, e.g. 100. This makes the filter aware that the magnetometer reading is not trustworthy and its heading information is ignored in this update.

[0082] S3, substituting the asymmetric Sigma point set into the measurement model, obtaining predicted measurement values and predicted measurement covariance through unscented transformation; calculating Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and performing weighted gate check on innovation composed of real measurement values and predicted measurement values; updating state vector and covariance matrix based on adjusted Kalman gain and innovation, thereby obtaining the attitude information of the vehicle at the current time.

[0083] Each asymmetric Sigma point generated in S1 is converted through the measurement model to calculate the values of the accelerometer and the magnetometer under the assumed state, and to calculate the mean value and covariance of the predicted measurement values. In an embodiment, the measurement model includes at least two sub-models, i.e. an accelerometer measurement model and a magnetometer measurement model. Preferably, the accelerometer measurement model is wherein C(q) is a rotation matrix from the geographic coordinate system to the vehicle body coordinate system, specifically, the current attitude quaternion is extracted from the state vector, and then C(q) is obtained according to the attitude quaternion, is a gravity vector in the geographic coordinate system, is the current accelerometer bias extracted from the state vector. Preferably, the magnetometer measurement model is is a reference geomagnetic vector, is the current magnetic field disturbance vector extracted from the state vector.

[0084] ​Based on the predicted measurement covariance and the measurement noise covariance matrix determined in S2, the Kalman gain is calculated. Before updating the state, the innovation vector, i.e. the distance between the real measurement value and the predicted measurement value mean, and the theoretical covariance of this difference, preferably using Mahalanobis distance, is calculated. If the distance is very low, it means that the measurement value is as expected, and a weighting factor close to 1 is used. If the distance is very large, for example a sensor suddenly outputs an extreme value, a weighting factor close to 0 is calculated according to the distance, and the Kalman gain is scaled by the weighting factor. In this way, both abnormal values polluting the system and part of the useful information in the critical values are preserved. The predicted state vector is corrected using the Kalman gain adjusted by the weighting factor to obtain the most accurate posterior state vector at the current time. The attitude quaternion is extracted from the vector to obtain the current stable and reliable attitude information of the vehicle. Those skilled in the art know that each error in the state vector is also updated.

[0085] In an optional embodiment, the process of determining the high-frequency energy component of the current gyroscopic angular velocity signal includes:

[0086] Obtaining the three-axis angular velocity measurement value of the gyroscope at the current sampling time;

[0087] Obtaining the three-axis angular velocity measurement value of the gyroscope at the current sampling time;

[0088] Calculating the energy of the high-frequency component to obtain a high-frequency energy value;

[0089] Multiplying the high-frequency energy value by a preset scaling coefficient matrix and adding it to the basic process noise covariance matrix to obtain the process noise covariance matrix at the current time.

[0090] Specifically, when the vehicle is slowly translating or hovering in water, the motion is stable and the three-axis angular velocity readings of the gyroscope change very little. For example, the reading at the last time instant is 0.01 degree per second rotation in the X-axis, and the reading at the current time instant is 0.011 degree per second, the difference between the two is almost zero, and the energy value of the high frequency component is also close to zero, which means the vehicle is in a predictable and stable state. Conversely, when the mechanical arm of the vehicle suddenly extends to work or the propeller suddenly exerts force to avoid obstacles, a sudden rotation will occur, and the reading of the gyroscope may jump from 0.1 degree per second to 5 degrees per second in an instant. At this moment, the difference between the readings at the two time instants is huge, and the calculated high frequency energy value will be very large. The calculated high frequency energy value can measure the current motion intensity of the vehicle, and the energy value will be multiplied by a pre-set scaling coefficient matrix, which converts the energy unit into a noise variance unit matched with the state, and then added to a basic process noise matrix representing the minimum uncertainty of the system, to obtain the final process noise covariance matrix. When the vehicle is hovering stably, the high frequency energy value is very small, and the process noise matrix also maintains a very low level, and the prediction model is also more reliable. When the vehicle is moving violently, the high frequency energy value will significantly increase the process noise matrix, and the prediction model is less reliable, and more actual measurement data of the accelerometer and the magnetometer need to be used for correction to ensure that the attitude estimation can keep up with the violent motion.

[0091] In an optional embodiment, the predicting the prior state vector and the prior covariance matrix at the current time instant based on the state vector at the last time instant and the angular velocity obtained from the current gyroscope measurement and the process noise covariance matrix comprises:

[0092] extracting the attitude quaternion and the gyroscope bias from the posterior state vector at the last time instant;

[0093] compensating the angular velocity measurement at the current time instant by using the gyroscope bias to obtain a compensated angular velocity;

[0094] integrating the compensated angular velocity based on the quaternion kinematics differential equation to obtain a predicted attitude quaternion at the current time instant;

[0095] combining the gyroscope bias, the accelerometer bias and the magnetic field disturbance vector at the last time instant as the predicted values at the current time instant, and the predicted attitude quaternion to obtain the prior state vector at the current time instant;

[0096] calculating the state covariance after transmission of the Sigma point set at the last time instant by using the quaternion kinematics differential equation, and adding the process noise covariance matrix to obtain the prior covariance matrix at the current time instant.

[0097] Specifically, if the vehicle has completed a filtering update at the last time k-1, it has obtained its accurate attitude at that time, and the zero point bias of the gyroscope is about 0.1 degrees per second around the Z axis. At the current time k, the vehicle's gyroscope returns an original Z-axis angular velocity reading of 9.9 degrees per second. From the complete state case at the last time, since the state vector contains the attitude quaternion and the gyroscope bias, the attitude quaternion and the 0.1-degree Z-axis bias in the state vector are extracted, and the original reading of 9.9 degrees is reduced by the 0.1-degree bias to obtain an angular velocity of 9.8 degrees per second. The quaternion kinematics equation is used to integrate the 9.8-degree angular velocity for a very short time to calculate the predicted attitude of the vehicle at the current time k. The quaternion kinematics equation is the relationship equation between the change rate of the attitude quaternion and the angular velocity of the vehicle body, specifically , and the predicted attitude quaternion is obtained after integration and normalization.

[0098] For other slowly changing error terms in the state, such as accelerometer bias and internal magnetic field interference, they will not change suddenly in a short time. The optimal estimate value at the last time is taken as the predicted value at the current time, and the predicted attitude quaternion is combined to obtain the complete prior state vector. In order to determine the uncertainty of the new state, the Sigma point sample set representing the uncertainty distribution at the last time is also input into the same quaternion kinematics equation for calculation. Since the initial state itself has uncertainty, the dispersion degree between the sample points will be enlarged after motion transmission, so as to calculate the state covariance after transmission. The calculated process noise covariance matrix is added to the state covariance to obtain the prior covariance matrix containing the total uncertainty of the predicted state at the current time.

[0099] In an optional embodiment, the asymmetric Sigma point set is generated based on the prior state vector and the prior covariance matrix, and the asymmetry of the asymmetric Sigma point set is determined by the third moment of the historical prediction error, including:

[0100] In a sliding time window, the vector difference between the historical predicted state vector and the posterior state vector is calculated to obtain a prediction error sequence;

[0101] The third central moment of the prediction error sequence is calculated to obtain a skewness coefficient vector;

[0102] The prior covariance matrix at the current time is decomposed to obtain a lower triangular matrix;

[0103] The weights of the first group of Sigma points and the weights of the second group of Sigma points are obtained, and the difference between the weights of the first group of Sigma points and the weights of the second group of Sigma points is proportional to the skewness coefficient vector.​​

[0104] generating a set of Sigma points that are asymmetrically distributed in the state space using the prior state vector, the lower triangular matrix, and the first and second sets of weights.

[0105] The traditional unscented Kalman filter assumes that the prediction error is symmetrically distributed, but in reality, it is not always symmetrically distributed, for example, the effect of a received crosswind or one-sided water flow on an aircraft. A sliding time window of the prediction error sequence of the last, for example, 50 filter periods is set, and within the window, the difference between the predicted roll angle and the finally confirmed roll angle is calculated. If the mean of the difference sequence is close to zero, but most of the data falls on the same side of zero, a skewed distribution is presented. By calculating the third central moment of the error sequence, that is, the skewness, a skewness coefficient vector that can quantify the asymmetry is obtained, for example, the skewness coefficient vector indicates that the error distribution of the roll angle dimension is left-skewed. The prior covariance matrix of the current prediction uncertainty is decomposed to obtain a lower triangular matrix, and the lower triangular matrix includes the shape and direction of the uncertainty ellipsoid. Since the historical data shows that the roll angle prediction has a left-skewed tendency, when generating the Sigma points, the weights of the sample points on the right side of the uncertainty ellipsoid are adjusted to be higher, and the weights of the sample points on the left side are adjusted to be lower. The difference in weights is proportional to the skewness coefficient. A set of Sigma points that are asymmetrically distributed in the state space is generated using the predicted state vector, the lower triangular matrix, and the asymmetric weights. Compared with the symmetric Sigma point set, the asymmetrically distributed Sigma point set more truly reflects the current state, Figure 3 A comparison chart of the symmetric Sigma point set and the asymmetric Sigma point set is shown.

[0106] In an optional embodiment, the determination of the measurement noise covariance matrix using the statistical variance of the latest innovation sequence within a preset sliding time window comprises:

[0107] The difference between the true measurement value at the current time and the predicted measurement value is obtained to obtain an innovation vector at the current time;

[0108] The innovation vector at the current time is stored in a preset sliding time window with a length of M to obtain an innovation sequence;

[0109] The covariance of the innovation sequence is calculated to obtain an innovation covariance matrix;

[0110] The measurement noise covariance matrix at the current time is obtained by subtracting the covariance of the state prediction in the measurement space obtained by unscented transformation from the innovation covariance matrix.

[0111] Specifically, the vehicle continuously converts its attitude prediction value into corresponding theoretical sensor readings when performing a task, and compares them with the real measurements of the accelerometer and magnetometer. The difference between the two is the innovation vector. For example, if the vehicle is predicted to be horizontal, the corresponding Z-axis accelerometer reading should be 9.8, but the actual measurement is 9.9, then the innovation of the Z-axis acceleration is -0.1. This innovation vector, as well as all the innovation vectors calculated in the past, for example, in the last 30 sampling periods, forms an innovation sequence. The covariance matrix of the sequence of 30 innovation vectors is calculated, which reflects the overall dispersion of the differences between the prediction and the measurement. The total difference is composed of two parts, one part is due to the inaccurate prediction of the system state itself, and the other part is due to the noise contained in the sensor measurement itself. Through the unscented transformation, the former, i.e. the covariance part contributed by the state prediction uncertainty, is calculated. From the total covariance matrix of the innovation sequence, subtract the covariance part contributed by the state prediction, and the remaining is theoretically the noise and error introduced by the sensor measurement process itself. The difference is used as the measurement noise covariance matrix at the current time. For example, when the vehicle enters turbid water, the impact of the water flow on the ship body causes the real reading of the accelerometer to fluctuate intensively, and the overall variance of the innovation sequence will become larger, and the measurement noise covariance matrix obtained will also increase accordingly, thereby reducing the trust degree of the current accelerometer reading.

[0112] In an optional embodiment, the step of increasing the component corresponding to the magnetometer measurement in the measurement noise covariance matrix according to the deviation of the norm of the current magnetometer measurement from the norm of the local reference geomagnetic model, comprises:

[0113] calculating the L2 norm of the three-axis measurement of the magnetometer at the current time, and calculating the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, calculating the absolute value of the difference between the L2 norm of the magnetometer measurement and the L2 norm of the reference geomagnetic field vector to obtain the norm deviation;

[0114] inputting the norm deviation into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; multiplying the diagonal element corresponding to the three-axis measurement of the magnetometer in the measurement noise covariance matrix determined by the latest innovation sequence by the noise scaling factor.

[0115] The total intensity of the Earth's magnetic field is relatively stable in local areas, and the magnitude can be obtained from a global geomagnetic model. For example, in a certain sea area, the total intensity of the reference geomagnetic model is 50 μΤ. Normally, when the vehicle arrives at this location, the magnetometer should also measure a total intensity of about 50 μΤ by calculating the vector length of the three-axis measurements, i.e., the L2 norm. However, when the vehicle approaches an iron-rich seamount or a steel shipwreck, the local magnetic field will be distorted, and the total intensity measured by the magnetometer can be 200 μΤ. The difference between the measured norm and the reference norm is 150 μΤ, indicating that the magnetometer reading is completely disturbed by the external environment. Substituting the deviation value into an exponential function, for example, the function is shown in Figure 4 In the above example, the calculated noise scaling factor is 1024. The noise scaling factor is used to adjust the diagonal elements of the measurement noise covariance matrix corresponding to the three axes of the magnetometer, i.e., the measurement noise of the magnetometer is amplified by 1024 times. In the subsequent calculation of the Kalman gain, the weight contributed by the magnetometer is almost attenuated to zero. Even if the magnetometer returns the wrong heading information, it will be ignored, and the attitude estimation will completely depend on the integration of the gyroscope and the gravity vector correction of the accelerometer, thereby ensuring the stability of the heading angle and reducing or even avoiding the external strong magnetic interference.

[0116] In an optional embodiment, the gating test on the innovation composed of the true measurement value and the predicted measurement value comprises:

[0117] calculating the inverse matrix of the covariance matrix of the innovation vector, calculating the squared value of the distance of the innovation using the innovation vector and the inverse matrix of the covariance matrix of the innovation vector; and substituting the squared value of the distance into a Gaussian decay function to obtain a continuous weighting factor in the interval [0, 1];

[0118] multiplying the continuous weighting factor and the unadjusted Kalman gain to obtain the adjusted Kalman gain.

[0119] When the vehicle attitude system is running stably, the innovation of each measurement, i.e., the difference between the prediction and the true measurement value, corresponds to a theoretical covariance matrix, which defines a reasonable error range in multiple dimensions. When all sensors of the vehicle are working normally, the innovation vector falls within the reasonable error range, and the squared value of the Mahalanobis distance is calculated using the innovation vector and the inverse matrix of the covariance matrix of the innovation vector. When running stably, the value is very small, for example, 0.5. Substituting the squared distance value, i.e., 0.5, into a Gaussian decay function, the Gaussian decay function makes the output close to 1 when the input value is very small, as shown in Figure 5If the input is 0.5, the function can output a continuous weighting factor of 0.98. The unadjusted Kalman gain represents the weight of trust in the measurement under normal conditions. Multiplying the weighting factor with the entire Kalman gain matrix results in a Kalman gain that is almost unchanged and is used to update the attitude normally. However, if a thruster of the vehicle is entangled with water grass and causes the vehicle to shake violently, the accelerometer generates an abnormal reading, and the calculated Mahalanobis distance square value is 50. The Gaussian decay function outputs a very small value, for example, 0.01, and the weighting factor of 0.01 causes the Kalman gain to decay to nearly zero, so that the error measurement caused by the abnormal shaking is almost ignored in the update, thereby ensuring the stability of the attitude estimation.

[0120] The above examples are only used to illustrate the technical solutions of the present application, but not limit the present application; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that the technical solutions recorded in the foregoing examples can be modified, or some technical features can be replaced equivalently; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application. In addition, the various different embodiments of the embodiments of the present application can be combined arbitrarily, as long as it does not deviate from the idea of the embodiments of the present application, and it should also be considered as the disclosed content of the embodiments of the present application.

Claims

1. A method of attitude monitoring in navigation of a vehicle, characterized by, The method comprises the following steps: initializing a state vector and a covariance matrix of the state vector, the state vector comprising at least a quaternion of an attitude of an aircraft, real-time biases of a three-axis gyroscope, real-time biases of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in a body coordinate system of the aircraft; in each filtering period, determining a process noise covariance matrix by using a high-frequency energy component of a current angular velocity signal measured by the gyroscope, and predicting a prior state vector and a prior covariance matrix at a current time according to a state vector at a previous time and the current angular velocity measured by the gyroscope and the process noise covariance matrix; generating an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, the asymmetry of the asymmetric Sigma point set being determined by a third moment of a historical prediction error; determining a measurement noise covariance matrix by using a statistical variance of a latest innovation sequence within a preset sliding time window, and increasing a component corresponding to a magnetic field measurement value in the measurement noise covariance matrix according to a deviation of a current magnetic field measurement norm from a norm of a local reference geomagnetic model; substituting the asymmetric Sigma point set into a measurement model to obtain a predicted measurement value and a predicted measurement covariance by means of an unscented transformation; calculating a Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and performing a weighted gate check on innovation composed of a real measurement value and the predicted measurement value; updating the state vector and the covariance matrix based on the adjusted Kalman gain and the innovation, so as to obtain attitude information of the aircraft at the current time; the step of generating the asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, the asymmetry of the asymmetric Sigma point set being determined by the third moment of the historical prediction error, comprises the following steps: calculating a vector difference between a historical prediction state vector and a posterior state vector to obtain a prediction error sequence within a sliding time window; calculating a third central moment of the prediction error sequence to obtain a skewness coefficient vector; decomposing the prior covariance matrix at the current time to obtain a lower triangular matrix; obtaining a weight of a first group of Sigma points and a weight of a second group of Sigma points, wherein a difference between the weight of the first group of Sigma points and the weight of the second group of Sigma points is proportional to the skewness coefficient vector; generating the Sigma point set asymmetrically distributed in a state space by using the prior state vector, the lower triangular matrix, and the weights of the first group and the second group; the step of increasing the component corresponding to the magnetic field measurement value in the measurement noise covariance matrix according to the deviation of the current magnetic field measurement norm from the norm of the local reference geomagnetic model, comprises the following steps: calculating an L2 norm of a three-axis measurement value of a magnetic field meter at the current time, and calculating an L2 norm of a reference geomagnetic field vector at a current position obtained from a local reference geomagnetic model, and calculating an absolute value of a difference between the L2 norm of the magnetic field measurement value and the L2 norm of the reference geomagnetic field vector to obtain a norm deviation; Inputting the norm deviation into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; multiplying diagonal elements corresponding to three-axis measurements of the magnetometer in a measurement noise covariance matrix determined by a latest innovation sequence by the noise scaling factor.

2. The method of claim 1, wherein, The process of determining the process noise covariance matrix using a high-frequency energy component of the current angular velocity signal includes: Obtaining three-axis angular velocity measurements of the gyroscope at the current sampling time; Obtaining a high-frequency component of the angular velocity signal by performing vector difference between the current angular velocity measurement and the last time angular velocity measurement; Calculating the energy of the high-frequency component to obtain a high-frequency energy value; Multiplying the high-frequency energy value by a preset scaling coefficient matrix and adding the basic process noise covariance matrix to obtain the process noise covariance matrix at the current time.

3. The method of claim 1, wherein, The process of predicting the prior state vector and the prior covariance matrix at the current time according to the state vector at the last time, the angular velocity obtained from the current gyroscope measurement and the process noise covariance matrix includes: Extracting the attitude quaternion and the gyroscope bias from the posterior state vector at the last time; Compensating the gyroscope angular velocity measurement at the current time using the gyroscope bias to obtain a compensated angular velocity; Integrating the compensated angular velocity based on the quaternion kinematics differential equation to obtain the predicted attitude quaternion at the current time; Combining the gyroscope bias, the accelerometer bias and the magnetic field disturbance vector at the last time as the predicted values at the current time with the predicted attitude quaternion to obtain the prior state vector at the current time; Transferring the state covariance of the Sigma point set at the last time through the quaternion kinematics differential equation and adding the process noise covariance matrix to obtain the prior covariance matrix at the current time.

4. The method of claim 1, wherein, The process of determining the measurement noise covariance matrix using the statistical variance of the latest innovation sequence within a preset sliding time window includes: Obtaining and calculating the difference between the real measurement value at the current time and the predicted measurement value to obtain the innovation vector at the current time; Storing the innovation vector at the current time in a preset sliding time window with a length of M to obtain an innovation sequence; Calculating the covariance of the innovation sequence to obtain an innovation covariance matrix; Subtracting the covariance of the state prediction in the measurement space obtained by the unscented transformation from the innovation covariance matrix to obtain the measurement noise covariance matrix at the current time.

5. The method of claim 1, wherein, The process of performing weighted gate detection on the innovation composed of the real measurement value and the predicted measurement value includes: Calculating the inverse matrix of the covariance matrix of the innovation vector, calculating the square value of the distance of the innovation using the innovation vector and the inverse matrix of the covariance matrix of the innovation vector; substituting the square value of the distance into the Gaussian decay function to obtain a continuous weighting factor in the range of [0, 1]; Performing multiplication operation on the continuous weighting factor and the unadjusted Kalman gain to obtain the adjusted Kalman gain.

6. A system for monitoring the attitude of a vehicle during navigation, the system comprising: The process includes: A sigma point set generating unit is configured to initialize a state vector and a covariance matrix of the state vector, the state vector including at least a quaternion of an attitude of the vehicle, real-time biases of a three-axis gyroscope, real-time biases of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in a body coordinate system of the vehicle; In each filtering cycle, a high-frequency energy component of a current gyroscope measured angular velocity signal is used to determine a process noise covariance matrix, and a prior state vector and a prior covariance matrix at a current time are predicted based on a state vector at a previous time, a current gyroscope measured angular velocity, and the process noise covariance matrix; An asymmetric sigma point set is generated based on the prior state vector and the prior covariance matrix, and an asymmetry degree of the asymmetric sigma point set is determined based on a third moment of a historical prediction error; A measurement noise covariance matrix generating unit is configured to determine a measurement noise covariance matrix based on a statistical variance of a latest innovation sequence within a preset sliding time window, and increase a component corresponding to a magnetometer measured value in the measurement noise covariance matrix based on a deviation of a current magnetometer measurement norm from a local reference geomagnetic model norm; A monitoring unit is configured to substitute the asymmetric sigma point set into a measurement model to obtain a predicted measurement value and a predicted measurement covariance through an unscented transformation, calculate a Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and perform a weighted gate test on an innovation composed of a real measurement value and the predicted measurement value; The state vector and the covariance matrix are updated based on the adjusted Kalman gain and the innovation, and thus an attitude information of the vehicle at the current time is obtained; The asymmetric sigma point set is generated based on the prior state vector and the prior covariance matrix, and an asymmetry degree of the asymmetric sigma point set is determined based on a third moment of a historical prediction error, including: A prediction error sequence is obtained by calculating a vector difference between a historical prediction state vector and a posterior state vector within a sliding time window; A third central moment of the prediction error sequence is calculated to obtain a skewness coefficient vector; A lower triangular matrix is obtained by decomposing the prior covariance matrix at the current time; A weight of a first group of sigma points and a weight of a second group of sigma points are obtained, and a difference between the weight of the first group of sigma points and the weight of the second group of sigma points is proportional to the skewness coefficient vector; The sigma point set asymmetrically distributed in the state space is generated based on the prior state vector, the lower triangular matrix, and the weights of the first group and the second group; The component corresponding to the magnetometer measured value in the measurement noise covariance matrix is increased based on a deviation of a current magnetometer measurement norm from a local reference geomagnetic model norm, including: An L2 norm of a three-axis magnetometer measurement value at the current time is calculated, an L2 norm of a reference geomagnetic field vector at a current position obtained from a local reference geomagnetic model is calculated, and an absolute value of a difference between the L2 norm of the magnetometer measurement value and the L2 norm of the reference geomagnetic field vector is calculated to obtain a norm deviation. The norm deviation is input into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; and diagonal elements corresponding to three-axis measurements of the magnetometer in a measurement noise covariance matrix determined by a latest innovation sequence are multiplied by the noise scaling factor.

7. The system of claim 6, wherein, The process noise covariance matrix is determined by using a high-frequency energy component of the current angular velocity signal of the gyroscope, and includes the following steps: obtaining three-axis angular velocity measurements of the gyroscope at a current sampling time; obtaining a high-frequency component of the angular velocity signal by performing vector difference between the current angular velocity measurement and a previous angular velocity measurement; calculating energy of the high-frequency component to obtain a high-frequency energy value; multiplying the high-frequency energy value by a preset scaling coefficient matrix and adding a basic process noise covariance matrix to obtain a process noise covariance matrix at the current time.

8. The system of claim 6, wherein, The prior state vector and the prior covariance matrix at the current time are predicted according to a state vector at a previous time, an angular velocity obtained by the current gyroscope measurement and a process noise covariance matrix, and include the following steps: extracting a posture quaternion and a gyroscope bias from the posterior state vector at the previous time; compensating the gyroscope angular velocity measurement at the current time by using the gyroscope bias to obtain a compensated angular velocity; integrating the compensated angular velocity by using a quaternion kinematics differential equation to obtain a predicted posture quaternion at the current time; combining the predicted posture quaternion at the current time with the prior state vector at the current time, wherein the prior state vector at the current time is obtained by using the gyroscope bias at the previous time, an accelerometer bias and a magnetic field disturbance vector as predicted values at the current time; calculating a state covariance after transmission of a Sigma point set at the previous time by using the quaternion kinematics differential equation and adding the process noise covariance matrix to obtain a prior covariance matrix at the current time.

Images