Array adaptive kalman filter attitude measurement method based on spatial variance

Abstract

This invention discloses an array adaptive Kalman filter attitude measurement method based on spatial variance, comprising the following steps: (1) acquiring inertial measurement data of each IMU in the IMU array; (2) compensating for size effects in the inertial measurement data of each IMU; (3) constructing a spatial variance vector and an adaptive measurement noise covariance matrix; (4) constructing a state equation based on quaternion differential equations for attitude prior estimation; (5) constructing a covariance prediction equation; (6) constructing an attitude observation equation based on the acceleration after size effect compensation and the measurement value of the geomagnetic sensor; (7) calculating the Kalman gain based on spatial variance, correcting the prior state, obtaining a posterior estimate in quaternion form, and updating the covariance matrix; (8) converting the quaternion into Euler angles. The spatial variance vector and adaptive measurement noise covariance matrix constructed in this invention can effectively cope with the impact of sudden dynamic shocks on the stable attitude output.

Description

Technical Field

[0001] This invention relates to the field of inertial measurement technology for microelectromechanical systems (MEMS), and particularly to an array adaptive Kalman filter attitude measurement method based on spatial variance. Background Technology

[0002] Microelectromechanical systems (MEMS) inertial measurement units (IMUs) have been widely used in navigation, motion capture, and robotics due to their small size, low cost, and high autonomy. A basic IMU typically contains two core sensors: a gyroscope and an accelerometer. The gyroscope measures angular velocity, but its output suffers from significant drift errors. The accelerometer measures specific force and can calculate attitude information by calculating the gravitational component, but it is susceptible to interference from the vehicle's acceleration in dynamic environments. Therefore, a single sensor cannot independently provide long-term, stable, and accurate motion state information. Effective fusion of gyroscope and accelerometer data within the IMU is necessary to achieve reliable attitude state estimation.

[0003] However, due to manufacturing limitations, the overall performance of a single MEMS IMU faces bottlenecks. To improve system accuracy and reliability, researchers employ sensor arrays composed of multiple IMUs to enhance measurement accuracy and thus attitude accuracy. IMU arrays, through hardware redundancy and data fusion, can effectively suppress random noise at the hardware level, improving measurement signal-to-noise ratio and accuracy. However, when the system is subjected to drastic environmental changes such as high dynamics, strong vibrations, or complex maneuvers, the error characteristics of each IMU, such as zero bias and noise statistics, may become inconsistent and subject to time-varying uncertainties. In such cases, the effectiveness of simple redundant data fusion methods decreases because they cannot distinguish and address the performance differences and reliability changes of different sensors in the array under transient environments. The hardware redundancy advantage of the array is difficult to translate into stable and adaptive accuracy improvements in practical applications.

[0004] Therefore, while hardware-level IMU arrays provide a foundation for improving IMU measurement accuracy, a fusion algorithm is still needed that can adapt to the multi-source information of the IMU array and dynamically respond to changes in system state and sensor perception. Traditional Kalman filtering algorithms typically use a fixed measurement noise covariance matrix. When encountering high-frequency mechanical resonance, external impacts, or strong electromagnetic interference, the sensor's measurement noise will exhibit non-Gaussian and non-stationary characteristics. In this case, if a fixed covariance matrix is ​​continued to be used, the filter may fail to detect the sharp decline in external observation quality, increase the weight of inferior observation data, and ultimately lead to severe divergence in attitude calculation. Summary of the Invention

[0005] The purpose of this invention is to provide an adaptive array Kalman filter attitude measurement algorithm for the spatial variance of IMU array structure. This method can effectively suppress the impact of sudden environmental changes on attitude output, thereby improving the stability, reliability and output accuracy of attitude measurement system attitude information.

[0006] The objective of this invention is achieved through the following technical solution: an array adaptive Kalman filter attitude measurement method based on spatial variance, comprising the following steps:

