A volume kalman filter attitude solution method based on motion acceleration compensation

Abstract

This invention discloses a capacitive Kalman filter navigation method based on motion acceleration compensation, comprising the following steps: Step 1. In a pedestrian navigation system, the pedestrian's attitude is acquired using sensors, and a sensor measurement model is established; Step 2. Based on the sensor measurement model, the acceleration fitting interval is determined according to the accelerometer measurement results, and the fitting results are used to predict and compensate for motion acceleration within the fitting interval; Step 3. A capacitive Kalman filter is designed to estimate the pedestrian's navigation attitude, and the data from the gyroscope, accelerometer, and magnetometer are fused to complete the attitude estimation. This method determines the acceleration fitting interval based on the accelerometer measurement results, compensates the accelerometer measurement value within the fitting interval using the fitting results, and then uses the CKF algorithm to estimate the attitude after compensation, and then calculates the velocity and position. It has the characteristics of high accuracy and robustness in velocity and position calculation.

Description

Technical Field

[0001] This invention relates to the fields of attitude calculation and deep learning technology, specifically to a capacitive Kalman filter attitude calculation method based on motion acceleration compensation. Background Technology

[0002] Attitude estimation is one of the important tasks of many navigation systems, and it has wide applications in satellite control, UAV control, industrial robotic arms, intelligent robots, and pedestrian navigation. The solution in the case of stationary conditions is to compare non-parallel measurement vectors in the volume coordinate system with the corresponding known inertial vectors (the orientation of stars, the Earth's gravity, the Earth's magnetic field, etc.) to complete the attitude estimation. For example, the Quaternion Estimator (QUEST) uses the least squares idea to solve the attitude problem, which has received widespread attention and has led to the development of many related algorithms.

[0003] In attitude estimation algorithms based on inertial measurement units (IMUs) under motion conditions, the angular velocity measurements from gyroscopes can be used to calculate attitude changes. However, due to gyroscope drift, estimation errors gradually accumulate, necessitating compensation from data from multiple sensors such as accelerometers and magnetometers. Attitude calculation also involves nonlinear state estimation problems. In the field of nonlinear state estimation, the most commonly used algorithm is the Extended Kalman Filter (EKF). Based on different noise characteristics, it is divided into Additive Extended Kalman Filter (AEKF) and Multiplicative Extended Kalman Filter (AEKF). When the state vector is a quaternion, the EKF (Extended EKF) uses addition to correct the quaternion, which may cause the quaternion's modulus to no longer be 1, thus losing the normality of the quaternion. Therefore, the estimation result needs to be normalized. The MEKF, on the other hand, uses quaternion multiplication for correction. These two algorithms are essentially equivalent. Since the linearization process of EKF reduces the algorithm's accuracy and stability, to solve this problem, Simon J Julier, Jeffrey K Uhlmann, and others developed the Unscented Kalman Filter (UKF), and Ienkaran Arasaratnam, Simon Haykin, and others developed the Cubature Kalman Filter (Cubature Kalman Filter). Both UKF and CKF are based on the idea that "approximating a distribution should be easier for approximating any nonlinear function." They use Sigma points to approximate the Gaussian distribution, reducing accuracy loss during linearization and making UKF and CKF outperform EKF. However, UKF may have negative center weights during unscented transformations, and its accuracy decreases when the state vector exceeds three dimensions. While both use Sigma points to approximate the Gaussian distribution, CKF's core principle is the spherical-radial volume rule, which is superior to UKF in both accuracy and convergence. CKF also uses one less Sigma point than UKF, with the same weights, avoiding negative weights. To improve CKF performance, scholars have proposed many improved algorithms, such as square root capillary Kalman filtering, iterative adaptive capillary Kalman filtering, and strong tracking capillary Kalman filtering, but these significantly increase computational complexity. CKF is widely used in aircraft and satellites, but its application in pedestrian navigation attitude calculation is yet to be seen.

[0004] Inertial pedestrian navigation algorithms mainly include zero-velocity detection, zero-velocity correction, and navigation calculation. While there is considerable research on zero-velocity detection and correction algorithms, research on navigation calculation is relatively limited. Because foot acceleration is significant in the non-zero-velocity range, pure gyroscope calculation is often used, relying solely on angular velocity measurements to calculate attitude, without fully utilizing attitude information from accelerometer and magnetometer measurements. Aida Makni et al. modeled motion acceleration, assuming it to be a constant superposition of white noise, and designed a quaternion descriptor filter (QDF). This QDF can handle situations with motion acceleration in any direction and was experimentally tested in pedestrian navigation. Its calculation accuracy is superior to the extended Kalman filter algorithm. However, this algorithm suffers from overly simplistic assumptions about motion acceleration, failing to reduce the impact of motion acceleration on attitude calculation.

