Moving object attitude control method based on improved LKF

Abstract

The invention relates to the technical field of attitude control, in particular to a moving object attitude control method based on an improved LKF. Predicting prior attitude estimation at the next moment; calculating a process error covariance matrix; calculating a measurement attitude; calculating a quaternion measurement error covariance matrix; constructing feature vectors of acceleration interference and geomagnetic interference amplitude levels, and calculating an interference amplitude level value by using a Gaussian function model; updating the process error covariance matrix and the measurement error covariance matrix by using the interference amplitude level value; calculating a priori attitude estimation error covariance matrix by using the updated process error covariance matrix; calculating a Kalman gain matrix by using the priori attitude estimation error covariance matrix and the updated measurement error covariance matrix; calculating attitude posteriori estimation at the next moment; and performing attitude estimation at the next moment. According to the method, the problems of insufficient estimation precision and poor interference suppression capability of the attitude observation error covariance matrix of the existing LKF are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of attitude control, and in particular to a method for controlling the attitude of a moving object based on an improved LKF. Background Art

[0002] Magnetic inertial measurement units (MIMUs) have significant advantages such as low cost, compact size, light weight, and no reliance on external artificial signal sources. These advantages make MIMUs widely used in many fields of moving objects, including human motion analysis, motion perception of drones, surface ships, underwater vehicles, and ground vehicles.

[0003] The implementation of attitude calculation relies on the fusion processing of the output data of three sensors, but the attitude error directly affects the stability and control accuracy of the moving object.

[0004] The linear Kalman filter (LKF) uses linear process equations and measurement equations, and its attitude estimation results are optimal. However, the LKF has limitations and cannot effectively estimate the gyroscope bias. When the gyroscope bias needs to be estimated, the process equation will be transformed into a nonlinear form, and a nonlinear Kalman filter is required.

[0005] Problems with existing LKF applications include:

[0006] 1. The estimation accuracy of the attitude observation error covariance matrix is ​​insufficient;

[0007] 2. Poor interference suppression capability. Summary of the Invention

[0008] Aiming at the deficiencies of the existing methods, the present invention solves the problems of insufficient estimation accuracy of the attitude observation error covariance matrix and poor interference suppression capability of the existing LKF.

[0009] The technical solution adopted by the present invention is: a method for controlling the posture of a moving object based on an improved LKF comprises the following steps:

[0010] Step 1: Collect MIMU data of moving objects;

[0011] Step 2: Set the initial quaternion parameters of MIMU;

[0012] As a preferred embodiment of the present invention, the MIMU includes: a gyroscope, an accelerometer, and a magnetometer.

[0013] As a preferred embodiment of the present invention, the initial parameters include: an initial estimate and an initial error covariance matrix.

[0014] Step 3: Use the state transfer matrix and the current moment posterior attitude estimate to predict the next moment prior attitude estimate; calculate the process error covariance matrix;

[0015] Step 4: Calculate the measurement attitude by fusing the measurement values ​​of the accelerometer and magnetometer; calculate the quaternion measurement error covariance matrix;

[0016] As a preferred embodiment of the present invention, the measurement error covariance matrix The formula is:

[0017]

[0018] in,

[0019]

[0020] and are the variances of the normalized measurement errors of the accelerometer and magnetometer on any axis respectively; I is the identity matrix.

[0021] Step 5: Construct the characteristic vectors of the acceleration interference and geomagnetic interference amplitude levels, and calculate the interference amplitude level value using the Gaussian function model; use the interference amplitude level value to update the process error covariance matrix and the measurement error covariance matrix;

[0022] As a preferred embodiment of the present invention, the formula of the characteristic vector of the interference amplitude level is:

[0023]

[0024] Among them, b′ g·k and b′ m·k are the unnormalized measurements of the gravity vector and the geomagnetic field vector at time k, respectively, and T is the transpose.

[0025] As a preferred embodiment of the present invention, the formula for the interference amplitude level value is:

[0026]