[0007] (1) Acquire inertial measurement data of each IMU in the IMU array;

[0008] (2) Size effect compensation is performed on the inertial measurement data of each IMU based on the principle of rigid body mechanics;

[0009] (3) Construct a spatial variance vector and an adaptive measurement noise covariance matrix based on the data after size effect compensation;

[0010] (4) Construct state equations based on quaternion differential equations for attitude prior estimation;

[0011] (5) Construct the covariance prediction equation:

[0012] (6) An attitude observation equation is constructed based on the acceleration after size effect compensation and the measurement values ​​of the geomagnetic sensor;

[0013] (7) Calculate the Kalman gain based on the spatial variance, correct the prior state, obtain the posterior estimate in quaternion form, and update the covariance matrix;

[0014] (8) Convert the quaternion to Euler angles.

[0015] The beneficial effects of this invention are as follows: The Spatial-Variance Adaptive Array Kalman Filter (SV-AAKF) algorithm proposed in this invention constructs a size effect compensation model based on rigid body kinematics to eliminate the dynamic centrifugal error component in acceleration, based on the hardware of various redundant sensor arrays. Furthermore, it constructs a spatial variance vector and an adaptive measurement noise covariance matrix for real-time sensing of external disturbances in the dynamic environment and adaptive updates, effectively addressing the impact of sudden dynamic shocks on attitude stability output. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the adaptive Kalman filter attitude measurement method of the present invention;

[0017] Figure 2 This is a comparison chart of the pose results of the method of the present invention with those of the traditional EKF and Madgwick algorithms. Detailed Implementation

[0018] This invention proposes a Spatial-Variance Adaptive Array Kalman Filter (SV-AAKF) algorithm. Based on the hardware of various redundant sensor arrays, a size effect compensation model based on rigid body kinematics is constructed to eliminate the dynamic centrifugal error component in acceleration. Furthermore, a spatial variance vector and an adaptive measurement noise covariance matrix are constructed for real-time sensing of external disturbances in dynamic environments and adaptive updates, effectively addressing the impact of sudden dynamic shocks on attitude stability output. The technical solution of this invention is further illustrated below with reference to the accompanying drawings.

[0019] like Figure 1 As shown, the present invention provides an array adaptive Kalman filter attitude measurement method based on spatial variance, comprising the following steps:

[0020] (1) Acquire inertial measurement data of each IMU in the IMU array;

[0021] (2) Size effect compensation for each sensor data based on the principle of rigid body mechanics; Since each IMU in the IMU array is installed in a different spatial position, the accelerometer installed at a non-center of mass position will not only measure the translational force at the center of the carrier, but will also be subject to the additional acceleration caused by the angular motion of the carrier. Therefore, it is necessary to compensate for the size effect of the measurement data of each sensor and make it equivalent to the center of mass.

[0022] The specific implementation method of size effect compensation is as follows:

[0023] Since, within the rigid body kinematics framework, the angular velocities of any nodes distributed on the carrier are theoretically consistent, the angular velocity of the system... The mean value is obtained from the output of each gyroscope in the IMU array:

[0024] ;

[0025] in Represents the angular velocity measured by the i-th gyroscope, and N represents the number of IMUs;

[0026] The measurement value of the i-th accelerometer Represented as:

[0027] ;

[0028] In the formula, a is the actual linear acceleration at the reference center O of the carrier; Let be the instantaneous angular velocity vector of the carrier. , , These are the components of the instantaneous angular velocity along the X, Y, and Z axes; This is the angular acceleration vector of the carrier; The tangential acceleration component is denoted as . ; The centripetal acceleration component is denoted as . ; This refers to the high-frequency white noise of the sensor itself.

[0029] Because the high-frequency angular vibration components of the actual carrier are relatively small, while the centrifugal effect caused by continuous high-speed maneuvering and turning is very significant, and in the engineering practice of embedded microcontrollers, the discrete gyroscope signal with noise is also a concern. When performing differential calculations to determine angular acceleration, it will cause a significant amplification of high-frequency noise. Therefore, during the compensation process, the tangential acceleration term is ignored, while the centripetal acceleration term is retained.

