Anti-disturbance attitude estimation method based on DBN noise covariance prediction

Abstract

The invention discloses an anti-disturbance attitude estimation method based on DBN noise covariance prediction, which does not adopt a constant value to replace a measurement noise covariance matrix in an MEKF, but predicts a measurement covariance matrix in a Kalman filtering step by utilizing the characteristic that a DBN network can output a variance and a mean value, so that the noise covariance has a non-Gaussian form. And the error problem caused by inaccurate measurement noise covariance of the sensor in the traditional filtering process is solved. Meanwhile, due to the self-adaptive learning capability of the Bayesian network, Kalman filtering has higher anti-interference capability, and Kalman gain calculation is more effective in the MEKF filtering process. According to the method, the dynamic Bayesian network and Kalman filtering are fused, and the attitude estimation method which can adapt to non-Gaussian measurement noise and is higher in anti-jamming capability and more accurate in filtering result is provided.

Description

Technical Field

[0001] The present invention relates to a field, in particular to a disturbance-resistant attitude estimation method based on DBN noise covariance prediction. Background Art

[0002] With the continuous development of posture estimation, 3D motion tracking technology has been widely used in many fields, such as VR, AR, robotic control, and healthcare. Different sensors, such as visual sensors and electromyography sensors, are used in various fields according to actual needs.

[0003] Magnetic, angular rate, and gravity (MARG) sensors are widely used in attitude estimation due to their lightweight, low-cost, and high-integration capabilities. MARG sensors simultaneously provide angular velocity, acceleration, and magnetic field information, making them excellent for attitude estimation in dynamic environments. However, sensor noise can lead to inaccurate measurements. To address these issues, researchers have attempted to improve accuracy through filtering. However, traditional Kalman filtering approaches cannot meet the filtering requirements of nonlinear systems, and a more optimal filtering approach is needed to handle changes such as rotation. After experimenting with methods such as the EKF and MEKF, researchers discovered that the MEKF (Multiplicative Extended Kalman Filter) excels in filtering rotating systems. Therefore, they proposed the MEKF approach to achieve more accurate attitude estimation. By using the MEKF to filter the multi-source information from the MARG sensor, the MEKF aims to reduce noise interference and achieve more precise and reliable attitude estimation. However, in practical applications, the accuracy of attitude estimation remains uncertain. Accelerometers and gyroscopes suffer from unavoidable bias. Although this noise is approximated using Gaussian white noise, the noise distribution still differs from the actual noise distribution. This leads to inaccurate MEKF results when encountering non-Gaussian noise. The emergence of DBNs (Dynamic Bayesian Networks) can address the aforementioned non-Gaussian noise situation. Unlike other neural networks, DBNs have nodes that are normally distributed rather than constant, and the network can output both variance and mean. Therefore, in time series analysis, the network can output a distribution of predicted values. Using DBNs to predict the sensor's measurement noise distribution matrix eliminates the need for fixed Gaussian noise processing, making the MEKF more robust to interference and resolving the aforementioned issues. Therefore, this paper proposes a disturbance-resistant attitude estimation method based on DBN noise covariance prediction to address the issue of non-Gaussian measurement noise and improve MEKF accuracy. Summary of the Invention

[0004] The purpose of the present invention is to provide a disturbance-resistant attitude estimation method based on DBN noise covariance prediction to solve the problems raised in the above background technology.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] A disturbance-resistant attitude estimation method based on DBN noise covariance prediction includes the following steps:

[0007] Step 1: Establish a DBN discrete time prediction model, initialize the network nodes of the DBN discrete time prediction model to Gaussian distribution N~(0,1), initialize the attitude quaternion q0 and covariance matrix

[0008] Step 2: Train the DBN discrete-time prediction model, using the MARG sensor measurements as input and the measurement noise covariance matrix as output. The pose label values ​​are obtained through a high-precision optical motion capture system.

[0009] Step 3: Construct the state equation and observation equation based on quaternion and define the state variable x;

[0010] Step 4: State prediction, calculate the estimated attitude quaternion and covariance matrix

[0011] Step 5: Estimate the measurement noise covariance matrix R at time k through the DBN discrete time prediction model k ;