[0005] Therefore, it is urgent to design a capacitive Kalman filter attitude calculation method based on motion acceleration compensation to solve the problems existing in the above-mentioned technologies. Summary of the Invention

[0006] To address the aforementioned problems, this invention aims to provide a capacitive Kalman filter attitude calculation method based on motion acceleration compensation. This method determines the acceleration fitting interval based on the accelerometer measurement results, and uses the fitting results to compensate the accelerometer measurement values ​​within the fitting interval. After compensation, the attitude is estimated using the CKF algorithm, and then the velocity and position are calculated. This method features high accuracy and robustness in velocity and position calculation.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] A capacitive Kalman filter attitude calculation method based on motion acceleration compensation includes...

[0009] Step 1. In the pedestrian navigation system, sensors are used to acquire pedestrian posture and a sensor measurement model is established;

[0010] Step 2. Based on the sensor measurement model, determine the acceleration fitting interval according to the accelerometer measurement results, and use the fitting results to predict and compensate for the motion acceleration within the fitting interval;

[0011] Step 3. Design a capacitive Kalman filter for estimating pedestrian navigation attitude, fusing data from the gyroscope, accelerometer, and magnetometer to complete attitude estimation.

[0012] Preferably, the process of establishing the sensor measurement model in step 1 includes:

[0013] Step 1.1. Obtain pedestrian posture using sensors in the pedestrian navigation system;

[0014] Step 1.2. Based on the pedestrian posture obtained in Step 1.1, establish the sensor measurement model as follows:

[0015] y g =ω+b g +δ g (1)

[0016] y a =C(q)(G+f)+δ a (2)

[0017] y m =C(q)m+δ m (3)

[0018] In the formula, C(q) is the rotation matrix from the geographic coordinate system N to the carrier coordinate system B, and y g y a and y m These are the outputs of the gyroscope, triaxial accelerometer, and triaxial magnetometer in the B-frame, respectively, where ω is the true angular velocity and b is the output of the gyroscope. g For the constant drift of the gyroscope, δ g For random drift of the gyroscope; G is the coordinate of the local gravitational acceleration in the N-frame, f is the coordinate of the external acceleration in the N-frame, and δ... a The random zero bias of the accelerometer; m is the coordinate of the Earth's magnetic field in the N frame, δ m This refers to the random noise of the magnetic sensor.

[0019] Step 1.3. Assuming the constant bias of the accelerometer and magnetometer has been compensated, the drift is estimated using the average measurement value of the gyroscope in a static state, and the gyroscope measurement value is compensated to obtain the gyroscope measurement result:

[0020] y g =ω+δ g (4).

[0021] Preferably, step 2, which involves determining the acceleration fitting interval based on the accelerometer measurement results and using the fitting results to predict and compensate for the motion acceleration within the fitting interval, includes...

[0022] Step 2.1. Establish a motion acceleration fitting model and a motion acceleration fitting error model based on the sensor measurement model;

[0023] Step 2.2. Determine the fitting interval.

[0024] Preferably, the process of establishing the motion acceleration fitting model and the motion acceleration fitting error model in step 2.1 includes:

[0025] Step 2.1.1. Use the estimated values ​​of motion acceleration at time k and time k-1. Estimated acceleration at time k+1 To perform fitting and prediction, a first-order linear model was used, resulting in:

[0026]

[0027] In the formula, the acceleration a at time k+1 p,k+1 For predicted values With prediction error sum:

[0028]

[0029] Among them, the prediction error of acceleration It consists of three parts: the motion acceleration estimation error δ at time k and time k-1. p,k and δ p,k-1 and the error generated by fitting

[0030]

[0031] Step 2.1.2. Analyze prediction error The statistical characteristics of the acceleration in the non-zero velocity interval are obtained by fitting the prediction error magnitude of the first-order linear model. The interval in which the fitting error of the acceleration in the non-zero velocity interval is close to that in the zero velocity interval is called the fitting interval.

[0032] Step 2.1.3. Analyze the fitting error of the fitting interval in each gait cycle, and substitute the equation into the formula to obtain:

[0033]

[0034] In the formula, This is an estimate of the rotation matrix from the N-system to the B-system. Let k be the predicted value of the acceleration at time k. Let k be the predicted value of the random zero bias of the accelerometer at time k.

[0035]

[0036] in, Let k be the prediction error of the acceleration at time k. and v a,k They are independent of each other;

[0037] The measurement covariance matrix of acceleration is obtained as follows:

[0038]

[0039] In the formula, E is the mathematical expectation, and T represents the transpose of a vector or matrix.