[0030] To achieve efficient matrix-based calculations, an antisymmetric angular velocity matrix is ​​introduced. : ;

[0031] The centripetal acceleration component is converted into:

[0032] ;

[0033] Introducing the system angular velocity The compensated acceleration for:

[0034] ;

[0035] in The expansion is as follows:

[0036] ;

[0037] Substitution From:

[0038] ;

[0039] After compensation by the lever arm, the accelerometers distributed in different spatial positions are equivalent to the same point, namely the reference origin O, thus eliminating the sensor differences caused by the normal motion of the carrier.

[0040] (3) Construct a spatial variance vector and an adaptive measurement noise covariance matrix based on the data after size effect compensation;

[0041] The specific implementation method is as follows:

[0042] (3.1) Construct a spatial variance vector based on the data after size effect compensation; define The spatial expectation vector of the IMU array at time step is The calculation formula is as follows:

[0043] ;

[0044] in, express Time of the first Acceleration of an accelerometer after size compensation;

[0045] definition Spatial covariance matrix at time step as follows:

[0046] ;

[0047] in, The main diagonal elements of the matrix , , These represent the pure spatial variances of the accelerometer array along the three orthogonal axes X, Y, and Z, respectively; the main diagonal elements are extracted to construct the spatial variance vector at the current moment. : ;

[0048] (3.2) Construction based on spatial variance Adaptive measurement noise covariance matrix at time step :

[0049] ;

[0050] Expand into matrix form:

[0051] ;

[0052] in It is a sensitive adjustment matrix that is adjusted according to the actual vibration intensity. The static noise basis matrix of the IMU. , , These represent the components of static noise along the X, Y, and Z axes, respectively.

[0053] (4) Construct state equations based on quaternion differential equations for attitude prior estimation;

[0054] (4) Attitude angles are essentially the direction and angle of rotation from one coordinate system to another. Euler angles are the most intuitive method for describing attitude. Their core idea is to decompose any spatial rotation into three sequential rotations around different coordinate axes. Depending on the order of rotation around the axes, there are various ways to define Euler angles. The ZYX rotation order is commonly used, i.e., heading first, then pitch, and finally roll. The definitions are as follows:

[0055] Heading angle Around Department The axis rotates, The axis rotates to the direction of the longitudinal axis of the carrier projected onto the horizontal plane, and the range of rotation is... Pitch angle about the axis after the first rotation Rotation, the angle between the longitudinal axis of the carrier and the horizontal plane, range Roll angle After the second rotation Rotation of the axis, the angle between the carrier's lateral symmetry plane and the vertical plane, range .

[0056] Based on the ZYX rotation sequence around the axis, the coordinate transformation matrix from the navigation coordinate system to the vehicle coordinate system. It can be obtained by multiplying the three fundamental rotation matrices together:

[0057] ;

[0058] Conversely, the transformation matrix from the vehicle coordinate system to the navigation coordinate system for transpose:

[0059] .

[0060] In inertial navigation, quaternion methods are often used to solve attitude updates in order to more easily describe the angular motion of rigid bodies. This effectively avoids the singularity problem caused by Euler angles and simplifies the computation.

[0061] A state equation is constructed based on quaternion differential equations for attitude prior estimation; the prior state estimation equation for Kalman filtering is constructed as follows:

[0062] ;

[0063] In the formula, the state vector , All attitudes are represented using attitude quaternions, which are defined as follows: In the formula For real numbers, , , These are the three bases of quaternions; for The state vector at time t, To utilize The state vector at time step is obtained through iterative calculation. The state vector at any given time; The discrete state transition matrix is ​​calculated as follows:

[0064] ;

[0065] Represents a 4×4 identity matrix;

