Robot attitude estimation method of quaternion LI-EKF

Abstract

The invention belongs to the field of mobile robots, and particularly relates to a quaternion LI-EKF robot attitude estimation method, which comprises the following steps: acquiring measurement data; constructing an attitude estimation model, and defining a multiplication error; inputting measurement data into the attitude estimation model to obtain a discrete time state updating equation; calculating a Jacobian matrix according to the discrete time state updating equation; estimating a covariance matrix of the Jacobian matrix by adopting an EM algorithm; according to the covariance matrix, using an RTS smoother and a lagging first-order covariance smoother to obtain a smoothed state and covariance; establishing a log-likelihood function, solving the minimum value of the log-likelihood function, and updating the noise covariance matrix; estimating the posture of the robot according to the state at the current moment by adopting LI-EKF; according to the method, the quaternion and the left invariant extended Kalman filter are used for obtaining the data, so that the posture of the robot is more accurate.

Description

Technical Field

[0001] The invention belongs to the field of mobile robots, and in particular relates to a robot posture estimation method based on quaternion LI-EKF. Background Art

[0002] Kalman filters and their variants have been widely used in various estimation and control applications, especially in the field of robotics. When dealing with nonlinear systems such as attitude estimation, the extended Kalman filter (EKF) is often used to linearize the system around the current state estimate. The multiplicative extended Kalman filter (MEKF) emerged as a solution to deal with attitude estimation more efficiently, especially when quaternions are used to represent orientation. The MEKF operates on the error between the true quaternion and the estimated quaternion, solving the problems associated with over-parameterization and singularities. However, the MEKF may be limited when dealing with systems with strong nonlinearities or complex dynamics. Building on these advances, the left-invariant EKF (LI-EKF) has gained attention due to its ability to maintain the geometric structure of the state space, resulting in improved consistency and better convergence.

[0003] Accurate specification of the process noise covariance (Q) and measurement noise covariance (R) matrices is critical to the performance of the Kalman filter. These matrices represent the uncertainties in the system model and sensor measurements. Q and R are often unknown or time-varying, and if incorrectly specified, can lead to suboptimal performance or filter divergence. This has motivated the study of adaptive filtering techniques to estimate or adjust Q and R online during operation. Accurate estimation of these noise parameters is particularly important in attitude estimation problems, where system nonlinearities and measurement complexity can exacerbate the effects of incorrectly estimated noise covariance.

[0004] The first approach to attitude estimation utilized an additive extended Kalman filter, and while this approach was popular, it brought with it some conceptual and practical problems. The core conceptual flaw of the additive approach was to treat (unit norm) quaternions as vectors, whereas a more rigorous approach would be to treat them as Lie groups, closed under multiplication (rather than addition). Errors must also be defined in terms of multiplication rather than addition. It is obvious from the implementation that the noise is not Gaussian. In fact, in the attitude estimation problem, it is the tangent space of the manifold S3, not S3 itself, that takes a (nearly) Gaussian form.

[0005] None of the above considers the problem that the system noise parameters are unknown or vary over time and the noise covariance matrix (Q and R) in attitude estimation is difficult to specify accurately. Failure to consider these issues may lead to filter performance degradation or even divergence, which may seriously affect the accuracy of attitude estimation. Summary of the invention

[0006] In order to solve the problems existing in the above prior art, the present invention proposes a robot posture estimation method based on quaternion LI-EKF, which comprises:

[0007] S1. Obtain measurement data;

[0008] S2. Build a posture estimation model, define the multiplication error, and parameterize the error using exponential mapping; set the number of iterations;

[0009] S3, inputting the measurement data into the attitude estimation model to obtain the discrete time state update equation;

[0010] S4, calculating the Jacobian matrix according to the discrete-time state update equation;

[0011] S5, using the RTS smoothing filter to smooth the signal according to the Jacobian matrix;

[0012] S6, using a lagged first-order covariance smoother to process the smoothed signal through an EM algorithm to obtain a smoothed signal state and a covariance matrix;