[0040] Preferably, the process of determining the fitting interval in step 2.2 includes:

[0041] Based on zero-velocity detection, since the fitting interval lies between the two peaks of the acceleration modulus, finding the two peaks within the gait cycle can complete the determination of the fitting interval.

[0042] Preferably, the design process of the capacitive Kalman filter in step 3 includes:

[0043] Step 3.1. In the carrier coordinate system B(X) B ,Y B Z B ) and geographic coordinate system N(X) N ,Y N Z N In the process, attitude parameters are selected;

[0044] Step 3.2. Select Euler angle Φ and gyroscope drift b as state variables, and establish the state equation as follows;

[0045] x k =f(x) k-1 )+w k (20),

[0046] in:

[0047]

[0048]

[0049] w Φ,k =Δ t Jδ g,k-1 (twenty three);

[0050] In the formula, x k-1 x k Let J be the state at time k-1 and time k, respectively. The definition of J is given in equation (17). k Let w be the system noise at time k. Φ,k w b,k These represent the Euler angle noise and gyroscope drift noise at time k, respectively.

[0051] Step 3.3 Select the measurements from the accelerometer and magnetometer to form the measurement vector. The measurement equation is established as follows:

[0052]

[0053]

[0054] v m,k =δ m,k (26);

[0055] Step 3.4. Design the volumetric Kalman filter update algorithm.

[0056] Preferably, the attitude parameter selection process in step 3.1 includes:

[0057] Step 3.1.1. In the carrier coordinate system B(X) B ,Y B Z B ) and geographic coordinate system N(X) N ,Y N Z N In this context, the parameters used to describe attitude include the direction cosine matrix, Euler angles, and quaternions. The unit quaternion has the following form:

[0058]

[0059] In equation (11), q0 and These are the scalar and vector parts of a quaternion, respectively. A conjugate quaternion is defined as:

[0060]

[0061] Quaternions must satisfy the normalization property:

[0062] ||q||2=1 (13);

[0063] Step 3.1.2. Use Euler angles, which have low parameter redundancy and no normalization requirement, as attitude parameters:

[0064] Φ=[φ θ ψ] T

[0065] Where φ is the pitch angle, θ is the roll angle, and ψ is the yaw angle;

[0066] Step 3.1.3. Let sinx be abbreviated as sx and cosx as cx. The rotation matrix formed by Euler angles is:

[0067]

[0068] The differential equation for Euler angles is:

[0069] In the formula:

[0070]

[0071]

[0072] ω=[ω x ω y ω z ] T (18)

[0073] ω is the triaxial rotational angular velocity of the rigid body in the B frame; an approximate calculation is performed using the first-order Picard algorithm.

[0074]

[0075] Where θ≠90°.

[0076] Preferably, the design process of the volumetric Kalman filter update algorithm described in step 3.4 includes:

[0077] Step 3.4.1. Design the initialization equations for the volumetric Kalman filter.

[0078] Average attitude angles calculated using accelerometers and magnetometers As initial attitude value Initial values ​​of the error covariance matrix Take the diagonal matrix of eigenvalues, that is

[0079]

[0080]

[0081] Step 3.4.2. Design the time update equation for the volumetric Kalman filter;

[0082] Step 3.4.3. Design the volumetric Kalman filter measurement update equation.

[0083] Preferably, the design process of the volumetric Kalman filter time update equation described in step 3.4.2 includes:

[0084] (1) Generate volume points

[0085]

[0086]

[0087] In the formula, {1} i It is the i-th column of the following matrix:

[0088]

[0089] (2) Volume Point Update

[0090]

[0091] (3) One-step state prediction and covariance prediction

[0092]

[0093]

[0094] Preferably, the design process of the volumetric Kalman filter measurement update equation described in step 3.4.3 includes:

[0095] (1) Calculation of volume points and covariance matrix

[0096] a. Calculate the volume point

[0097]

[0098]

[0099] b. Calculate the volume point

[0100]

[0101] c. Calculate the predicted measurement values. The measurement covariance matrix and cross-covariance matrix are as follows:

[0102]

[0103]

[0104]

[0105] (2) Update of gain matrix, state variables and error covariance matrix

[0106] Calculate the Kalman gain K k Update state variables And error covariance matrix The equation is:

[0107]

[0108]

[0109]

[0110] The beneficial effects of this invention are: This invention discloses a capacitive Kalman filter attitude calculation method based on motion acceleration compensation. Compared with the prior art, the improvement of this invention lies in:

[0111] In pedestrian navigation systems based on foot-strap inertial measurement units (IMUs), the frequent and drastic changes in foot motion acceleration lead to a decrease in the accuracy of common attitude fusion algorithms. To reduce the impact of motion acceleration on attitude calculation, this invention proposes a capacitive Kalman filter (VKF) attitude calculation method based on motion acceleration compensation. This method performs first-order fitting compensation on the accelerometer output and designs a VKF filter and its algorithm for estimating pedestrian navigation attitude. In the non-fitting interval, the three-sample rotating vector method is used for attitude update. Theoretical analysis and experimental verification show that the acceleration-compensated VKF filter algorithm has higher accuracy than the pure three-sample rotating vector method when applied to pedestrian navigation attitude estimation, and has the advantages of high accuracy and robustness in velocity and position calculation. Attached Figure Description

[0112] Figure 1 This is a flowchart of the algorithm for the capacitive Kalman filter attitude calculation method based on motion acceleration compensation according to the present invention.

[0113] Figure 2 This is a structural diagram of a traditional pedestrian navigation algorithm.

[0114] Figure 3 This is a structural diagram of the improved pedestrian navigation algorithm of the present invention.

[0115] Figure 4 This is a schematic diagram of the structure of the MTi-G-710 sensor of the present invention.

[0116] Figure 5 This is a graph showing the fitting error of the triaxial motion acceleration and the measurement modulus of the accelerometer in this invention.

[0117] Figure 6 This is a graph showing the motion acceleration fitting error of the present invention.

[0118] Figure 7 This is a histogram of gait period length distribution and a distribution diagram of acceleration magnitude for the present invention.

[0119] Figure 8 This is the fitted interval curve of the present invention.

[0120] Figure 9 This is a structural diagram of the pedestrian navigation algorithm based on motion acceleration compensation and capacitive Kalman filtering according to the present invention.

[0121] Figure 10 This is a schematic diagram of the volumetric Kalman filter principle of the present invention.

[0122] Figure 11 The graph shows the Euler angle estimation error of the two algorithms in Embodiment 2 of the present invention.

[0123] Figure 12This is a diagram showing the results of horizontal position and height calculations in Embodiment 2 of the present invention.

[0124] Among them: Figure 6 In the figure, Figure (a) is the fitting error diagram of the X-axis motion acceleration, Figure (b) is the fitting error diagram of the Y-axis motion acceleration, and Figure (c) is the fitting error diagram of the Z-axis motion acceleration.

[0125] exist Figure 7 In the figure, Figure (a) is the distribution of period length, and Figure (b) is the distribution of acceleration magnitude.

[0126] exist Figure 11 In the figure, Figure (a) is the pitch angle error curve, Figure (b) is the roll angle error curve, and Figure (c) is the heading angle error curve.

[0127] exist Figure 12 In the figure, Figure (a) shows the result of the horizontal position calculation, and Figure (b) shows the result of the vertical position calculation. Detailed Implementation

[0128] To enable those skilled in the art to better understand the technical solutions of the present invention, the technical solutions of the present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0129] Example 1: Refer to Appendix Figure 1-12 The illustrated method is a volumetric Kalman filter attitude calculation method based on motion acceleration compensation, including...

[0130] Step 1. In the pedestrian navigation system based on foot-mounted inertial measurement units, sensors are used to acquire pedestrian posture, a sensor measurement model is established, and velocity and position are calculated. The algorithm structure is as follows: Figure 3 As shown

[0131] Step 1.1. First, the pedestrian posture is obtained by using sensors in the pedestrian navigation system based on foot-attached inertial measurement unit, wherein the sensors are fixed to the human foot and include gyroscope, three-axis accelerometer and three-axis magnetometer.

[0132] Step 1.2. Based on the pedestrian posture obtained in Step 1.1, establish the sensor measurement model as follows:

[0133] y g =ω+b g +δ g (1)

[0134] y a =C(q)(G+f)+δ a (2)

[0135] y m =C(q)m+δ m (3)

[0136] In the formula, C(q) is the rotation matrix from the geographic coordinate system N to the carrier coordinate system B, q is a quaternion, and y g y a and y m These are the outputs of the gyroscope, triaxial accelerometer, and triaxial magnetometer in the B-series (pedestrian navigation system based on a foot-strap inertial measurement unit), respectively, where ω is the actual angular velocity, and b... g This is the constant drift of the gyroscope, δ g For random drift of the gyroscope; G is the coordinate of the local gravitational acceleration in the N-frame, f is the coordinate of the external acceleration in the N-frame, and δ... a The random zero bias of the accelerometer; m is the coordinate of the Earth's magnetic field in the N frame, δ m To account for the random noise of the magnetic sensor, this embodiment assumes that the constant zero bias of the accelerometer and magnetometer has been compensated.