[0066] in The sampling period is The oblique symmetric matrix formed by angular velocities is represented as:

[0067] .

[0068] (5) Construct the covariance prediction equation, expressed as:

[0069] ;

[0070] The system process noise covariance characterizes the residual gyroscope random walk and broadband white noise after array fusion; for Error covariance of state estimation at time step, To utilize The error covariance of the state estimation at time step is obtained through iterative calculation. Error covariance of time-state estimation.

[0071] (6) An attitude observation equation is constructed based on the acceleration after size effect compensation and the measurement values ​​of the geomagnetic sensor;

[0072] In the Kalman filter algorithm, the attitude observation equation is expressed as:

[0073] ;

[0074] yes The system state at any given moment. yes Theoretical observations at time [time] The observation function in the Kalman filter observation equation is... For measuring noise.

[0075] In this embodiment, magnetic induction data is incorporated into attitude observation, based on The attitude observation vector is constructed from the geomagnetic sensor measurements at any time and the data after size effect compensation. Attitude observation vector at time step Acceleration after size effect compensation Compared with the measurement value of the geomagnetic sensor Together, they constitute six-dimensional data; Attitude observation vector at time step Represented as:

[0076] ;

[0077] in , for , The vector after modulus normalization is:

[0078] ;

[0079] The gravity vector in the reference coordinate system n With local geomagnetic reference vector Projected onto the carrier coordinate system b, the theoretical observation vector is extracted. :

[0080] ;

[0081] in The coordinate transformation matrix from the reference coordinate system (n-system) to the carrier coordinate system (b-system) is expressed as:

[0082] ;

[0083] Expand get The system of scalar equations is as follows:

[0084] ;

[0085] Taking partial derivatives of the scalar equations, we obtain the Jacobian matrix of the observation function with respect to the state variables in the Kalman filter at the current time. :

[0086] ;

[0087] in:

[0088] ;

[0089] ;

[0090] For the six-dimensional observation vector composed of triaxial acceleration and triaxial magnetic induction, the spatial variance vector is defined. :

[0091] ;

[0092] Right now:

[0093] ;

[0094] In the formula, This represents the inherent noise variance matrix of the geomagnetic sensor. It is a 3×3 zero matrix.

[0095] (7) Calculate the Kalman gain based on the spatial variance, correct the prior state, obtain the posterior estimate in quaternion form, and update the covariance matrix;

[0096] Based on spatial variance vector Calculate the Kalman gain matrix at the current time step. :

[0097] ;

[0098] This is the Jacobian matrix of the observation function with respect to the state variables in Kalman filtering;

[0099] State update: The prior state is corrected using joint measurement residuals to obtain a posterior estimate in quaternion form. :

[0100] ;

[0101] Normalization :

[0102] ;

[0103] Update the posterior covariance matrix for the next iteration:

[0104] .

[0105] (8) Convert quaternions to Euler angles. The specific method is as follows:

[0106] ;

[0107] For heading angle, For pitch angle, Let be the roll angle, which is the three-dimensional attitude angle at time k.

[0108] A high-frequency vibration noise interference with an amplitude of 5g and a frequency of 10Hz is injected midway through the multi-axis sinusoidal dynamic motion of the carrier (18-20s). The SVAAKF proposed in this invention is compared with the traditional EKF and Madgwick algorithms; the attitude results output by the three methods are shown to be... Figure 2 As shown in the figure, the SVAAKF provided by the present invention effectively suppresses the influence of sudden noise on the system attitude output.

[0109] Those skilled in the art will recognize that the embodiments described herein are intended to help the reader understand the principles of the invention, and should be understood that the scope of protection of the invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations based on the technical teachings disclosed in this invention without departing from the spirit of the invention, and these modifications and combinations are still within the scope of protection of this invention.

Claims