[0012] Step 6: Status update, posture information collected by MARG sensor Calculate the observation matrix H k , according to the R obtained in step 5 k Calculate the Kalman filter gain K k , thus obtaining the posterior state estimate at time k and covariance matrix

[0013] Step 7, based on the result from step 6 and Update the attitude quaternion at time k

[0014] Step 8: Repeat steps 4 to 7 to obtain the pose estimation for all k moments.

[0015] Furthermore, the DBN discrete time prediction model in step 1 is a model with a 9-dimensional input and a 6*6 noise covariance matrix output. The input is composed of a three-dimensional acceleration a x ,ay ,a z , three-dimensional angular velocity g x ,g y ,g z , three-dimensional magnetic field m x ,m y ,m z Composition, expressed as: [a x ,a y ,a z ,g x ,g y ,g z ,m x ,m y ,m z ], the output is the estimated observation noise covariance matrix, expressed as: is the acceleration measurement noise covariance, is the measurement noise variance of the magnetometer, O 3×3 is a 3×3 zero matrix; the model includes 10 hidden layers, and the number of nodes in each hidden layer is [9*2 3 ,9*2 4 ,9*2 5 ,9*2 6 ,9*2 7 ,9*2 8 ,9*2 7 ,9*2 6 ,9*2 5 ,9*2 4 ]; Each node of the DBN discrete time prediction model conforms to its own distribution μ i 、 are the mean and variance of the distribution, both of which are adjustable parameters.

[0016] Furthermore, the posture tag value in step 2 is a quaternion obtained by a high-precision optical motion capture system;

[0017] The training process of the DBN discrete-time prediction model includes: measuring and recording a set of data through the MARG sensor. After the measurement is completed, the measurement value at the initial moment is input into the DBN discrete-time prediction model to predict the measurement noise covariance matrix R0, further calculating the initial Kalman gain K0, using K0 to obtain the initial posterior state estimate value, calculating the initial posture quaternion through the initial posterior state estimate value and comparing it with the label value, calculating the loss function, backpropagating to adjust the node distribution parameters, and then reading the measurement value at the next moment to repeat the training.

[0018] Furthermore, in step 3, the state equation of the quaternion-based attitude estimation system is as follows:

[0019] xk =Φ k x k-1 +v k

[0020] where x k is the estimated value of the state vector at time k, and the state matrix Φ k ≈I+A·Δt, A is the coefficient matrix, is the skew-symmetric matrix of angular velocity, I is the identity matrix, Δt is the time step, v k is the process noise, which is used to represent the uncertainty of the model;

[0021] The observation equation is as follows:

[0022]

[0023] are the values ​​of the accelerometer and magnetometer in the sensor coordinate system, are the estimated acceleration and magnetometer values, is the skew-symmetric matrix of accelerometer and magnetometer values, f b is the actual acceleration, η a ,η m are the measurement noise of the accelerometer and magnetometer, respectively.

[0024] Furthermore, the state variable x in step 3 is defined as:

[0025]

[0026] Where δθ represents the Euler angle estimation error, and δβ represents the gyroscope bias estimation error.

[0027] Furthermore, the estimated attitude quaternion in step 4 is and covariance matrix The calculation follows the following formula:

[0028]

[0029] Where, represents the derivative of the quaternion with respect to time at time k-1, q k-1 is the k-1 moment quaternion, is the estimated angular velocity at time k-1, represents the quaternion at time k, Δt is a time step, is the estimated covariance matrix at time k, Φ k-1 is the state transition matrix, Transpose it, is the posterior covariance matrix at time k-1, Q is the process noise covariance matrix, Represents quaternion multiplication; are the covariance of the gyroscope angle random walk noise and the covariance of the gyroscope bias instability noise, respectively, and are independent zero-mean Gaussian white noises.

[0030] Furthermore, in step 5, after the DBN discrete time prediction model is trained, the measurement data of the MARG sensor is input into the DBN discrete time prediction model to obtain the estimated measurement noise covariance R k .

[0031] Furthermore, the observation matrix H in step 6 is k , Kalman filter gain K k , the posterior estimate and covariance MEKF modeling used:

[0032]

[0033] in They are the antisymmetric matrices of the accelerometer and magnetometer parameters in the sensor coordinate system, K k is the Kalman gain at time k, It is H k The transpose of is the updated value of the state vector at time k, are the measurements of the accelerometer and magnetometer, g n 、m n are the reference values ​​of gravity vector and geomagnetic vector respectively, are the estimated attitude quaternion and its conjugate, is the k-time posterior estimate covariance matrix.

[0034] Furthermore, the attitude quaternion at time k in step 7 is The update process is as follows:

[0035]

[0036] q e,k represents the error quaternion at time k, is the attitude error estimate, q k represents a real quaternion, is the estimated quaternion, q k conjugation, β k , β k-1 are the estimated values ​​of the gyroscope bias error at time k and time k-1, is the estimated value of the gyroscope bias error at time k.

[0037] Compared with the prior art, the present invention has the following beneficial effects: by introducing a dynamic Bayesian network, the present invention solves the problem of non-Gaussian noise distribution of the sensor, enables the MEKF to better adapt to the actual noise distribution, enhances the anti-interference ability of the MEKF, and improves the accuracy of filtering. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 is a flow chart of the present invention;

[0039] Figure 2 This is a schematic diagram of the DBN discrete-time prediction model proposed in the present invention;

[0040] Figure 3 It is a structural block diagram of the present invention. DETAILED DESCRIPTION

[0041] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0042] Reference Figure 1 、 Figure 2 、 Figure 3 A disturbance-resistant posture estimation method based on DBN noise covariance prediction is provided for human posture estimation. The method comprises the following steps:

[0043] Step 1: Establish a DBN discrete time prediction model, initialize the network nodes of the DBN discrete time prediction model to Gaussian distribution N~(0,1), initialize the attitude quaternion q0 and covariance matrix

[0044] The DBN discrete time prediction model is a 9-dimensional input model that outputs a 6*6 noise covariance matrix. The input consists of a three-dimensional acceleration a x ,a y ,a z , three-dimensional angular velocity g x ,g y ,g z , three-dimensional magnetic field m x ,m y ,m z Composition, expressed as: [a x ,a y ,a z ,g x ,g y ,g z ,m x ,my ,m z ], the output is the estimated observation noise covariance matrix, expressed as: is the acceleration measurement noise covariance, is the measurement noise variance of the magnetometer, O 3×3 is a 3×3 zero matrix; the model includes 10 hidden layers, and the number of nodes in each hidden layer is [9*2 3 ,9*2 4 ,9*2 5 ,9*2 6 ,9*2 7 ,9*2 8 ,9*2 7 ,9*2 6 ,9*2 5 ,9*2 4 ]; Each node of the DBN discrete time prediction model conforms to its own distribution μ i 、 are the mean and variance of the distribution, both of which are adjustable parameters.

[0045] Step 2: Train the model. A set of data is measured and recorded using the MARG sensor. After the measurement is complete, the initial measurement value is used as the input to the DBN discrete-time prediction model. The prediction passes through the DBN discrete-time prediction model's feedforward process and reaches the output layer. The output measurement noise covariance matrix R0 is used to calculate the initial Kalman gain K0, which is used to obtain the initial posterior state estimate. The initial pose quaternion is calculated from the initial posterior state estimate and compared with the label quaternion obtained by OptiTrack's high-precision optical motion capture system. The loss function is calculated, backpropagation is performed, and node distribution parameters are adjusted. The next measurement value is then read and training is repeated.

[0046] Step 3: Construct the state equation of the quaternion-based attitude estimation system and define the state variable x. The specific formula is as follows:

[0047] Equation of state:

[0048] x k =Φ k x k-1 +v k

[0049] where x k is the estimated value of the state vector at time k, and the state matrix Φ k ≈I+A·Δt, A is the coefficient matrix, is the skew-symmetric matrix of angular velocity, I is the identity matrix, Δt is the time step, v kis the process noise, which is used to represent the uncertainty of the model.

[0050] Observation equation:

[0051]