[0013] S7, determine whether the current number of iterations has reached the maximum number of iterations, if not, increase the number of iterations by 1 and return to step S3, otherwise output the current state;

[0014] S8. Use LI-EKF to estimate the robot posture at the current state.

[0015] Beneficial effects of the present invention:

[0016] 1. High-precision attitude estimation: The present invention adopts the quaternion left-invariant extended Kalman filter (LI-EKF) to make full use of the geometric characteristics of quaternions, avoid the attitude parameter redundancy or singularity problems caused by the accumulation of addition errors in traditional methods, and significantly improve the accuracy of attitude estimation.

[0017] 2. Adaptive noise covariance adjustment: Combined with the EM algorithm, the process noise covariance (Q) and the measurement noise covariance (R) are estimated online in real time, solving the problem that the noise parameters in traditional filters depend on prior knowledge. This method can still maintain optimal filtering performance in an environment where the noise parameters are unknown or time-varying (such as sensor drift, dynamic interference), and avoid filter divergence caused by parameter mismatch.

[0018] 3. Strong robustness and anti-interference ability: The adaptive mechanism proposed in the present invention significantly reduces the sensitivity to initial noise parameters.

[0019] 4. Computational efficiency optimization: Through the collaborative design of the sliding window mechanism and the RTS smoother, the computational complexity is controlled within the real-time processing range while ensuring the accuracy of noise parameter estimation, solving the problem of large computational complexity of the traditional EM algorithm.

[0020] 5. Wide applicability: This method is suitable for posture estimation scenarios in complex dynamic environments, including but not limited to: aerospace, robotics, autonomous systems, etc. Its modular design can be seamlessly integrated into the existing sensor fusion framework (such as IMU, GPS, visual odometry) to improve the efficiency of multimodal data fusion. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is the overall flow chart of the present invention. DETAILED DESCRIPTION

[0022] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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 creative work are within the scope of protection of the present invention.

[0023] A quaternion left-invariant extended Kalman filter (LI-EKF) attitude estimation method is proposed, combined with an adaptive noise covariance estimation algorithm. The filter uses an iterative expectation maximization (EM) method to effectively estimate the covariance of process and measurement noise. First, the attitude estimation model is constructed, and the attitude is represented by quaternions. A continuous-time model is established, which includes the angular velocity measured by the gyroscope and the measurements of the accelerometer and magnetometer, and the effects of model noise and measurement noise are considered. Then, the multiplication error is defined, the unit quaternion is regarded as an element on the Lie group, the error is parameterized using an exponential mapping, and the discrete-time state update equation is derived. The Jacobian matrix is ​​calculated, and the noise covariance matrix is ​​estimated using the expectation maximization (EM) algorithm. The RTS smoother and the lagged first-order covariance smoother are run to obtain the smoothed state and covariance estimates, minimize the log-likelihood function, and update the noise covariance matrix. Finally, the extended Kalman filter algorithm is applied for state estimation.

[0024] A robot posture estimation method based on quaternion LI-EKF, the method comprising:

[0025] S1. Obtain measurement data;

[0026] S2. Build a posture estimation model, define the multiplication error, and parameterize the error using exponential mapping; set the number of iterations;

[0027] S3, inputting the measurement data into the attitude estimation model to obtain the discrete time state update equation;

[0028] S4, calculating the Jacobian matrix according to the discrete-time state update equation;

[0029] S5, using the RTS smoothing filter to smooth the signal according to the Jacobian matrix;

[0030] S6, using a lagged first-order covariance smoother to process the smoothed signal through an EM algorithm to obtain a smoothed signal state and a covariance matrix;

[0031] S7, determine whether the current number of iterations has reached the maximum number of iterations, if not, increase the number of iterations by 1 and return to step S3, otherwise output the current state;

[0032] S8. Use LI-EKF to estimate the robot posture at the current state.

[0033] In this embodiment, the EKF and attitude estimation model includes:

[0034] x k+1 =f(x k ,w k ) (1)

[0035] z k =h(x k ,v k ) (2)

[0036] Among them, x k is the state vector, z k is the measurement vector at time step k. k and v k are the model noise and measurement noise vectors, defined as:

[0037]

[0038] Among them, Q and R are w k and v k The corresponding noise covariance matrix of .

[0039] The system is linearized by computing the Jacobian matrix of the functions f and h with respect to the current estimates, namely:

[0040]

[0041] Among them, F k is the Jacobian matrix of the state transfer matrix at time k, is the state estimate at the kth time step, H k is the Jacobian matrix of the observation matrix at time k, is the prior state estimate at time k.

[0042] The state x and its error covariance P can now be estimated iteratively as:

[0043]

[0044]

[0045] in, is the state of the system at time step k, is the state at time step k-1, f is the state transition function, is the prior error covariance matrix, F k-1 is the Jacobian matrix of the state transfer function f at time k-1 with respect to state x, is the k-1 moment posterior error covariance matrix, is the transpose of the matrix, G k-1 is the Jacobian matrix of the state transfer function f at time k-1 with respect to the process noise w, Q is the process noise covariance matrix, is the transpose of the matrix, K k is the Kalman gain, is the transpose of the matrix, H k is the Jacobian matrix of the measurement function h with respect to the state x, L k is the Jacobian matrix of the measurement function h with respect to the measurement noise v, is the transpose of the matrix, z k is the sensor measurement at time step k, I is the identity matrix, is the prior attitude estimation, I4 is the fourth-order unit matrix, Δt is the time step, and Ω is ω k-1 is the process noise vector at time k-1.

[0046] The continuous-time model for attitude estimation using inertial sensors is:

[0047]

[0048] where q is of the form The unit quaternion, where q w ∈R is called the “real part” of the quaternion, q v ∈R 3 is called the "imaginary part" of the quaternion. The conjugate and inverse of a quaternion are defined similarly to complex numbers: Quaternion multiplication is written as It is called the Hamilton product. It can be encoded as a matrix multiplication:

[0049]

[0050] For quaternions p and q, Ξ and Ω represent left multiplication and right multiplication respectively, defined as:

[0051]

[0052] where [v] × , for v = [v x v y v z ] is an antisymmetric matrix:

[0053]

[0054] Quaternion vector multiplication is defined as:

[0055] in, is the estimated attitude, ω is the angular velocity measured by the gyroscope, η is the noise of the gyroscope, a is the measurement value of the accelerometer, which represents the acceleration of the object in the body coordinate system, m is the measurement value of the magnetometer, which represents the magnetic field of the object in the body coordinate system, g is the gravity vector of the earth, m0 is the magnetic field vector of the earth, and v a is the measurement noise of the accelerometer, assumed to be zero-mean Gaussian noise, ν a ~N(0,Σ a ), v m is the measurement noise of the magnetometer, assumed to be zero-mean Gaussian noise, ν m ~N(0,Σ m );I3 is the third-order identity matrix, p w and q w are the “real part” of the quaternion, p v and q v is the "imaginary part" of the quaternion; V T is the transpose of V.

[0056] In this embodiment, the unit norm constraint means that Only under this constraint does it represent the correct rotation. It can be deduced that the unit quaternion is consistent with the real three-dimensional sphere S 3 Points on are bijective, which can be used to represent the set of all unit quaternions: right is the set of all quaternions. It can be easily shown that for the unit quaternion, conjugate and inverse are equivalent.

[0057] is the angular velocity of the object measured by the gyroscope, is the intrinsic acceleration in the object's coordinate system measured by the accelerometer. is the magnetic field in the body coordinate system, measured by the magnetometer. In the absence of acceleration and low magnetic interference, the measurements of the accelerometer and magnetometer are the Earth's gravity field g and magnetic field m0, respectively, rotated to the body coordinate system. These measurements are corrupted by zero-mean Gaussian noise η and ν, defined as follows:

[0058]

[0059] Discretizing (11) with time step t yields:

[0060]

[0061]

[0062] z k =h(q k )+v k (17)