[0137] Step 1.3. Since the drift of the gyroscope changes relatively slowly, the drift can be estimated by using the average measurement value of the gyroscope in a static state, and then the measurement value of the gyroscope can be compensated. At this time, the measurement result of the gyroscope can be approximately expressed as equation (4):

[0138] y g =ω+δ g (4)

[0139] In equations (2) to (4), the noise term δ of each sensor a δ m and δ g It is independent and identically distributed Gaussian white noise, and its covariance matrices are respectively Where σ g , σ a , σ m This can be determined from the sensor's technical specifications or through laboratory testing; in this embodiment, the sensor used is as follows: Figure 4 The image shows the Xsense MTi-G-710 sensor.

[0140] Step 2. Based on the sensor measurement model, determine the acceleration fitting interval according to the accelerometer measurement results. Within the fitting interval, use the fitting results to compensate for the acceleration output, predict and compensate for the motion acceleration, predict the motion acceleration at the next moment and compensate for it into the accelerometer measurement results, obtaining a more accurate measurement value of gravitational acceleration. This can improve the accuracy of pedestrian navigation calculation and avoid errors caused by an overly simplistic model.

[0141] Step 2.1. Establish the motion acceleration fitting model and the motion acceleration fitting error model.

[0142] Step 2.1.1. Due to the large amplitude and long duration of acceleration that occurs during walking. ap At this point, directly using the accelerometer to compensate for the gyroscope's measurements will result in a large error; therefore, this embodiment uses the motion acceleration estimates at time k and time k-1. Estimated acceleration at time k+1 To perform fitting and prediction, using a first-order linear model, we can obtain equation (5):

[0143]

[0144] The acceleration a at time k+1 p,k+1 For predicted values With prediction error sum:

[0145]

[0146] Among them, the prediction error of acceleration It consists of three parts: the motion acceleration estimation error δ at time k and time k-1. p,k and δ p,k-1 and the error generated by fitting

[0147]

[0148] Step 2.1.2. Since it is difficult to obtain the three independent statistical characteristics of the error from the collected acceleration data, the following data analysis will directly analyze and study the prediction error. Statistical characteristics; such as Figure 5 The paper presents the acceleration measurement magnitude collected during a walking experiment, as well as the prediction error magnitude after fitting with a first-order linear model.

[0149] from Figure 5 It can be observed that there is a section in the non-zero velocity range where the acceleration fitting error is similar to that in the zero velocity range. During this motion, the fitting results can be used to compensate for the accelerometer. This range is called the fitting range.

[0150] Step 2.1.3. Analyze the fitting error of the fitting interval in each gait cycle, and the error distribution diagram is shown below. Figure 6 As shown in Figure 6, the error distribution approximates a normal distribution; the mean and variance of the errors are shown in Table 1.

[0151] Table 1: Mean value of fitting error for motion acceleration (m / s²) 2 ) and variance

[0152]

[0153] This gives us an approximate value for δ. p,k+1 Statistical characteristics;

[0154] Substituting the equation into the expression, we get:

[0155]

[0156] In the formula, This is an estimate of the rotation matrix from the N-system to the B-system. Let k be the predicted value of the acceleration at time k. Let k be the predicted value of the random zero bias of the accelerometer at time k.

[0157]

[0158] in, Let k be the prediction error of the acceleration at time k. and v a,k If they are independent, the measurement covariance matrix of acceleration is obtained as follows:

[0159]

[0160] In the formula, E is the mathematical expectation, and T represents the transpose of a vector or matrix.

[0161] Step 2.2. Determine the fitting interval.

[0162] To apply this algorithm in pedestrian navigation, it is also necessary to accurately determine the fitting interval. Figure 5 As can be seen, this interval lies between the two peaks of the acceleration modulus. Therefore, it is only necessary to find the two peaks within the gait cycle and then statistically analyze the gait duration. Figure 7 The distribution of acceleration magnitudes within a gait cycle is shown, illustrating the gait cycle length and the gait cycle itself.

[0163] in, Figure 6 (a) indicates that the gait cycle length is distributed between 1.3 seconds and 1.6 seconds. Figure 6 (b) This indicates that the fitting interval is located near the 170th sampling point, approximately 0.85 seconds into the gait cycle. After detecting the completion of one step, the peak values ​​of the first 170 sampling points and those after the 170th sampling point are detected. To eliminate errors caused by drastic changes near the peak values, the middle three-quarters of the interval is extracted, such as... Figure 8 As shown;

