MEKF attitude estimation method based on physical information neural network

Abstract

The invention discloses an MEKF attitude estimation method based on a physical information neural network, and the method comprises the steps: dynamically predicting an observation matrix of an MEKF through the dynamic modeling capability and constraint embedding characteristics of the physical information neural network, and extracting sensor sequence features through the long-range dependence capturing capability of a time domain convolutional network; and meanwhile, the physical rule constraint is embedded into network training to ensure the physical rationality of a prediction result. Through data-driven nonlinear modeling and a recursive estimation framework of the MEKF, the limitation that the MEKF is high in dependence on noise covariance and poor in dynamic adaptability is effectively overcome, and the precision and the anti-interference capability of attitude estimation in a complex environment are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of underwater robot posture estimation, and in particular to a MEKF posture estimation method based on physical information neural network. Background Art

[0002] Underwater robots (AUVs) are widely used in marine resource exploration, hydrological data collection, and underwater intelligent operations. Estimating their attitude is crucial for ensuring stable and precise robot operation. Early studies employed the Euler angle-based Kalman filter method, which achieves attitude estimation by integrating angular rate. However, this method has inherent drawbacks. During the update phase, singularization caused by gimbal lock can affect the stability of the Kalman gain and even destroy the positive definiteness of the covariance matrix. To overcome this limitation, researchers have proposed incorporating quaternions into the extended Kalman filter framework, utilizing quaternion parameterization to avoid singularities and approximating the nonlinear relationship between the quaternion and angular velocity using a first-order Taylor expansion. However, this method significantly increases the linearization error when the angular acceleration varies dramatically, resulting in a decrease in estimation accuracy. To overcome these limitations, researchers have proposed the multiplicative extended Kalman filter (MEKF) method. This method represents the quaternion attitude as the product of the nominal state and the tangent space error state. The error state is locally parameterized using Lie algebras to strictly maintain the linearity of the error dynamics, meeting the linearization requirements of the Kalman filter. However, MEKF still suffers from high computational complexity and reliance on linearization assumptions, resulting in insufficient estimation accuracy in complex dynamic scenes. In recent years, with the development of deep learning technology, researchers have leveraged the nonlinear representation capabilities of neural networks to attempt to construct end-to-end mapping models between sensor signals and posture parameters. However, purely data-driven black-box models are overly dependent on training data and fail to adhere to the fundamental laws of kinematics, resulting in insufficient generalization capabilities of the trained models.

[0003] In response to the above technical bottlenecks, the present invention proposes a MEKF attitude estimation method based on a physical information neural network. This method dynamically predicts the MEKF observation matrix through the dynamic modeling capabilities and constraint embedding characteristics of the physical information neural network, extracts sensor sequence features using the long-range dependency capture capability of the time domain convolutional network (TCN), and embeds physical law constraints into network training to ensure the physical rationality of the prediction results. The present invention effectively overcomes the limitations of MEKF's strong dependence on noise covariance and poor dynamic adaptability through data-driven nonlinear modeling and the recursive estimation framework of MEKF, significantly improving the accuracy and anti-interference ability of attitude estimation in complex environments. Summary of the Invention

[0004] The purpose of the present invention is to provide a MEKF attitude estimation method based on physical information neural network 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 MEKF attitude estimation method based on physical information neural network includes the following steps:

[0007] Step 1, establishing a physical information neural network model;

[0008] Step 2: introduce physical information constraints and train the physical information neural network model;

[0009] Step 3: Establish a discrete posture estimation system based on the physical information neural network model;

[0010] Step 4: Introduce measurement and calculate filter gain K t , update the state posterior estimate and its covariance

[0011] Step 5: Correct the error quaternion q e,t and gyro bias β t ;

[0012] Step 6: State propagation, calculation of estimated quaternion and its covariance

[0013] Step 7: Repeat steps 4 to 6 to obtain the pose estimation results at all times.

[0014] Furthermore, the physical information neural network model in step 1 includes a time domain convolutional network layer and a fully connected layer. The input of the physical information neural network model is the inertial measurement unit data, and the output is the predicted gravity acceleration reference value. and the reference value of the geomagnetic field

[0015] Reference value of gravity acceleration predicted by physical information neural network model and the reference value of the geomagnetic field To calculate the observation matrix H t , expressed as:

[0016]

[0017] Where [·×] represents a skew-symmetric matrix.

[0018] Furthermore, the step 2 includes:

[0019] Step 2.1: construct the physical information constraint based on the optimal gain theorem, which is expressed as:

[0020]