[0063] Where η is the noise of the gyroscope, is a Gaussian distribution, Σ η is the noise covariance matrix of the gyroscope, v a is the measurement noise of the accelerometer, Σ a is the accelerometer measurement noise covariance matrix, v m is the measurement noise of the magnetometer, Σ m is the measurement noise covariance matrix of the magnetometer, I4 is the fourth-order unit matrix, Δt is the time step, Ω represents the right multiplication, ω k is the angular velocity at time k, z k is the measurement vector at time k.

[0064] In this embodiment, the left invariant extended Kalman filter includes: for the estimated state and the true state q, both are unit quaternions, and the quaternion multiplication error is written as:

[0065]

[0066] if If and q are both unit quaternions representing rotations, then ε must be of unit norm and represent the "difference" between two rotations. The filter is named after the invariance of this error to left multiplication: for any quaternion Γ,

[0067] Index Mapping It can be defined as:

[0068]

[0069] in, represents the estimated state, and q represents the true state; is a three-dimensional Euclidean space, which is a unit quaternion manifold S 3 The tangent space is used to represent the linearized form of the attitude error; S is a three-dimensional sphere, and ξ is a vector in the tangent space.

[0070] Substituting equation (18) into equation (11) and solving it with Exp(x / 2)=ε, we obtain:

[0071]

[0072] Among them, ω is the angular velocity vector, which represents the rotation speed of the object, [.] × is an antisymmetric matrix, x is the state vector of attitude estimation error, and η is the noise in the gyroscope measurement.

[0073] Expression (20) is obtained from the approximation with small error: Exp(ξ)→[1 0 0 0] T ,ξ→[0 0 0] T ; Discretize (20) and get:

[0074] x k+1 =(I3-Δt[ω k ] × )x k -Δtη k (twenty one)

[0075] Among them, x k+1 represents the state matrix at time k+1, Δt represents the sampling time interval, ω k represents the angular velocity at time k, x k represents the state matrix at time k, η k is zero-mean Gaussian noise.

[0076] The EKF can now be run with x as the state vector. The filter parameters {F, H, Q, R} are defined. The measurement model remains the same as for the additive case given by Eq. (17). However, following the EKF approach, h needs to be discretized with respect to x (rather than q). Recall that Then, the measurement sensitivity matrix H can be calculated as:

[0077]

[0078] in, is the estimated state at time k, H k is the Jacobian matrix of the function h.

[0079] From (21) and (17), we can deduce:

[0080] F k =I3-Δt[ω k ]× (twenty three)

[0081] Q=(Δt) 2 Σ η ,R=Σ ν (twenty four)

[0082] Where Q is the updated process noise covariance matrix, Σ η is the noise covariance matrix of the gyroscope, R is the updated measurement noise covariance matrix, Σ v is the noise covariance matrix of the accelerometer and magnetometer.

[0083] Using the parameters defined above and x as the state vector, we iterate equations (6)-(10). Note that x is not the direction estimate, but only the parameterization of the estimation error. The actual estimate for each iteration is calculated as follows:

[0084]

[0085] in, is the prior attitude estimation, I4 is the fourth-order unit matrix, Δt is the time step, and Ω is ω k-1 is the process noise vector at time k-1.

[0086] In this embodiment, the noise covariance estimation includes: The noise statistics of the sensor may be unknown, so it is necessary to estimate Σ during the filter operation η and Σ ν Expectation-maximization (EM) is a popular technique that has been applied to the joint state parameter estimation problem in Kalman filtering.

[0087] LI-EKF is performed on a window of length n with an initial parameter estimate Θ 0 = {Q 0 , R 0} Run. Then apply the Rauch-Tung-Striebel (RTS) smoother and lag-one covariance smoother given below on the same window.

[0088] Initialize the RTS smoother as:

[0089]

[0090] Where i = n-1, ..., 1, 0;

[0091]

[0092] in, is the initial state estimate of the RTS smoother, is the state estimate at the nth time step, P nis the initial error covariance matrix of the RTS smoother, is the state estimation error covariance matrix of the nth time step, J i is the smoothed gain matrix, is the posterior error covariance matrix at the i-th time step, F i is the Jacobian matrix of the state transfer matrix, is the prior error covariance matrix of the i+1th time step, P i is the smoothed state estimation error covariance matrix at the i-th time step, is the transpose of the matrix, is the smoothed state estimate at the i-th time step, is the posterior state estimate at time i, is the prior state estimate at time i+1.