[0164] Since the acceleration compensation interval can be approximated as a zero-velocity interval within the fitting interval, a volumetric Kalman filter can be used to fuse data from the gyroscope, accelerometer, and magnetometer to estimate the attitude. In previous studies, the attitude calculation algorithm for the non-zero-velocity interval was the three-sample rotating vector method. Therefore, in the non-fitting interval, the three-sample rotating vector method is still used, with only angular velocity information used for attitude updates. In this case, the navigation calculation process of the volumetric Kalman filter with acceleration compensation is as follows: Figure 9 As shown;

[0165] Step 3. Design a capacitive Kalman filter for estimating pedestrian navigation attitude, fusing data from the gyroscope, accelerometer, and magnetometer to complete attitude estimation.

[0166] Step 3.1. Parameter Selection

[0167] Step 3.1.1. In three-dimensional space, the attitude of a rigid body can be expressed using the carrier coordinate system B(X). B ,Y B Z B ) and geographic coordinate system N(X) N ,Y N Z N The relative position of ) is described; where X in the geographic coordinate system N Pointing due east, Y N Pointing due north, Z N Parallel to the direction of gravity and pointing towards the sky, this coordinate system is the Northeast-East-South (ENU) coordinate system. Commonly used parameters for describing attitude include the direction cosine matrix, Euler angles, and quaternions. Quaternions are widely used because they have low parameter redundancy and no singular values. The form of a unit quaternion is as follows:

[0168]

[0169] In equation (11), q0 and These are the scalar and vector parts of a quaternion, respectively. A conjugate quaternion is defined as:

[0170]

[0171] The quaternions used to describe attitude need to satisfy the normalization property:

[0172] ||q||2=1 (13)

[0173] Step 3.1.2. Considering that addition is required when constructing the volume point, which makes it impossible for quaternions to satisfy the formula, and the symmetry of the volume point cannot be guaranteed after normalization, affecting subsequent calculations, this embodiment uses Euler angles with low parameter redundancy and no normalization requirement as attitude parameters; when using Euler angles of ZYX rotation, its definition is as follows:

[0174] Φ=[φ θ ψ] T

[0175] Where φ is the pitch angle, θ is the roll angle, and ψ is the yaw angle;

[0176] Step 3.1.3. To simplify the expression of the equations, sinx is abbreviated as sx, cosx as cx, and the rotation matrix formed by Euler angles is:

[0177]

[0178] The differential equation for Euler angles is:

[0179] In the formula:

[0180]

[0181]

[0182] ω=[ω x ω y ω z ] T (18)

[0183] ω is the triaxial rotational angular velocity of the rigid body in the B frame; in practical applications, a first-order Picard algorithm can be used for approximate calculation.

[0184]

[0185] As can be seen from the formula, θ should be avoided from approaching 90° during the solution process. In pedestrian navigation, it is generally impossible for θ to approach 90°.

[0186] Step 3.2. Establish the state equations

[0187] Euler angles Φ and gyroscope drift b are chosen as state variables, i.e., x = [Φ]. T b T ] T Based on equations and , the state equations can be obtained as follows:

[0188] x k =f(x) k-1 )+w k (20)

[0189] in:

[0190]

[0191]

[0192] w Φ,k =ΔtJδg,k-1 (twenty three)

[0193] In equations (20) and (22), x k-1 x k The state variables at time k-1 and time k are respectively, w k Let w be the system noise at time k. Φ,k w b,k These represent the Euler angle noise and gyroscope drift noise at time k, respectively.

[0194] Step 3.3. Establish measurement equations

[0195] The measurement vector is constructed by selecting measurements from the accelerometer and magnetometer. Combining equations 1, 2, and 3, we obtain the measurement equation as follows:

[0196]

[0197]

[0198] v m,k =δ m,k (26)

[0199] In the formula, C(Φ) k ) is based on Euler angles Φ k Let v be the rotation matrix of the variable from the N-system to the B-system. a,k v m,k Let δ be the measurement noise of the accelerometer and the magnetometer at time k, respectively. a,k Let δ be the random zero bias of the accelerometer at time k. m,k Let be the random noise of the magnetic sensor at time k;

[0200] Step 3.4. Design the volumetric Kalman filter update algorithm

[0201] Step 3.4.1. Design the initialization equations for the volumetric Kalman filter.

[0202] To accelerate algorithm convergence, relatively accurate initial values ​​are needed. The algorithm should remain stationary before walking, and the average attitude angles calculated using accelerometers and magnetometers should be used. As initial attitude value Initial values ​​of the error covariance matrix We can simply take a diagonal matrix with a larger eigenvalue;

[0203]

[0204]

[0205] Step 3.4.2. Design the time update equation for the volumetric Kalman filter.

[0206] (1) Generate volume points

[0207]

[0208]