[0021] Where, is the partial derivative, J(·) is the state posterior estimate The covariance P t + The trace of J(K t )=tr(P t + ), tr(·) represents the trace of the calculation matrix, K t is the filter gain, H t is the observation matrix, H t The transposed matrix, P t - is the covariance matrix of the state prediction value, R is the observation noise covariance;

[0022] Step 2.2, construct the composite loss function L total , composite loss function L total Including physical information loss L physics and data loss L data , expressed as:

[0023]

[0024] L total =λL physics +(1-λ)L data

[0025] Where, P t - is the state prediction covariance at time t, are the gravitational acceleration and geomagnetic field in the sensor coordinate system predicted by the physical information neural network model, g n , m n are the gravitational acceleration and geomagnetic field in the navigation coordinate system, is the label quaternion q true The conjugate quaternion of is the quaternion multiplication operation, λ is the regularization coefficient;

[0026] Furthermore, the step 2 also includes: step 2.3, the trained label quaternion is obtained through a high-precision optical positioning system, the training adopts the Adam optimizer optimization algorithm, the initial learning rate is set to 0.001, and the learning rate is adjusted by the cosine annealing algorithm.

[0027] Furthermore, in step 3,

[0028] The state equation of the discrete attitude estimation system is expressed as:

[0029] x t =F t xt-1 +w t-1

[0030] Where x t , x t-1 are the state variables at time t and t-1 respectively, F t is the state matrix, w t-1 is the process noise;

[0031] The observation equation based on the physical information neural network model is expressed as:

[0032] z t =H t x t +ν t

[0033] Where z t is the observed value, x t is the state variable at time t, H t is the observation matrix predicted by the physical information neural network, ν t To measure noise;

[0034] The state variable x of the discrete pose estimation system is defined as:

[0035] x=[ΔθΔβ] Τ

[0036] Where Δθ is the estimated error of the attitude angle, Δβ is the estimated error of the gyroscope bias, and T represents the transpose.

[0037] Furthermore, in step 4,

[0038] Filter gain K t The calculation is expressed as:

[0039]

[0040] Where, P t - is the state prediction covariance at time t, H t and are the observation matrix and its transposed matrix respectively, R is the observation noise covariance;

[0041] State posterior estimate and its covariance P t + The update process is expressed as:

[0042]

[0043] Where, is the state prediction value at time t, z t is the observation value, and I is the unit matrix.

[0044] Furthermore, in step 5,

[0045] Error quaternion q e,t and gyro bias β t The correction process can be expressed as:

[0046]

[0047] Where, represents the attitude angle error part of the state posterior estimation, represents the gyroscope bias error portion of the state posterior estimate, To estimate the quaternion, q t is a real quaternion, is the quaternion multiplication operation, β t-1 is the gyroscope bias at time t-1;

[0048] Through the error quaternion q e,t After correcting the attitude estimation, the real quaternion q t After normalization, it is expressed as:

[0049]

[0050] Where, q t The conjugate quaternion of .

[0051] Furthermore, the quaternion estimated in step 6 is and its covariance The calculation process is expressed as:

[0052]

[0053] Where, To estimate the angular velocity after compensating for the deviation, is the actual angular velocity, q t and are real quaternions and their derivatives, and are the estimated quaternion and its derivative, is the estimated quaternion at time t+1, Δt is the sampling time, F t and F t Τ are the state matrix and its transposed matrix, P t + is the posterior estimated covariance at time t, and Q is the process noise covariance.

[0054] Compared with the existing technology, the present invention has the following advantages: It proposes a MEKF attitude estimation method based on a physical information neural network. This method dynamically predicts the MEKF observation matrix through the dynamic modeling capabilities and constraint embedding characteristics of the physical information neural network. It also uses the long-range dependency capture capability of the time-domain convolutional network to extract sensor sequence features. At the same time, it embeds physical law constraints into network training to ensure the physical rationality of the prediction results. Through data-driven nonlinear modeling and the recursive estimation framework of the MEKF, the present invention effectively overcomes the limitations of the MEKF, such as its strong dependence on noise covariance and poor dynamic adaptability, providing a new technical path for robust attitude estimation in dynamic and complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0056] Figure 2 It is a structural block diagram of the present invention;

[0057] Figure 3 It is a schematic diagram of the physical information neural network of the present invention. DETAILED DESCRIPTION

[0058] 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.

[0059] Reference Figure 1 , Figure 2 and Figure 3 , a MEKF attitude estimation method based on physical information neural network, comprising the following steps:

[0060] Step 1: Establish a physical information neural network model. The physical information neural network model includes a time domain convolutional network layer (TCN) and a fully connected layer. The input of the physical information neural network model is the data of the inertial measurement unit, and the output is the predicted gravity acceleration reference value. and the reference value of the geomagnetic field

[0061] Reference value of gravity acceleration predicted by physical information neural network model and the reference value of the geomagnetic field To calculate the observation matrix H t , which can be expressed as:

[0062]

[0063] Where [·×] represents a skew-symmetric matrix.

[0064] like Figure 3 As shown in the figure, the time domain convolutional network layer uses causal convolution and dilated convolution modules to capture long-term dependencies, and uses residual connections to ensure the stability of training.

[0065] Step 2: Introduce physical information constraints and train the physical information neural network model, including:

[0066] Step 2.1, constructing physical information constraints based on the optimal gain theorem, can be expressed as:

[0067]

[0068] Where, is the partial derivative, J(·) is the state posterior estimate The covariance P t + The trace of J(K t )=tr(P t + ), tr(·) represents the trace of the calculation matrix, K t is the filter gain, H t is the observation matrix, H t The transposed matrix of is the covariance matrix of the state prediction value, and R is the observation noise covariance.

[0069] Step 2.2, construct the composite loss function L total , composite loss function L total Including physical information loss L physics and data loss L data , which can be expressed as:

[0070]

[0071] L total =λL physics +(1-λ)L data

[0072] Where, P t - is the state prediction covariance at time t, are the gravitational acceleration and geomagnetic field in the sensor coordinate system predicted by the physical information neural network model, g n , m n are the gravitational acceleration and geomagnetic field in the navigation coordinate system, is the label quaternion qtrue The conjugate quaternion of is the quaternion multiplication operation, λ is the regularization coefficient, and its value is 0.5.

[0073] In step 2.3, the trained label quaternion is obtained through a high-precision optical positioning system. The training adopts the Adam optimizer optimization algorithm, the initial learning rate is set to 0.001, and the learning rate is adjusted by the cosine annealing algorithm.

[0074] Step 3: Establish a discrete posture estimation system based on the physical information neural network model. The state equation of the discrete posture estimation system can be expressed as:

[0075]

[0076] Where, are the estimated values ​​of the state at time t and t-1, respectively, t is the state matrix, w t-1 is the process noise.

[0077] The observation equation based on the physical information neural network model can be expressed as:

[0078]

[0079] Where z t is the observed value, is the estimated state value at time t, H t is the prediction result of the physical information neural network, ν t To measure noise.

[0080] The state variable of the discrete pose estimation system is defined as x = [ΔθΔβ] Τ , Δθ is the estimated error of the attitude angle, and Δβ is the estimated error of the gyroscope bias.

[0081] Step 4: Introduce measurement and calculate filter gain K t , update the state posterior estimate and its covariance

[0082] Filter gain K t The calculation can be expressed as:

[0083]

[0084] Where, P t - is the state prediction covariance at time t, H t and are the observation matrix and its transposed matrix respectively, and R is the observation noise covariance.

[0085] State posterior estimate and its covariance P t + The update process can be expressed as:

[0086]

[0087] Where, is the state prediction value at time t, K t is the filter gain, z t is the observed value, H t is the observation matrix, I is the identity matrix, P t - is the state prediction covariance at time t.

[0088] Step 5: Correct the error quaternion q e,t and gyro bias β t , the correction process can be expressed as:

[0089]

[0090] Where, represents the attitude angle error part of the state posterior estimation, represents the gyroscope bias error portion of the state posterior estimate, To estimate the quaternion, q t is a real quaternion, is the quaternion multiplication operation, β t-1 is the gyroscope bias at time t-1.

[0091] Through the error quaternion q e,t After correcting the attitude estimate, the real quaternion q t After normalization, it can be expressed as:

[0092]

[0093] Where, is the quaternion multiplication operation, q t The conjugate quaternion of .

[0094] Step 6: State propagation, calculation of estimated quaternion and its covariance The calculation process can be expressed as:

[0095]

[0096]

[0097] Where, To estimate the angular velocity after compensating for the deviation, is the actual angular velocity, q t and are real quaternions and their derivatives, and are the estimated quaternion and its derivative, is the estimated quaternion at time t+1, Δt is the sampling time, F t and F t Τ are the state matrix and its transposed matrix, P t + is the posterior estimated covariance at time t, and Q is the process noise covariance.

[0098] Step 7: Repeat steps 4 to 6 to obtain the pose estimation results at all times.