[0093] Similarly, the lag-one covariance smoother is initialized as:

[0094]

[0095] Among them, I is the identity matrix, K n is the Kalman gain matrix at time n, H n is the Jacobian matrix of the observation matrix at time n, F n-1 is the Jacobian matrix of the state transfer matrix at time n-1, is the prior covariance matrix at time n-1.

[0096] And iterate i=n-1,...,1

[0097]

[0098] In this embodiment, the covariance matrix of the Jacobian matrix estimated by the EM algorithm includes:

[0099] Step 1, constructing a log-likelihood function, and formulating the log-likelihood function through information; processing the smoothed signal using a lagged first-order covariance smoother according to the log-likelihood function to generate a smoothed state and covariance estimate of the signal;

[0100] The log-likelihood function is formulated using information and is expressed as:

[0101]

[0102] Current estimate (Θ j = {Q j ,R j}) and the expressions for the smoothed state and covariance estimates based on the current parameters are:

[0103]

[0104] Where G represents the log-likelihood function, Θ is the current noise covariance matrix estimate, n is the length of the sliding window, which represents the number of samples used to estimate the noise covariance, Q is the process noise covariance matrix, R is the measurement noise covariance matrix, tr is the trace of the matrix, that is, the sum of the diagonal elements of the matrix, S 11 ,S 10 ,S 00 S is an intermediate variable for calculating the noise covariance matrix. 11 represents the autocorrelation of the state vector, S 10 represents the cross-correlation between the state vector and the lagged state vector, S 00 represents the autocorrelation of the lagged state vector, z i is the measured value at the i-th time step, P i is the Jacobian matrix of the measurement model, H i is the smoothed state error covariance matrix, is the estimated value of the state quantity.

[0105] Step 2, Minimization: The log-likelihood function is minimized with respect to the parameters in the current iteration, producing updated parameter estimates given by:

[0106]

[0107] Among them, Q j is the process noise covariance matrix after the jth iteration update, R j is the updated measurement noise covariance matrix for the jth iteration.

[0108] The filter-smoother-EM process is iterated on the same window until convergence of the parameters is achieved, or until a maximum number of iterations is reached. The filter then proceeds to estimate the noise covariance matrix.

[0109] The above embodiments further illustrate the purpose, technical solutions and advantages of the present invention in detail. It should be understood that the above embodiments are only preferred implementation modes of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made to the present invention within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot posture estimation method based on quaternion LI-EKF, characterized in that: include: S1. Obtain measurement data; S2. Build a posture estimation model, define the multiplication error, and parameterize the error using exponential mapping; Set the number of iterations; S3, inputting the measurement data into the attitude estimation model to obtain the discrete time state update equation; S4, calculating the Jacobian matrix according to the discrete-time state update equation; S5, using the RTS smoothing filter to smooth the signal according to the Jacobian matrix; S6, using a lagged first-order covariance smoother to process the smoothed signal through an EM algorithm to obtain a smoothed signal state and a covariance matrix; S7, determine whether the current number of iterations has reached the maximum number of iterations, if not, increase the number of iterations by 1 and return to step S3, otherwise output the current state; S8. Use LI-EKF to estimate the robot posture at the current state.

2. The robot posture estimation method of the quaternion LI-EKF according to claim 1, characterized in that: The measurement data includes angular velocity measured by a gyroscope, acceleration measured by an accelerometer, and magnetic field measured by a magnetometer.

3. The robot posture estimation method of the quaternion LI-EKF according to claim 1, characterized in that: Constructing the attitude estimation model includes: using the inertial sensor to perform the continuous time model of attitude estimation; discretizing the continuous time model by using the time step to obtain the attitude estimation model.