[0209] In the formula, This is the state estimate at time k-1. Let be the error covariance matrix at time k-1. {1} i It is the i-th column of the following matrix:

[0210]

[0211] (2) Volume Point Update

[0212]

[0213] In the formula, the function f is shown in equation (21).

[0214] (3) One-step state prediction and covariance prediction

[0215]

[0216]

[0217] Step 3.4.3. Design the volumetric Kalman filter measurement update equation

[0218] (1) Calculation of volume points and covariance matrix

[0219] a. Calculate the volume point

[0220]

[0221]

[0222] b. Calculate the volume point

[0223]

[0224] In the formula, the h function is shown in equation (24).

[0225] c. Calculate the predicted measurement values. The measurement covariance matrix and cross-covariance matrix are as follows:

[0226]

[0227]

[0228]

[0229] (2) Update of gain matrix, state variables and error covariance matrix

[0230] Calculate the Kalman gain K k Update state variables And error covariance matrix The equation is:

[0231]

[0232]

[0233]

[0234] The structure diagram of the volumetric Kalman filter algorithm is as follows: Figure 10 As shown.

[0235] Example 2: To verify the effectiveness and superiority of the capacitive Kalman filter attitude calculation method based on motion acceleration compensation described in Example 1 of this invention, this example is designed to verify the above method.

[0236] Step 4. Experimental Verification

[0237] Step 4.1. Simulation Verification

[0238] In this step, the proposed algorithm is evaluated using numerical simulation. The data sampling period of the sensor is set to 0.01 seconds. Since the time period for the volumetric Kalman filter to participate in the solution is about 0.5 seconds, only a 2-second simulation is performed. The three-dimensional motion angular velocity of the rigid body is shown in Table 2.

[0239] Table 2: Angular Velocity of Three-Dimensional Motion

[0240]

[0241] The Euler angles within these 2 seconds can be obtained using the Euler angle difference equation (19); the measurement results of the accelerometer and magnetometer can be simulated according to equations (2) and (3), and then Gaussian white noise is added to the sensor data. The standard deviations of the noise from the accelerometer and magnetometer are σ and σ, respectively. acc =0.01m / s 2 and σ mag =0.01 Gauss, the standard deviation of the gyroscope noise is σ gyro =0.05rad / s; In order to simulate foot movements in pedestrian navigation and to test the performance of the algorithm, the motion accelerations added are shown in Table 3;

[0242] Table 3: Motion acceleration applied in different time intervals

[0243]

[0244] To compare the estimation accuracy and computational complexity of the algorithms, the attitude calculation errors without and with capacitive Kalman filtering were compared in simulations. Figure 11 As shown;

[0245] from Figure 11 It can be observed that within the acceleration fitting range, the volumetric Kalman filter algorithm for acceleration compensation exhibits higher accuracy and a significant reduction in error. To more accurately compare the algorithm's accuracy, the root mean square error (RMSE) is used to compare the algorithm's performance. The RMSE is calculated as follows:

[0246]

[0247] In the formula, T represents the total solution time, and δx θ (t) is the solution error θ∈{pitch,theta,yaw} at time t. The root mean square error of each algorithm is shown in Table 4.

[0248] Table 4: Root Mean Square Error of Attitude Calculation

[0249]

[0250] As shown in Table 4, after adding CKF, the pitch angle error decreased by 38.8%, the roll angle error decreased by 39.8%, the heading angle error decreased by 29.0%, and the average accuracy improved by 35.3%.

[0251] Step 4.2. Experimental Verification

[0252] Walking along a 10m × 18m rectangular route, the navigation calculation used the zero-velocity detection algorithm based on pseudo-standard deviation and NP criterion, and the Kalman filter zero-velocity correction algorithm proposed by the research group in the past. The final position calculation result is as follows: Figure 12 As shown;

[0253] The horizontal and vertical errors at the start and end points of the path are shown in Table 5.

[0254] Table 5: Horizontal and vertical errors at the start and end points of the solution path

[0255]

[0256] Table 5 shows that after adding capacitive Kalman filtering, the horizontal error of the starting and ending points decreased by 56.3%, and the vertical error decreased by 20.3%. This indicates that the CKF algorithm with acceleration compensation can significantly improve the accuracy of pedestrian navigation.

[0257] The experimental results above show that the commutative Kalman filter algorithm based on motion acceleration compensation designed in Embodiment 1 of this invention has higher solution accuracy than the pure three-sample rotating vector method when applied to pedestrian navigation attitude estimation.

[0258] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A method for attitude determination based on motion acceleration compensation volume Kalman filter, characterized in that: include Step 1. In the pedestrian navigation system, sensors are used to acquire pedestrian posture and a sensor measurement model is established; Step 2. Based on the sensor measurement model, determine the acceleration fitting interval according to the accelerometer measurement results, and use the fitting results to predict and compensate for the motion acceleration within the fitting interval; Among them, including Step 2.