1. An array-adaptive Kalman filter attitude measurement method based on spatial variance, characterized in that, Includes the following steps: (1) Acquire inertial measurement data of each IMU in the IMU array; (2) Size effect compensation is performed on the inertial measurement data of each IMU based on the principle of rigid body mechanics; (3) Construct a spatial variance vector and an adaptive measurement noise covariance matrix based on the data after size effect compensation; (4) Construct state equations based on quaternion differential equations for attitude prior estimation; (5) Construct the covariance prediction equation: (6) An attitude observation equation is constructed based on the acceleration after size effect compensation and the measurement values ​​of the geomagnetic sensor; (7) Calculate the Kalman gain based on the spatial variance, correct the prior state, obtain the posterior estimate in quaternion form, and update the covariance matrix; (8) Convert the quaternion to Euler angles.

2. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 1, characterized in that, The specific implementation method of step (2) is as follows: the measured value of the i-th accelerometer Represented as: ； In the formula, a is the actual linear acceleration at the reference center O of the carrier; Let be the instantaneous angular velocity vector of the carrier. , , These are the components of the instantaneous angular velocity along the X, Y, and Z axes; This is the angular acceleration vector of the carrier; For tangential acceleration components, For the centripetal acceleration component, This refers to the high-frequency white noise of the sensor itself. During the compensation process, the tangential acceleration term is ignored, while the centripetal acceleration term is retained; the compensated acceleration is then obtained. for: 。 3. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 1, characterized in that, The specific implementation method of step (3) is as follows: (3.1) Construct a spatial variance vector based on the data after size effect compensation; define The spatial expectation vector of the IMU array at time step is The calculation formula is as follows: ； in, express Time of the first Acceleration of an accelerometer after size compensation; definition Spatial covariance matrix at time step as follows: ； Extracting the main diagonal elements to construct the spatial variance vector at the current time step : ; (3.2) Construction based on spatial variance Adaptive measurement noise covariance matrix at time step : ； in For sensitive adjustment matrix, This is the static noise basis matrix of the IMU.

4. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 3, characterized in that, The specific implementation method of step (4) is as follows: Construct the prior state estimation equation for Kalman filtering, which is expressed as: ； In the formula, the state vector , All attitudes are represented using attitude quaternions, which are defined as follows: In the formula For real numbers, , , These are the three bases of quaternions; for The state vector at time t, To utilize The state vector at time step is obtained through iterative calculation. The state vector at any given time; It is a discrete state transition matrix.

5. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 4, characterized in that, In step (5), the covariance prediction equation is expressed as: ； For the system process noise covariance, for Error covariance of state estimation at time step, To utilize The error covariance of the state estimation at time step is obtained through iterative calculation. Error covariance of time-state estimation.

6. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 5, characterized in that, The specific implementation method of step (6) is as follows: Attitude observation vector at time step Acceleration after size effect compensation Compared with the measurement value of the geomagnetic sensor Together they constitute, represented as: ； in , for , The vector after modulus normalization; The gravity vector in the reference coordinate system n With local geomagnetic reference vector Projected onto the carrier coordinate system b, the theoretical observation vector is extracted. : ； in This is the coordinate transformation matrix from the reference coordinate system to the vehicle coordinate system; Based on the definition of spatial variance vector : ； In the formula, This represents the inherent noise variance matrix of the geomagnetic sensor. It is a 3×3 zero matrix.

7. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 6, characterized in that, The implementation method of step (7) is as follows: based on the spatial variance vector Calculate the Kalman gain matrix at the current time step. : ； This is the Jacobian matrix of the observation function with respect to the state variables in Kalman filtering; State update: The prior state is corrected using joint measurement residuals to obtain a posterior estimate in quaternion form. : ； Normalization : ； Update the posterior covariance matrix for the next iteration: 。 8. The array adaptive Kalman filter attitude measurement method based on spatial variance according to claim 7, characterized in that, The method for converting quaternions to Euler angles is as follows: ； For heading angle, For pitch angle, This refers to the roll angle.

Images