[0027] Where X0 and ∑ are respectively k The mean and variance of the Gaussian distribution, X k The eigenvector at the kth moment, tansig is the tansig function.

[0028] As a preferred embodiment of the present invention, the update formula is:

[0029]

[0030] Among them, λ k is the interference amplitude level value.

[0031] Step 6: Calculate the prior attitude estimation error covariance matrix using the state transfer matrix and the updated process error covariance matrix;

[0032] Step 7: Calculate the Kalman gain matrix using the prior attitude estimation error covariance matrix and the updated measurement error covariance matrix;

[0033] Step 8: Use the Kalman gain matrix to calculate the posterior estimate of the attitude at the next moment;

[0034] Step 9: Use the Kalman gain matrix to calculate the attitude estimate at the next moment.

[0035] As a preferred embodiment of the present invention, the improved LKF mobile object posture control system includes: a memory for storing instructions executable by a processor; and a processor for executing the instructions to implement the improved LKF mobile object posture control method.

[0036] As a preferred embodiment of the present invention, a computer readable medium stores computer program code, and when the computer program code is executed by a processor, the computer program code implements a method for controlling the attitude of a moving object using an improved LKF.

[0037] Beneficial effects of the present invention:

[0038] 1. By improving the LKF, it can be applied to the fields of human motion analysis, motion perception of drones, surface ships, underwater vehicles, and ground vehicles, reducing attitude errors and improving the stability and control accuracy of moving objects;

[0039] 2. Compared with NgμLKF and NgλLKF methods, the present invention and λ k Combined with the above, the static and dynamic performance of LKF is significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 is a flow chart of a method for controlling the attitude of a moving object based on an improved LKF of the present invention;

[0041] Figure 2 is a pitch angle estimation error diagram of NgλLKF, NgμLKF and NnμLKF of the present invention;

[0042] Figure 3 is a roll angle estimation error diagram of NgλLKF, NgμLKF and NnμLKF of the present invention;

[0043] Figure 4 It is the azimuth estimation error diagram of NgλLKF, NgμLKF and NnμLKF of the present invention. DETAILED DESCRIPTION

[0044] The present invention will be further described below in conjunction with the accompanying drawings and embodiments. This figure is a simplified schematic diagram, which only illustrates the basic structure of the present invention in a schematic manner, and therefore only shows the components related to the present invention.

[0045] In terms of attitude observation error covariance matrix estimation, there are the algebraic quaternion algorithm (AQUA) and its corresponding error covariance matrix, as well as the error covariance matrix of the new quaternion determination method (NQDM). However, the error covariance matrix calculation process of these two methods is extremely cumbersome. Although the proposer of the quaternion estimator (QUEST) gave a formula for calculating the error covariance matrix, the LKF based on the quaternion estimator did not use this formula. Some methods directly set the error covariance matrix to a constant diagonal matrix. Although this simplified process reduces the computational complexity, it affects the estimation accuracy.

[0046] To address the problem of interference suppression, neural network models, hidden Markov models, and fuzzy logic models have been used for interference identification; however, these models have the disadvantages of complex structure and time-consuming calculations, making it difficult to meet real-time requirements; methods based on covariance matching have been proven in practice to be insufficiently adaptable when dealing with complex motion scenes; in contrast, threshold-based methods are widely used in some applications due to their ease of operation and efficient calculations, but their reliance on historical sensor data results in a delay in responding to rapid changes in sensor output, making it impossible to respond to dynamic interference in a timely manner.

[0047] like Figure 1 As shown, a method for controlling the attitude of a moving object based on an improved LKF includes the following steps:

[0048] Step 1: Collect MIMU data of moving objects;

[0049] Moving objects include: human movement, drones, surface ships, underwater vehicles and ground vehicles.

[0050] MIMU models include: MTi.

[0051] MIMUs are commonly used in the automotive field. By integrating accelerometers, gyroscopes, and magnetometer sensors, they provide high-precision measurements of vehicle motion. For example:

[0052] 1. Autonomous driving and ADAS positioning and navigation: When GPS signals are lost (such as in tunnels and urban canyons), the MIMU integrates inertial navigation (INS) with wheel speedometers and GNSS to achieve continuous positioning (such as DR dead reckoning), matching it with high-precision maps, and improving the global path planning accuracy of autonomous vehicles;

[0053] 2. Attitude estimation: real-time monitoring of vehicle roll, pitch, and yaw angles for hill assist and rollover warning;

[0054] 3. Electronic Stability Control System (ESC) body dynamic control, MIMU detects the vehicle's lateral acceleration and yaw rate, and ESC prevents slipping by braking a single wheel or adjusting the torque (such as ESP function).

[0055] Step 2: Set the initial quaternion parameters of MIMU;

[0056] MIMU includes: gyroscope, accelerometer and magnetometer;

[0057] The initial parameters of the quaternion include: the initial estimate q0 and the corresponding initial error covariance matrix P0;

[0058] At the initial sampling moment, the QUEST algorithm is used to obtain the quaternion initial estimate q0 and its initial error covariance matrix P0;

[0059] Among them, quaternion is the fusion of accelerometer and magnetometer measurement data;

[0060] Step 3: Use the state transfer matrix and the current moment posterior attitude estimate to predict the next moment prior attitude estimate; calculate the process error covariance matrix;

[0061] At sampling time k, the gyroscope measurement value is obtained, and the attitude at time k is predicted using formula (1). The formula is:

[0062] q k / k-1 =Φ k ·q k-1 / k-1 (1)

[0063] Among them, Φ k is the state transfer matrix at the kth moment, q k-1 / k-1 is the posterior pose estimate at sampling time k-1.

[0064]

[0065] Among them, ω x 、ω y and ω z are the measurement values ​​of the gyroscope along the X, Y and Z axes respectively; T s is the sampling time.

[0066] The calculation formula of the process error covariance matrix is:

[0067]

[0068] in, is the measurement error variance of the gyroscope in each axis; Q kis the process error covariance matrix; q0, q1, q2 and q3 are quaternions q k / k-1 The four elements of q k / k-1 It can be expressed as q k / k-1 =q 0·(k / k-1) +q 1·(k / k-1) i+q 2·(k / k-1) j+q 3·(k / k-1) k, for estimation, is expressed in vector form as: q k / k-1 =[q 1·(k / k-1) ,q 2·(k / k-1) ,q 3·(k / k-1) ,q 0·(k / k-1) ] T ; For simplicity, q k / k-1 The subscript “·k / k-1” in each element is omitted;

[0069] Step 4: Use the QUEST algorithm to calculate the measured attitude by fusing the measurements of the accelerometer and magnetometer Calculate the quaternion measurement error covariance matrix generated by the QUEST algorithm

[0070] Assume that the normalized gravity vector and geomagnetic field vector are expressed as and

[0071] Measurement error covariance matrix The formula is:

[0072]

[0073] in,

[0074] and are the variances of the normalized measurement errors of the accelerometer and magnetometer on any axis respectively; I is the unit matrix;

[0075] Step 5: Construct the characteristic vector and Gaussian function model of the acceleration interference and geomagnetic interference amplitude levels;

[0076] Among them, the formula of the characteristic vector of the interference amplitude level is:

[0077]

[0078] Among them, b′ g·k and b′ m·k are the unnormalized measured values ​​of the gravity vector and the geomagnetic field vector at time k, respectively.

[0079] The formula of the Gaussian function model is:

[0080]

[0081] Where X0 and ∑ are respectively k The mean and variance of the Gaussian distribution it obeys;

[0082] The formula for the tansig function is:

[0083]

[0084] The purpose of using the tansig function is to convert λ k Restricted to the interval [0,1].

[0085] Then, Q k and Through λ k Perform adaptive adjustment, the specific implementation is as follows:

[0086]