1. Establish a motion acceleration fitting model and a motion acceleration fitting error model based on the sensor measurement model; Step 2.1.

1. Using the motion acceleration estimate at time the motion acceleration estimate at time to the motion acceleration estimate at time a first order linear model to obtain （5）， In the formula, motion acceleration at the time is a prediction value and a prediction error sum: （6）， Among them, the prediction error of acceleration It consists of three parts. Time and Error in motion acceleration estimation at time step and and the error generated by fitting , （7）， Step 2.1.

2. Analyze prediction error The statistical characteristics of the acceleration in the non-zero velocity interval are obtained by fitting the prediction error magnitude of the first-order linear model. The interval in which the fitting error of the acceleration in the non-zero velocity interval is close to that in the zero velocity interval is called the fitting interval. Step 2.1.

3. Analyze the fitting error of the fitting interval in each gait cycle, and obtain: （8）， In the formula, for Tie The estimated value of the rotation matrix. for k The predicted value of the acceleration at any given moment. for k The predicted value of the random zero bias of the accelerometer at any given time. （9）， wherein is k the prediction error of the acceleration at the moment, and are independent of each other; The measurement covariance matrix of acceleration is obtained as follows: （10）， In the formula, E Let T be the mathematical expectation, and let T denote the transpose of a vector or matrix; Step 2.

2. Determine the fitting interval: Based on zero-velocity detection, since the fitting interval is located between the two peaks of the acceleration modulus, finding the two peaks within the gait cycle can complete the determination of the fitting interval; Step 3. Design a capacitive Kalman filter for estimating pedestrian navigation attitude, and fuse data from the gyroscope, accelerometer, and magnetometer to complete attitude estimation.

2. The method of claim 1, wherein the method is based on motion acceleration compensation. The process of establishing the sensor measurement model described in step 1 includes: Step 1.

1. Obtain pedestrian posture using sensors in the pedestrian navigation system; Step 1.

2. Based on the pedestrian posture obtained in Step 1.1, establish the sensor measurement model as follows: （1） （2） （3） In the formula, Geographic coordinate system Attached to the carrier coordinate system The rotation matrix of the system, and These are a gyroscope, a three-axis accelerometer, and a three-axis magnetometer. The output of the system, For the actual angular velocity, This refers to the constant drift of the gyroscope. The gyroscope drifts randomly; For the local gravitational acceleration in Coordinates in the system For external acceleration in Coordinates in the system For the random zero bias of the accelerometer; For the Earth's magnetic field in Coordinates in the system This refers to the random noise of the magnetic sensor. Step 1.

3. Assuming the constant bias of the accelerometer and magnetometer has been compensated, the drift is estimated using the average measurement value of the gyroscope in a static state, and the gyroscope measurement value is compensated to obtain the gyroscope measurement result: （4）。 3. The method of claim 2, wherein the method is based on motion acceleration compensation. Step 3 describes the design process for the capacitive Kalman filter, which includes... Step 3.

1. In the carrier coordinate system and geographic coordinate system In the process, attitude parameters are selected; Step 3.

2. Select Euler angles and gyroscope drift As state variables, the state equation is established as follows; （20）， in: （21）， （22）， （23）， In the formula, They are respectively Time and The state at any given moment, The definition is shown in equation (17). for System noise at any given moment They are respectively Euler angle noise and gyroscope drift noise at time step; T is the transpose; （17） Step 3.3 Selecting the measurements of the accelerometer and the magnetometer to form the measurement vector The measurement equation is established as: （24）， （25）， （26）， In the formula, Euler angles For the variable from Tie The rotation matrix of the system, They are respectively The measurement noise of the accelerometer and the measurement noise of the magnetometer at different times. for The random zero bias of the accelerometer at any moment, for Random noise of the time-lapse magnetic sensor; Step 3.

4. Design the volumetric Kalman filter update algorithm.

4. The attitude determination method based on volume Kalman filter with motion acceleration compensation according to claim 3, wherein: The design process of the capacitive Kalman filter update algorithm described in step 3.4 includes... Step 3.4.

1. Design the initialization equations for the volumetric Kalman filter. Average value of attitude angle calculated using accelerometer and magnetometer As an initial value of attitude , initial value of error covariance matrix Diagonal matrix taking eigenvalues, that is, （27）， （28）， Step 3.4.

2. Design the time update equation for the volumetric Kalman filter; Step 3.4.

3. Design the volumetric Kalman filter measurement update equation.

Images