4. The robot posture estimation method of a quaternion LI-EKF according to claim 1, characterized in that: The posture estimation model processes data including: Step 1: Define the multiplication error and exponential mapping; the multiplication error is: The index is mapped to Defined as in, represents the estimated state, and q represents the true state; is a three-dimensional Euclidean space, which is a unit quaternion manifold S 3 The tangent space is used to express the linearized form of the attitude error; S is a three-dimensional sphere, and ξ is a vector in the tangent space; Step 2: Set the parameters Exp(x / 2)=ε, Exp(ξ)→[1 0 0 0] according to the multiplication error and exponential mapping. T ,ξ→[0 00] T ; Step 3: Solve the posture estimation model using the set parameters to obtain: Among them, ω is the angular velocity vector, which represents the rotation speed of the object, [.] × is an antisymmetric matrix, x is the state vector of attitude estimation error, and η is the noise in gyroscope measurement; Step 4: Discretize the expression obtained in step 3 to obtain: x k+1 =(I3-Δt[ω k ] × )x k -Dtη k Among them, x k+1 represents the state matrix at time k+1, Δt represents the sampling time interval, ω k represents the angular velocity at time k, x k represents the state matrix at time k, η k is zero-mean Gaussian noise.

5. The robot posture estimation method of quaternion LI-EKF according to claim 1, characterized in that: The Jacobian matrix is: in, is the estimated state at time k, H k is the Jacobian matrix of the function h.

6. The robot posture estimation method of quaternion LI-EKF according to claim 1, characterized in that: Estimating the covariance matrix using the EM algorithm includes: Step 1: construct a log-likelihood function and formulate the log-likelihood function through information; Step 2: Process the smoothed signal using a lagged first-order covariance smoother according to the log-likelihood function to generate a smoothed state and covariance estimate of the signal; Step 3: Calculate the minimum value of the likelihood function and generate update parameters based on the minimum value; Step 4: Update the covariance matrix according to the update parameters to obtain the covariance matrix.

7. The robot posture estimation method of quaternion LI-EKF according to claim 1, characterized in that: The RTS smoother processes the data including: in, is the initial state estimate of the RTS smoother, is the state estimate at the nth time step, P n is the initial error covariance matrix of the RTS smoother, is the state estimation error covariance matrix of the nth time step, J i is the smoothed gain matrix, is the posterior error covariance matrix at the i-th time step, F i is the Jacobian matrix of the state transfer matrix, is the prior error covariance matrix of the i+1th time step, P i is the smoothed state estimation error covariance matrix at the i-th time step, is the transpose of the matrix, is the smoothed state estimate at the i-th time step, is the posterior state estimate at time i, is the prior state estimate at time i+1; The lag-one covariance smoother is initialized as: Among them, I is the identity matrix, K n is the Kalman gain matrix at time n, H n is the Jacobian matrix of the observation matrix at time n, F n-1 is the Jacobian matrix of the state transfer matrix at time n-1, is the prior covariance matrix at time n-1.

8. The robot posture estimation method of quaternion LI-EKF according to claim 1, characterized in that: Using LI-EKF to estimate the robot posture at the current state includes: the LI-EKF algorithm iteratively estimates the state x and its error covariance P, and its expression is: The actual estimate for each iteration is calculated as: in, is the state of the system at time step k, is the state at time step k-1, f is the state transition function, is the prior error covariance matrix, F k-1 is the Jacobian matrix of the state transfer function f at time k-1 with respect to state x, is the k-1 moment posterior error covariance matrix, is the transpose of the matrix, G k-1 is the Jacobian matrix of the state transfer function f at time k-1 with respect to the process noise w, Q is the process noise covariance matrix, is the transpose of the matrix, K k is the Kalman gain, is the transpose of the matrix, H k is the Jacobian matrix of the measurement function h with respect to the state x, L k is the Jacobian matrix of the measurement function h with respect to the measurement noise v, is the transpose of the matrix, z k is the sensor measurement at time step k, I is the identity matrix, is the prior attitude estimation, I4 is the fourth-order unit matrix, Δt is the time step, and Ω is ω k-1 is the process noise vector at time k-1.

Images