[0087] X of the present invention k It is a mathematical representation of the amplitude level of the indirect characterization of linear acceleration interference and geomagnetic interference. After substituting the eigenvector value into the Gaussian function model, the interference level λ at each sampling moment is k will be calculated; the greater the interference level, the k The lower the k It is then used in Equation 13 to achieve dynamic adjustment of the covariance matrix to ultimately suppress the impact of interference.

[0088] Step 6: Calculate the prior attitude estimation error covariance matrix P k / k-1 ;

[0089]

[0090] Among them, P k-1 is the posterior attitude estimation error covariance matrix at time k-1.

[0091] Step 7: Calculate the Kalman gain matrix;

[0092] Kalman gain matrix K k The formula is:

[0093]

[0094] Step 8. Calculate the posterior estimate of the attitude at time k using the following formula:

[0095]

[0096] Step 9: Estimate the next moment posture P k ;

[0097] In order to prepare for the next round of attitude estimation, the posterior attitude estimation error covariance matrix P at time k k Update as follows:

[0098] P k =P k / k-1 -K k P k / k-1 (17)

[0099] Experimental process:

[0100] To verify the performance of the improved LKF of the present invention, actual tests were conducted. A MIMU (Model MTi) manufactured by Movellag, USA, was used in the test. During the test, it was manually assisted to perform approximately periodic rotations. Before and after the rotation, the MTi remained stationary. An electromagnetic tracking system manufactured by Polhemus, USA, was used as the attitude reference. The standard deviations of the measurement errors of the gyroscope, accelerometer, and magnetometer were 0.006 rad / s and 0.008 m / s, respectively. 2 and 0.001 Gauss; the sampling rates of the MTi and electromagnetic tracking system are 256 Hz and 240 Hz, respectively.

[0101] By combining NQDM algorithm, and λ k (abbreviated as NgλLKF) the attitude estimation error generated by the LKF, that is, the method of the present invention; by combining the NQDM algorithm, and μ k (abbreviated as NgμLKF) generated by the attitude estimation error; and the attitude estimation error generated by combining the NQDM algorithm, and μ k (abbreviated as NnμLKF) the pose estimation error generated by the LKF, plotting Figures 2 to 4 Comparison of pitch angle estimation, roll angle estimation and azimuth angle estimation; It should be noted that the attitude is expressed in Euler angles; Among them, The covariance matrix of the quaternion measurement error designed by the NQDM algorithm proposer (Bayesian Optimization for Fine-Tuning EKF Parameters in UAVAttitude and Heading Reference System Estimation.Aerospace) for the algorithm;

[0102] Threshold-based model μ k (Instead of λ k Adjust Q k and ) is constructed by the following formula:

[0103]

[0104] The two thresholds appearing in the formula are determined based on the Laida criterion (also known as the 3σ criterion).

[0105] By comparing the performance of NgμLKF and NnμLKF, we can find that It can improve the static performance of LKF, that is, the curve in the time interval [8000,14231] in the figure; in fact, The accuracy of can affect the Kalman gain matrix K k The accuracy of , and subsequently affects the accuracy of attitude correction, which ultimately affects the accuracy of attitude posterior estimation (Equation 13); compared to The accuracy is higher, which explains why the performance of NgμLKF is better than that of NnμLKF, and we can draw the following conclusion: Can improve the static performance of LKF.