[0052] are the values ​​of the accelerometer and magnetometer in the sensor coordinate system, are the estimated acceleration and magnetometer values, is the skew-symmetric matrix of accelerometer and magnetometer values, f b is the actual acceleration, η a ,η m are the measurement noise of the accelerometer and magnetometer, respectively.

[0053] State variables:

[0054]

[0055] δθ represents the Euler angle estimation error, and δβ represents the gyroscope bias estimation error.

[0056] Step 4: State prediction. Calculate the quaternion at time k based on the quaternion at time k-1 and the posterior covariance matrix. and the estimated covariance matrix The expression is as follows:

[0057]

[0058] Where, represents the derivative of the quaternion with respect to time at time k-1, q k-1 is the k-1 moment quaternion, is the estimated angular velocity at time k-1, represents the quaternion at time k, Δt is a time step, is the estimated covariance matrix at time k, Φ k-1 is the state transition matrix, Transpose it, is the posterior covariance matrix at time k-1, Q is the process noise covariance matrix, where are the covariance of the gyroscope angle random walk noise and the covariance of the gyroscope bias instability noise, respectively, and are independent zero-mean Gaussian white noises.

[0059] Step 5: Input the value of MARG nine-axis obtained at time k into the DBN discrete time prediction model to obtain the output R k value.

[0060] Step 6: Collect the posture information Calculate the observation matrix H k, and combined with the R obtained in step 5 k Calculate the Kalman gain K k , get the posterior state estimate at time k and the posterior covariance matrix The specific formula is as follows:

[0061]

[0062] They are the antisymmetric matrices of the accelerometer and magnetometer parameters in the sensor coordinate system. is the estimated covariance matrix at time k, K k is the Kalman gain at time k, R k is the noise covariance matrix output by the DBN network, is the updated value of the state vector at time k, are the measurements of the accelerometer and magnetometer, g n 、m n They are the reference values ​​of gravity vector and geomagnetic vector respectively. are the estimated attitude quaternion and its conjugate, represents quaternion multiplication, is the k-time posterior estimate covariance matrix.

[0063] Step 7: Update the position quaternion at time k based on the result from step 6. The specific formula is as follows:

[0064]

[0065] q e,k represents the error quaternion at time k, is the attitude error estimate, q k represents a real quaternion, is the estimated quaternion, q k conjugation, β k , β k-1 are the estimated values ​​of the gyroscope bias error at time k and time k-1, is the estimated value of the gyroscope bias error at time k.

[0066] Step 8: Repeat steps 4-7 until all pose estimates are obtained.

[0067] This paper proposes a disturbance-resistant attitude estimation method based on DBN noise covariance prediction. By integrating a dynamic Bayesian network, this method outputs a non-Gaussian noise covariance matrix, which can more accurately reflect the reliability of sensor data, enhance the adaptability of the MEKF to actual noise, and improve its anti-interference performance.

[0068] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A disturbance-resistant attitude estimation method based on DBN noise covariance prediction, characterized in that: The following steps are involved: Step 1: Establish a DBN discrete time prediction model, initialize the network nodes of the DBN discrete time prediction model to Gaussian distribution N~(0,1), initialize the attitude quaternion q0 and covariance matrix Step 2: Train the DBN discrete-time prediction model, using the MARG sensor measurements as input and the measurement noise covariance matrix as output. The pose label values ​​are obtained through a high-precision optical motion capture system. Step 3: Construct the state equation and observation equation based on quaternion and define the state variable x; Step 4: State prediction, calculate the estimated attitude quaternion and covariance matrix Step 5: Estimate the measurement noise covariance matrix R at time k through the DBN discrete time prediction model k ; Step 6: Status update, posture information collected by MARG sensor Calculate the observation matrix H k , according to the R obtained in step 5 k Calculate the Kalman filter gain K k , thus obtaining the posterior state estimate at time k and covariance matrix Step 7, based on the result from step 6 and Update the attitude quaternion at time k Step 8: Repeat steps 4 to 7 to obtain the pose estimation for all k moments.

2. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: The DBN discrete time prediction model in step 1 is a model with a 9-dimensional input and a 6*6 noise covariance matrix output. The input is composed of a three-dimensional acceleration a x ,a y ,a z , three-dimensional angular velocity g x ,g y ,g z , three-dimensional magnetic field m x ,m y ,m z Composition, expressed as: [a x ,a y ,a z ,g x ,g y ,g z ,m x ,m y ,m z ], the output is the estimated observation noise covariance matrix, expressed as: is the acceleration measurement noise covariance, is the measurement noise variance of the magnetometer, O 3×3 is a 3×3 zero matrix; the model includes 10 hidden layers, and the number of nodes in each hidden layer is [9*2 3 ,9*2 4 ,9*2 5 ,9*2 6 ,9*2 7 ,9*2 8 ,9*2 7 ,9*2 6 ,9*2 5 ,9*2 4 ]; Each node of the DBN discrete time prediction model conforms to its own distribution μ i 、 are the mean and variance of the distribution, both of which are adjustable parameters.

3. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: The posture tag value in step 2 is a quaternion obtained by a high-precision optical motion capture system; The training process of the DBN discrete-time prediction model includes: measuring and recording a set of data through the MARG sensor. After the measurement is completed, the measurement value at the initial moment is input into the DBN discrete-time prediction model to predict the measurement noise covariance matrix R0, further calculating the initial Kalman gain K0, using K0 to obtain the initial posterior state estimate value, calculating the initial posture quaternion through the initial posterior state estimate value and comparing it with the label value, calculating the loss function, backpropagating to adjust the node distribution parameters, and then reading the measurement value at the next moment to repeat the training.

4. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: In step 3, the state equation of the quaternion-based attitude estimation system is as follows: x k =Φ k x k-1 +v k where x k is the estimated value of the state vector at time k, and the state matrix Φ k ≈I+A·Δt, A is the coefficient matrix, is the skew-symmetric matrix of angular velocity, I is the identity matrix, Δt is the time step, v k is the process noise, which is used to represent the uncertainty of the model; The observation equation is as follows: are the values ​​of the accelerometer and magnetometer in the sensor coordinate system, are the estimated acceleration and magnetometer values, is the skew-symmetric matrix of accelerometer and magnetometer values, f b is the actual acceleration, η a ,η m are the measurement noise of the accelerometer and magnetometer, respectively.

5. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: In step 3, the state variable x is defined as: Where δθ represents the Euler angle estimation error, and δβ represents the gyroscope bias estimation error.

6. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: The estimated attitude quaternion in step 4 and covariance matrix The calculation follows the following formula: Where, represents the derivative of the quaternion with respect to time at time k-1, q k-1 is the k-1 moment quaternion, is the estimated angular velocity at time k-1, represents the quaternion at time k, Δt is a time step, is the estimated covariance matrix at time k, Φ k-1 is the state transition matrix, Transpose it, is the posterior covariance matrix at time k-1, Q is the process noise covariance matrix, Represents quaternion multiplication; are the covariance of the gyroscope angle random walk noise and the covariance of the gyroscope bias instability noise, respectively, and are independent zero-mean Gaussian white noises.

7. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: In step 5, after the DBN discrete time prediction model is trained, the measurement data of the MARG sensor is input into the DBN discrete time prediction model to obtain the estimated measurement noise covariance R k .

8. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: The observation matrix H in step 6 k , Kalman filter gain K k , the posterior estimate and covariance MEKF modeling used: in They are the antisymmetric matrices of the accelerometer and magnetometer parameters in the sensor coordinate system, K k is the Kalman gain at time k, It is H k The transpose of is the updated value of the state vector at time k, are the measurements of the accelerometer and magnetometer, g n 、m n are the reference values ​​of gravity vector and geomagnetic vector respectively, are the estimated attitude quaternion and its conjugate, is the k-time posterior estimated covariance matrix.

9. The method for anti-disturbance attitude estimation based on DBN noise covariance prediction according to claim 1, characterized in that: The attitude quaternion at time k in step 7 The update process is as follows: q e,k represents the error quaternion at time k, is the attitude error estimate, q k represents a real quaternion, is the estimated quaternion, q k conjugation, β k , β k-1 are the estimated values ​​of the gyroscope bias error at time k and time k-1, is the estimated value of the gyroscope bias error at time k.

Images