[0099] 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 MEKF attitude estimation method based on physical information neural network, characterized in that: The following steps are involved: Step 1, establishing a physical information neural network model; Step 2: introduce physical information constraints and train the physical information neural network model; Step 3: Establish a discrete posture estimation system based on the physical information neural network model; Step 4: Introduce measurement and calculate filter gain K t , update the state posterior estimate and its covariance Step 5: Correct the error quaternion q e,t and gyro bias β t ; Step 6: State propagation, calculation of estimated quaternion and its covariance Step 7: Repeat steps 4 to 6 to obtain the pose estimation results at all times.

2. The MEKF attitude estimation method based on physical information neural network according to claim 1 is characterized in that: The physical information neural network model in step 1 includes a time domain convolutional network layer and a fully connected layer. The input of the physical information neural network model is the inertial measurement unit data, and the output is the predicted gravity acceleration reference value. and the reference value of the geomagnetic field Reference value of gravity acceleration predicted by physical information neural network model and the reference value of the geomagnetic field To calculate the observation matrix H t , expressed as: Where [·×] represents a skew-symmetric matrix.

3. The MEKF attitude estimation method based on physical information neural network according to claim 1 is characterized in that: The step 2 includes: Step 2.1: construct the physical information constraint based on the optimal gain theorem, which is expressed as: Where, is the partial derivative, J(·) is the state posterior estimate The covariance P t + The trace of J(K t )=tr(P t + ), tr(·) represents the trace of the calculation matrix, K t is the filter gain, H t is the observation matrix, H t The transposed matrix, P t - is the covariance matrix of the state prediction value, R is the observation noise covariance; Step 2.2, construct the composite loss function L total , composite loss function L total Including physical information loss L physics and data loss L data , expressed as: Where, P t - is the state prediction covariance at time t, are the gravitational acceleration and geomagnetic field in the sensor coordinate system predicted by the physical information neural network model, g n , m n are the gravitational acceleration and geomagnetic field in the navigation coordinate system, is the label quaternion q true The conjugate quaternion of is the quaternion multiplication operation, and λ is the regularization coefficient.

4. The MEKF attitude estimation method based on physical information neural network according to claim 3 is characterized in that: The step 2 also includes: step 2.3, the trained label quaternion is obtained through a high-precision optical positioning system, the training adopts the Adam optimizer optimization algorithm, the initial learning rate is set to 0.001, and the learning rate is adjusted by the cosine annealing algorithm.

5. The MEKF attitude estimation method based on physical information neural network according to claim 1 is characterized in that: In the step 3, The state equation of the discrete attitude estimation system is expressed as: x t =F t x t-1 +w t-1 Where x t , x t-1 are the state variables at time t and t-1 respectively, F t is the state matrix, w t-1 is the process noise; The observation equation based on the physical information neural network model is expressed as: z t =H t x t +ν t Where z t is the observed value, x t is the state variable at time t, H t is the observation matrix predicted by the physical information neural network, ν t To measure noise; The state variable x of the discrete pose estimation system is defined as: x=[ΔθΔβ] Τ Where Δθ is the estimated error of the attitude angle, Δβ is the estimated error of the gyroscope bias, and T represents the transpose.

6. The MEKF attitude estimation method based on physical information neural network according to claim 1 is characterized in that: In the step 4, Filter gain K t The calculation is expressed as: Where, P t - is the state prediction covariance at time t, H t and are the observation matrix and its transposed matrix respectively, R is the observation noise covariance; State posterior estimate and its covariance P t + The update process is expressed as: Where, is the state prediction value at time t, z t is the observation value, and I is the unit matrix.

7. The MEKF attitude estimation method based on physical information neural network according to claim 1 is characterized in that: In the step 5, Error quaternion q e,t and gyro bias β t The correction process can be expressed as: Where, represents the attitude angle error part of the state posterior estimation, represents the gyroscope bias error portion of the state posterior estimate, To estimate the quaternion, q t is a real quaternion, is the quaternion multiplication operation, β t-1 is the gyroscope bias at time t-1; Through the error quaternion q e,t After correcting the attitude estimation, the real quaternion q t After normalization, it is expressed as: Where, q t The conjugate quaternion of .

8. The MEKF attitude estimation method based on physical information neural network according to claim 1 is characterized in that: The quaternion is estimated in step 6 and its covariance The calculation process is expressed as: Where, To estimate the angular velocity after compensating for the deviation, is the actual angular velocity, q t and are real quaternions and their derivatives, and are the estimated quaternion and its derivative, is the estimated quaternion at time t+1, Δt is the sampling time, F t and F t Τ are the state matrix and its transposed matrix, P t + is the posterior estimated covariance at time t, and Q is the process noise covariance.

Images