[0106] By comparing the performance of NgμLKF and NgλLKF, we can find that λ k It can improve the dynamic performance of LKF, that is, the curve in the time interval [6000,7400] in the figure; it is obvious that the feature constructed by the present invention It can fully and accurately reflect the amplitude of linear acceleration and geomagnetic interference, because once the linear acceleration interference (or geomagnetic interference) increases, then |b′ g·k |(or |b′ m·k |) will begin to deviate from the magnitude of the local gravitational acceleration (or the Earth's magnetic field), and the accelerometer observes the vector b′ g·k and b′ m·k The angle between It will also begin to deviate from b′ when the carrier is in an undisturbed state. g·k and b′ m·k The angle between the two; then, through the mapping effect of the constructed Gaussian model (Formula 11), the level of carrier linear acceleration and geomagnetic interference at each sampling moment will be accurately estimated; the Gaussian model is a probabilistic model, which can make an accurate estimate of the interference level according to the eigenvalue; finally, using the output result of the Gaussian model, through the adaptive adjustment of the covariance matrix (Formula 13), the LKF can achieve accurate suppression of interference, thereby improving its dynamic performance; the advantage of the present invention is that the constructed interference suppression method does not use the historical information of the accelerometer and the geomagnetic sensor, so while achieving accurate interference suppression, it does not affect the dynamic response capability of the LKF.

[0107] Therefore, it is clear that by and λ kCombined with other technologies, both the static and dynamic performance of LKF can be improved.

[0108] With the above-described preferred embodiments of the present invention as a guide, and with reference to the above description, relevant personnel are fully capable of making various changes and modifications without departing from the technical scope of this invention. The technical scope of this invention is not limited to the contents of the specification and must be determined according to the scope of the claims.

Claims

1. A method for controlling the attitude of a moving object based on an improved LKF, characterized in that: The following steps are involved: Step 1: Collect MIMU data of moving objects; Step 2: Set the initial quaternion parameters of MIMU; Step 3: Use the state transfer matrix and the current moment posterior attitude estimate to predict the next moment prior attitude estimate; calculate the process error covariance matrix; Step 4: Calculate the measured attitude by fusing the measurements of the accelerometer and magnetometer; Calculate the quaternion measurement error covariance matrix; Step 5: Construct the characteristic vectors of the acceleration interference and geomagnetic interference amplitude levels, and calculate the interference amplitude level value using the Gaussian function model; use the interference amplitude level value to update the process error covariance matrix and the measurement error covariance matrix; Step 6: Calculate the prior attitude estimation error covariance matrix using the state transfer matrix and the updated process error covariance matrix; Step 7: Calculate the Kalman gain matrix using the prior attitude estimation error covariance matrix and the updated measurement error covariance matrix; Step 8: Use the Kalman gain matrix to calculate the posterior estimate of the attitude at the next moment; Step 9: Use the Kalman gain matrix to calculate the attitude estimate at the next moment.

2. The method for controlling the attitude of a moving object based on the improved LKF according to claim 1, characterized in that: Measurement error covariance matrix The formula is: in, and are the variances of the normalized measurement errors of the accelerometer and magnetometer on any axis respectively; I is the unit matrix.

3. The method for controlling the attitude of a moving object based on the improved LKF according to claim 1, wherein: The formula for the eigenvector of the interference amplitude level is: Among them, b′ g·k and b′ m·k are the unnormalized measurements of the gravity vector and the geomagnetic field vector at time k, respectively, and T is the transpose.

4. The method for controlling the attitude of a moving object based on the improved LKF according to claim 3, characterized in that: The formula for the interference amplitude level is: Where X0 and ∑ are respectively k The mean and variance of the Gaussian distribution, X k The eigenvector at the kth moment, tansig is the tansig function.

5. The method for controlling the attitude of a moving object based on the improved LKF according to claim 4, characterized in that: The update formula is: Among them, λ k is the interference amplitude level value.

6. The method for controlling the attitude of a moving object based on the improved LKF according to claim 1, wherein: The MIMU includes: a gyroscope, an accelerometer, and a magnetometer.

7. The method for controlling the attitude of a moving object based on an improved LKF according to claim 1, wherein: The initial parameters include: initial estimate and initial error covariance matrix.

8. Improved LKF mobile object attitude control system, characterized in that, include: a memory for storing instructions executable by the processor; A processor, configured to execute instructions to implement the moving object posture control method using the improved LKF according to any one of claims 1 to 7.

9. A computer-readable medium storing computer program code, characterized in that When the computer program code is executed by a processor, the computer program code implements the method for controlling the attitude of a moving object by improving the LKF according to any one of claims 1 to 7.

Images