High-precision angular velocity, acceleration and zero offset online estimation method for array IMU (Inertial Measurement Unit)

Through the expansion of Kalman filter and segment-by-stage function fitting technology, the problems of asymmetry and zero-bias change in the installation of array inertial measurement units are solved, high-precision angular velocity and acceleration estimation are achieved, and the accuracy and stability of the motion state are improved. , reduces costs.

CN118913264APending Publication Date: 2024-11-08TRINA INTELLIGENT TECH (CHONGQING) CO LTD
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202411110912.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-14
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

In the prior art, the installation asymmetry and zero deviation of the array inertial measurement unit change with time lead to insufficient angular velocity and acceleration estimation accuracy, and the high-precision inertial measurement unit is costly, making it difficult to achieve effective accuracy improvement.

Method used

The extended Kalman filter is used to combine the data of the inertial measurement unit to estimate the angular velocity and accelerometer data online, calculate the acceleration and zero deviation of the array center, and perform segment-by-stage function fitting, replacing the state transition matrix to improve the estimation accuracy .

Benefits of technology

High-precision motion estimation of multi-inertial measurement unit array is realized, online estimation of acceleration and zero deviation is completed, the accuracy and stability of the motion state is improved, and the cost is reduced.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure SMS_1
    Figure SMS_1
  • Figure SMS_3
    Figure SMS_3
  • Figure SMS_5
    Figure SMS_5
Patent Text Reader

Abstract

The invention discloses an array type IMU (inertial measurement unit) high-precision angular velocity, acceleration and zero offset online estimation method, which can be used for realizing multi-inertial measurement unit array fusion so as to improve the motion estimation precision and finish online estimation of acceleration and zero offset. Comprising the following steps: 1) measuring respective angular velocity and acceleration of each inertial measurement unit by using data of a gyroscope and an accelerometer; 2) estimating the angular velocity, the acceleration of the array center and the zero offset of the accelerometer by using an extended Kalman filter; 3) calculating the angular velocities and accelerations of all inertial measurement units according to the estimated angular velocity, the acceleration of the array center and the zero offset of the accelerometer, and 4) performing segment-by-segment function fitting on the motion state, and if a matched function is fitted, changing the extended Kalman filter into window recursion estimation.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to the technical field of inertial navigation, and in particular to an array type IMU high-precision angular velocity, acceleration and zero bias online estimation method. Background Art

[0002] Angular velocity estimation is an important part of attitude estimation in navigation. Generally, gyroscopes are used to measure angular velocity, but due to the limitations of the device industry level and electronic signal characteristics, the angular velocity measured by gyroscopes usually contains large errors. Generally, the use of higher-precision inertial measurement units can improve the effect of angular velocity estimation. The industry has begun to use multiple inertial measurement units to form an array to estimate angular velocity to solve the problem that the accuracy improvement brought by high-precision inertial measurement units will lead to a rapid increase in costs.

[0003] However, there are two problems with the above method: (1) The inertial measurement units in the array must be installed precisely and symmetrically, otherwise the general fusion method cannot fuse the acceleration. (2) The inertial measurement units must be calibrated with high precision on a turntable before use, and the zero bias may change over time. Summary of the invention

[0004] The purpose of the present invention is to design an array IMU high-precision angular velocity, acceleration and zero bias online estimation method, which can digitally fuse multiple inertial measurement unit arrays to improve motion estimation accuracy and complete online estimation of acceleration and zero bias.

[0005] The present invention is implemented by the following technical solution: an array IMU high-precision angular velocity, acceleration and zero bias online estimation method, comprising the following steps:

[0006] 1) Use gyroscope and accelerometer data to measure the angular velocity and acceleration of each inertial measurement unit;

[0007] 2) Use the extended Kalman filter to estimate the angular velocity, the acceleration at the center of the array, and the zero bias of the accelerometer;

[0008] 3) Calculate the angular velocity and acceleration of all inertial measurement units based on the estimated angular velocity, the acceleration at the center of the array, and the zero bias of the accelerometer;

[0009] 4) Fit the motion state with functions piece by piece. If a matching function is fitted, the extended Kalman filter is replaced with window recursive estimation.

[0010] In order to better realize the high-precision online estimation method of angular velocity, acceleration and zero bias of an array IMU described in the present invention, the following setting is particularly adopted: the step 1) directly outputs the three-axis acceleration and the three-axis angular velocity of each inertial measurement unit carrier coordinate system after measurement;

[0011] ω i =ω gi -b g ;

[0012] a i =a ai ;

[0013] Among them, ω gi is the raw angular velocity reading of the i-th inertial measurement unit, ω i is the angular velocity value of the Kalman filter in step 2), a ai is the raw reading of the accelerometer of the i-th inertial measurement unit, a i is the acceleration value of the Kalman filter entering step 2), b g is the pre-calibrated gyroscope bias.

[0014] In order to better implement the array IMU high-precision angular velocity, acceleration and zero bias online estimation method described in the present invention, the following setting method is particularly adopted: the step 2) is specifically:

[0015] 2.1) The geometric center of the array is selected as the reference point. The specific force of each inertial measurement unit is synthesized by the specific force of the geometric center of the array and the centripetal force and torque:

[0016] f i =f o +ω×(ω×r i )+α×r i +b i +n (1);

[0017] Among them, ω is the angular velocity around the instantaneous center to be estimated, that is, the real output of the gyroscope, α is the angular acceleration around the instantaneous center, and f o is the acceleration of the array geometric center, f i is the i-th accelerometer reading in step 1); b i is the zero bias of the i-th accelerometer at the current moment; r i is the position radius vector of the i-th inertial measurement unit of the array relative to the geometric center of the array;

[0018] The measurement signal model of the present invention is derived from classical mechanics. The accelerometer signal at any point on the inertial measurement unit array is equal to the inertial measurement unit array geometric center specific force, centrifugal force, torque caused by angular acceleration, and accelerometer zero bias synthesis. The state recursion model is given by the first-order Markov model where the angular velocity is equal to the angular velocity plus angular acceleration at the previous moment and the accelerometer zero bias. Therefore, the state to be estimated is the real-time estimated value of the angular velocity, angular acceleration, array geometric center specific force, and inertial measurement unit (for example, 4) zero bias;

[0019] 2.2) Estimate the real-time estimated values ​​of angular velocity, angular acceleration, array geometric center force, and zero bias of inertial measurement units (for example, 4):

[0020] Define the state variables:

[0021]

[0022] Where f is the acceleration of the center of the array to be estimated, b is the zero bias of the four accelerometers at the current moment, which is a 12×1 column vector composed of the three-axis zero bias of the four accelerometers arranged longitudinally;

[0023] The measurement equation is:

[0024] ω is the angular velocity around the instantaneous center to be estimated, that is, the real output of the gyroscope; α is the angular acceleration around the instantaneous center, and r is the position vector of the inertial measurement unit relative to the geometric center of the array; among them, the current angular velocity is synthesized by the angular velocity and the angular velocity increment at the previous moment, and the zero bias b of the accelerometer at the current moment is calculated using the first-order Markov model:

[0025]

[0026] in, It represents the rate of change of zero bias, τ is the inverse of the accelerometer zero bias related time, n b is the accelerometer white noise;

[0027] The signal model is:

[0028]

[0029] Among them, ω o Represents the angular velocity of the IMU array;

[0030] 2.3) Using the extended Kalman filter, the measurement values ​​of each inertial measurement unit in the array are integrated to estimate the angular velocity, angular acceleration, specific force, and zero bias of each inertial measurement unit (taking 4 as an example) at the geometric center of the inertial measurement unit array:

[0031]

[0032] in:

[0033]

[0034] Among them, x k represents the state quantity to be estimated at time k, z k represents the measured value to be estimated at time k, v ω ,v α ,v f ,v bare the process noises of angular velocity around the instantaneous center, angular acceleration around the instantaneous center, and translation acceleration, respectively, w k is the measurement noise of the state quantity, T is the sampling period, τ is the inverse of the accelerometer zero bias correlation time, f i is the i-th accelerometer measurement, ω i is the i-th gyroscope measurement value, b1 is the first gyroscope measurement value, b2 is the second gyroscope measurement value, b3 is the third gyroscope measurement value, and b4 is the fourth gyroscope measurement value; r1 is the first inertial measurement unit position vector, b2 is the second inertial measurement unit position vector, b3 is the third inertial measurement unit position vector, b4 is the fourth inertial measurement unit position vector, and f is the array center acceleration to be estimated;

[0035] Right k =h(x k )+w k h(x k ) Find the first-order Jacobian matrix H k (x):

[0036]

[0037] Among them, Ω is the antisymmetric matrix of the three-dimensional vector, I3 is the three-dimensional identity matrix, I 12×12 is a 12-dimensional identity matrix;

[0038] One-step prediction is done as follows:

[0039] X k+1 =ΦX k (7);

[0040] X k represents the state quantity to be estimated at time k, and Φ represents the state transfer matrix;

[0041] The mean square error matrix is ​​calculated by the following formula:

[0042] P k+1 / k =ΦP k+1 Φ T +Q (8);

[0043] The filter gain is calculated by the following formula:

[0044] H k represents the measurement matrix at time k;

[0045] Update the state by:

[0046] X k =X k+1 / k +K k (z k -h(Xk+1 / k ))(10);K k represents the filter gain at time k, X k+1 / k Represents the state quantity predicted at time k;

[0047] Update the mean square error matrix by:

[0048] P k+1 =P k -K k H k P k (11), where Q = Ε(v k v j ),R=Ε(w k w j ) (12);

[0049] Q is the process noise matrix, R is the measurement noise matrix, and E() represents the expectation of the random vector.

[0050] In order to better realize the method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU described in the present invention, the following setting is particularly adopted: the step 3) calculates the angular velocity and acceleration of all inertial measurement units according to the estimated angular velocity, the acceleration of the center of the array, and the zero bias of the accelerometer, specifically:

[0051] 3.1) The ω obtained in step 2) k , a k Update the attitude, velocity and position;

[0052] In the "east-north-sky" navigation coordinate system, that is, the n-system, the carrier's attitude differential equation is:

[0053]

[0054] in, To get the vector The antisymmetric matrix of “×” represents the antisymmetric matrix of the orientation quantity; is the attitude matrix of system b relative to system n, is the first-order derivative of the attitude matrix, is the attitude rate, Transformed into:

[0055]

[0056] Substituting formula (14) into formula (13), we can obtain:

[0057]

[0058] In the formula, is the angular velocity output by the gyroscope, i.e., ω in step 2). k , and From the carrier latitude and the velocity in the n system, we can get:

[0059]

[0060] In the formula, R M Represents the radius of curvature of the Earth's meridian, R N R represents the radius of curvature of the Earth's circumplex, e represents the reference radius of the earth, e represents the eccentricity of the earth's ellipse; ω ie represents the angular velocity of the earth's rotation, h represents the local altitude, L represents the local latitude, and the speed v in the n system n =[v E v N v U ], where v E 、v N and v U They represent the eastward, northward and celestial components of the carrier velocity respectively;

[0061] To update the speed:

[0062]

[0063] in, Represents the acceleration of ground speed in the navigation coordinate system, is the specific force value measured by the accelerometer, is the Coriolis acceleration obtained by the interaction of the carrier's own acceleration and the earth's rotation angular velocity, is the centripetal acceleration of system n relative to system e, g n represents the local gravitational acceleration;

[0064] In formula (20), Simple note to do v n , and considering the influence of time t, as follows:

[0065]

[0066] represents the acceleration of the ground speed in the navigation coordinate system. m-1 ,t m ] to integrate, we get:

[0067]

[0068] Right now:

[0069]

[0070] in, and t m-1 and t m The inertial velocity at time is:

[0071]

[0072] In the formula, and T=t m -t m-1 n is the specific force velocity increment and harmful acceleration increment during the time period;

[0073] According to formula (23), the speed expression is:

[0074]

[0075] △θ m is the gyro sampling angle increment, and △v m It is the accelerometer sampling specific force velocity increment; The amount of rotation error compensation is called velocity.

[0076] In order to better realize the method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU described in the present invention, the following setting is particularly adopted: the step 4) performs piecewise function fitting on the motion state, and if a matching function is fitted, the extended Kalman filter is replaced with a window recursive estimation, including the following specific steps:

[0077] Perform function fitting on a motion state. If a matching function is fitted, replace equation (6) with the following equation (28):

[0078] Assume that the angular velocity is v, △t is the sampling time, η p is the noise, β is an unknown parameter, and

[0079]

[0080] With a window length of l, the least square solution of β is as follows:

[0081]

[0082] Calculate the estimated state x k :

[0083]

[0084] in:

[0085]

[0086] get:

[0087]

[0088] State transition matrix Φ (k-n:k-l) for:

[0089]

[0090] Represents the covariance matrix:

[0091]

[0092] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0093] The present invention can digitally fuse multiple inertial measurement unit arrays to improve motion estimation accuracy and complete online estimation of acceleration and zero bias.

[0094] The present invention firstly uses a gyroscope and an accelerometer to calculate the acceleration and angular velocity of a single inertial measurement unit; then uses an extended Kalman filter to estimate the angular velocity and the acceleration of the array center; finally, the estimated angular velocity, acceleration and zero bias are fed back to the motion estimation of each inertial measurement unit; if the motion satisfies a certain functional relationship, the state transfer matrix is ​​replaced by the transfer matrix of the window recursive estimation.

[0095] Other features and advantages of the present application will be described in the following description, and partly become apparent from the description, or understood by implementing the embodiments of the present application. The purpose and other advantages of the present application can be achieved and obtained by the structures specifically pointed out in the written description. DETAILED DESCRIPTION

[0096] The present invention is further described in detail below in conjunction with examples, but the embodiments of the present invention are not limited thereto.

[0097] In order to make the purpose, technical scheme and advantages of the embodiments of the present invention clearer, the technical scheme of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present invention. Therefore, the following detailed description of the embodiments of the present invention is not intended to limit the scope of the claimed invention, but merely represents the selected embodiments of the present invention.

[0098] It should be noted that in the description of this application, relational terms such as "first", "second", etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Furthermore, the term "and / or" in this application is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone.

[0099] In the description of the present invention, it is necessary to understand that the orientation or positional relationship indicated by terms, etc., is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0100] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, the meaning of "plurality" is two or more, and the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0101] In the description of this application, it should also be noted that, unless otherwise clearly specified and limited, the terms "connected" and "connection" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can also be an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0102] The functional modules in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0103] Alternatively, the present invention may be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented by software, the present invention may be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present invention is generated in whole or in part.

[0104] In this article, relational terms such as first and second, etc. are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, principle, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, principle, article or device. In the absence of further restrictions, the elements defined by the statement "include..." do not exclude the presence of other identical elements in the process, principle, article or device including the elements.

[0105] Embodiment 1:

[0106] An array IMU high-precision angular velocity, acceleration and zero bias online estimation method can digitally fuse multiple inertial measurement unit arrays to improve motion estimation accuracy and complete online estimation of acceleration and zero bias, including the following steps:

[0107] 1) Use gyroscope and accelerometer data to measure the angular velocity and acceleration of each inertial measurement unit;

[0108] 2) Use the extended Kalman filter to estimate the angular velocity, the acceleration at the center of the array, and the zero bias of the accelerometer;

[0109] 3) Calculate the angular velocity and acceleration of all inertial measurement units based on the estimated angular velocity, the acceleration at the center of the array, and the zero bias of the accelerometer;

[0110] 4) Fit the motion state with functions piece by piece. If a matching function is fitted, the extended Kalman filter is replaced with window recursive estimation.

[0111] Embodiment 2:

[0112] This embodiment is further optimized on the basis of the above embodiment, and the same points as the above technical solution are not repeated here. In order to better realize the array IMU high-precision angular velocity, acceleration and zero bias online estimation method described in the present invention, the following setting is particularly adopted: the step 1) directly outputs the three-axis acceleration and the three-axis angular velocity of each inertial measurement unit carrier coordinate system after measurement;

[0113] ω i =ω gi -b g ;

[0114] a i =a ai ;

[0115] Among them, ω gi is the raw angular velocity reading of the i-th inertial measurement unit, ω i is the angular velocity value of the Kalman filter in step 2), a ai is the raw reading of the accelerometer of the i-th inertial measurement unit, a i is the acceleration value of the Kalman filter entering step 2), b g is the pre-calibrated gyroscope bias.

[0116] Embodiment 3:

[0117] This embodiment is further optimized on the basis of any of the above embodiments, and the same points as the above technical solutions are not repeated here. In order to better realize the array IMU high-precision angular velocity, acceleration and zero bias online estimation method described in the present invention, the following setting method is particularly adopted: the step 2) is specifically:

[0118] 2.1) The geometric center of the array is selected as the reference point. The specific force of each inertial measurement unit is synthesized by the specific force of the geometric center of the array and the centripetal force and torque:

[0119] f i =f o +ω×(ω×r i )+α×r i +b i +n (1);

[0120] Among them, ω is the angular velocity around the instantaneous center to be estimated, that is, the real output of the gyroscope, α is the angular acceleration around the instantaneous center, and f o is the acceleration of the array geometric center, f i is the i-th accelerometer reading in step 1); b i is the zero bias of the i-th accelerometer at the current moment; r i is the position radius vector of the i-th inertial measurement unit of the array relative to the geometric center of the array;

[0121] The measurement signal model of the present invention is derived from classical mechanics. The accelerometer signal at any point on the inertial measurement unit array is equal to the inertial measurement unit array geometric center specific force, centrifugal force, torque caused by angular acceleration, and accelerometer zero bias synthesis. The state recursion model is given by the first-order Markov model where the angular velocity is equal to the angular velocity plus angular acceleration at the previous moment and the accelerometer zero bias. Therefore, the state to be estimated is the real-time estimated value of the angular velocity, angular acceleration, array geometric center specific force, and four inertial measurement unit zero biases;

[0122] 2.2) Estimate the angular velocity, angular acceleration, array geometric center force, and real-time estimates of the zero bias of the four inertial measurement units:

[0123] Define the state variables:

[0124]

[0125] Where f is the acceleration of the center of the array to be estimated, b is the zero bias of the four accelerometers at the current moment, which is a 12×1 column vector composed of the three-axis zero bias of the four accelerometers arranged longitudinally;

[0126] The measurement equation is:

[0127] ω is the angular velocity around the instantaneous center to be estimated, that is, the real output of the gyroscope; α is the angular acceleration around the instantaneous center, and r is the position vector of the inertial measurement unit relative to the geometric center of the array; among them, the current angular velocity is synthesized by the angular velocity and the angular velocity increment at the previous moment, and the zero bias b of the accelerometer at the current moment is calculated using the first-order Markov model:

[0128]

[0129] in, It represents the rate of change of zero bias, τ is the inverse of the accelerometer zero bias related time, n b is the accelerometer white noise;

[0130] The signal model is:

[0131]

[0132] Among them, ω o Represents the angular velocity of the IMU array;

[0133] 2.3) Using the extended Kalman filter, the measurement values ​​of each inertial measurement unit in the array are integrated to estimate the angular velocity, angular acceleration, specific force, and zero bias of each inertial measurement unit (taking 4 as an example) at the geometric center of the inertial measurement unit array:

[0134]

[0135] in:

[0136]

[0137] Among them, x k represents the state quantity to be estimated at time k, z k represents the measured value to be estimated at time k, v ω ,v α ,v f ,v b are the process noises of angular velocity around the instantaneous center, angular acceleration around the instantaneous center, and translation acceleration, respectively, w k is the measurement noise of the state quantity, T is the sampling period, τ is the inverse of the accelerometer zero bias correlation time, fi is the i-th accelerometer measurement, ω i is the i-th gyroscope measurement value, b1 is the first gyroscope measurement value, b2 is the second gyroscope measurement value, b3 is the third gyroscope measurement value, and b4 is the fourth gyroscope measurement value; r1 is the first inertial measurement unit position vector, b2 is the second inertial measurement unit position vector, b3 is the third inertial measurement unit position vector, b4 is the fourth inertial measurement unit position vector, and f is the array center acceleration to be estimated;

[0138] Right k =h(x k )+w k h(x k ) Find the first-order Jacobian matrix H k (x):

[0139]

[0140] Among them, Ω is the antisymmetric matrix of the three-dimensional vector, I3 is the three-dimensional identity matrix, I 12×12 is a 12-dimensional identity matrix;

[0141] One-step prediction is done as follows:

[0142] X k+1 =ΦX k (7);

[0143] X k represents the state quantity to be estimated at time k, and Φ represents the state transfer matrix;

[0144] The mean square error matrix is ​​calculated by the following formula:

[0145] P k+1 / k =ΦP k+1 Φ T +Q (8);

[0146] The filter gain is calculated by the following formula:

[0147] H k represents the measurement matrix at time k;

[0148] Update the state by:

[0149] X k =X k+1 / k +K k (z k -h(X k+1 / k ))(10);K k represents the filter gain at time k, X k+1 / k Represents the state quantity predicted at time k;

[0150] Update the mean square error matrix by:

[0151] P k+1 =P k -K k H k P k (11), where Q = Ε(v k v j ),R=Ε(w k w j ) (12);

[0152] Q is the process noise matrix, R is the measurement noise matrix, and E() represents the expectation of the random vector.

[0153] Embodiment 4:

[0154] This embodiment is further optimized on the basis of any of the above embodiments, and the same points as the above technical solutions are not repeated here. In order to better realize the online estimation method of high-precision angular velocity, acceleration and zero bias of an array IMU described in the present invention, the following setting is particularly adopted: the step 3) calculates the angular velocity and acceleration of all inertial measurement units according to the estimated angular velocity, the acceleration of the center of the array, and the zero bias of the accelerometer, specifically:

[0155] 3.1) The ω obtained in step 2) k , a k Update the attitude, velocity and position;

[0156] In the "east-north-sky" navigation coordinate system, that is, the n-system, the carrier's attitude differential equation is:

[0157]

[0158] in, To get the vector The antisymmetric matrix of “×” represents the antisymmetric matrix of the orientation quantity; is the attitude matrix of system b relative to system n, is the first-order derivative of the attitude matrix, is the attitude rate, Transformed into:

[0159]

[0160] Substituting formula (14) into formula (13), we can obtain:

[0161]

[0162] In the formula, is the angular velocity output by the gyroscope, i.e., ω in step 2). k , and From the carrier latitude and the velocity in the n system, we can get:

[0163]

[0164] In the formula, R M Represents the radius of curvature of the Earth's meridian, R N R represents the radius of curvature of the Earth's circumplex, e represents the reference radius of the earth, e represents the eccentricity of the earth's ellipse; ω ie represents the angular velocity of the earth's rotation, h represents the local altitude, L represents the local latitude, and the speed v in the n system n =[v E v N v U ], where v E 、v N and v U They represent the eastward, northward and celestial components of the carrier velocity respectively;

[0165] To update the speed:

[0166]

[0167] in, Represents the acceleration of ground speed in the navigation coordinate system, is the specific force value measured by the accelerometer, is the Coriolis acceleration obtained by the interaction of the carrier's own acceleration and the earth's rotation angular velocity, is the centripetal acceleration of system n relative to system e, g n represents the local gravitational acceleration;

[0168] In formula (20), Simple note to do v n , and considering the influence of time t, as follows:

[0169]

[0170] represents the acceleration of the ground speed in the navigation coordinate system. m-1 ,t m ] to integrate, we get:

[0171]

[0172] Right now:

[0173]

[0174] in, and t m-1 and t m The inertial velocity at time is:

[0175]

[0176] In the formula, and T=t m -t m-1 n is the specific force velocity increment and harmful acceleration increment during the time period;

[0177] According to formula (23), the speed expression is:

[0178]

[0179] △θ m is the gyro sampling angle increment, and △v m It is the accelerometer sampling specific force velocity increment; The amount of rotation error compensation is called velocity.

[0180] Embodiment 5:

[0181] This embodiment is further optimized on the basis of any of the above embodiments, and the similarities with the above technical solutions are not repeated here. In order to better realize the array IMU high-precision angular velocity, acceleration and zero bias online estimation method described in the present invention, the following setting method is particularly adopted: the step 4) performs piecewise function fitting on the motion state, and if a matching function is fitted, the extended Kalman filter is replaced with a window recursive estimation, including the following specific steps:

[0182] Perform function fitting on a motion state. If a matching function is fitted, replace equation (6) with the following equation (28):

[0183] Assume that the angular velocity is v, △t is the sampling time, η p is the noise, β is an unknown parameter, and

[0184]

[0185] With a window length of l, the least square solution of β is as follows:

[0186]

[0187] Calculate the estimated state x k :

[0188]

[0189] in:

[0190]

[0191] get:

[0192]

[0193] State transition matrix Φ (k-n:k-l) for:

[0194]

[0195] Represents the covariance matrix:

[0196]

[0197] The above is only a preferred embodiment of the present invention and is not intended to limit the scope of protection of this application. For those skilled in the art, this application may have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the principles, ideas, spirits and principles of this application shall be included in the scope of protection of this application.

Claims

1. A method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU, characterized by: The steps include: 1) Use gyroscope and accelerometer data to measure the angular velocity and acceleration of each inertial measurement unit; 2) Use the extended Kalman filter to estimate the angular velocity, the acceleration at the center of the array, and the zero bias of the accelerometer; 3) Calculate the angular velocity and acceleration of all inertial measurement units based on the estimated angular velocity, the acceleration at the center of the array, and the zero bias of the accelerometer; 4) Fit the motion state with functions piece by piece. If a matching function is fitted, the extended Kalman filter is replaced with window recursive estimation.

2. The method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU according to claim 1, characterized in that: The step 1) directly outputs the three-axis acceleration and the three-axis angular velocity in the carrier coordinate system of each inertial measurement unit after measurement; oh i =ω gi -b g ; a i =a ai ; Among them, ω gi is the raw angular velocity reading of the i-th inertial measurement unit, ω i is the angular velocity value of the Kalman filter in step 2), a ai is the raw reading of the accelerometer of the i-th inertial measurement unit, a i is the acceleration value of the Kalman filter entering step 2), b g is the pre-calibrated gyroscope bias.

3. The method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU according to claim 1, characterized in that: The step 2) is specifically as follows: 2.1) The geometric center of the array is selected as the reference point. The specific force of each inertial measurement unit is synthesized by the specific force of the geometric center of the array and the centripetal force and torque: f i =f o +ω×(ω×r i )+α×r i +b i +n (1); Among them, ω is the angular velocity around the instantaneous center to be estimated, that is, the real output of the gyroscope, α is the angular acceleration around the instantaneous center, and f o is the acceleration of the array geometric center, f i is the i-th accelerometer reading in step 1); b i is the zero bias of the i-th accelerometer at the current moment; r i is the position radius vector of the i-th inertial measurement unit of the array relative to the geometric center of the array; 2.2) Estimate the real-time estimated values ​​of angular velocity, angular acceleration, array geometric center force, and inertial measurement unit zero bias: Define the state variables: Among them, f is the acceleration of the center of the array to be estimated, b is the zero bias of the accelerometer at the current moment, which is a 12×1 column vector composed of the three-axis zero bias of the accelerometer arranged longitudinally; The measurement equation is: ω is the angular velocity around the instantaneous center to be estimated, that is, the real output of the gyroscope; α is the angular acceleration around the instantaneous center, and r is the position vector of the inertial measurement unit relative to the geometric center of the array; among them, the current angular velocity is synthesized by the angular velocity and the angular velocity increment at the previous moment, and the zero bias b of the accelerometer at the current moment is calculated using the first-order Markov model: in, It represents the rate of change of zero bias, τ is the inverse of the time of accelerometer zero bias, n b is the accelerometer white noise; The signal model is: Among them, ω o Represents the angular velocity of the IMU array; 2.3) Using the extended Kalman filter, the measurement values ​​of each inertial measurement unit in the array are integrated to estimate the angular velocity, angular acceleration, specific force, and zero bias of each inertial measurement unit array geometric center: in: Among them, x k represents the state quantity to be estimated at time k, z k represents the measured value to be estimated at time k, v ω ,v α ,v f ,v b are the process noises of angular velocity around the instantaneous center, angular acceleration around the instantaneous center, and translation acceleration, respectively, w k is the measurement noise of the state quantity, T is the sampling period, τ is the inverse of the accelerometer zero bias correlation time, f i is the i-th accelerometer measurement, ω i is the i-th gyroscope measurement value, b1 is the first gyroscope measurement value, b2 is the second gyroscope measurement value, b3 is the third gyroscope measurement value, and b4 is the fourth gyroscope measurement value; r1 is the first inertial measurement unit position vector, b2 is the second inertial measurement unit position vector, b3 is the third inertial measurement unit position vector, b4 is the fourth inertial measurement unit position vector, and f is the array center acceleration to be estimated; Right k =h(x k )+w k h(x k ) Find the first-order Jacobian matrix H k (x): Among them, Ω is the antisymmetric matrix of the three-dimensional vector, I3 is the three-dimensional identity matrix, I 12×12 is a 12-dimensional identity matrix; One-step prediction is done as follows: X k+1 =ΦX k (7); X k represents the state quantity to be estimated at time k, and Φ represents the state transfer matrix; The mean square error matrix is ​​calculated by the following formula: P k+1 / k =ΦP k+1 Φ T +Q (8); The filter gain is calculated by the following formula: H k represents the measurement matrix at time k; Update the state by: X k =X k+1 / k +K k (z k -h(X k+1 / k )) (10); K k represents the filter gain at time k, X k+1 / k Represents the state quantity predicted at time k; Update the mean square error matrix by: P k+1 = P k - K k H k P k (11), where Q = Ε(v k v j ), R = Ε(w k w j ) (12); Q is the process noise matrix, R is the measurement noise matrix, and E() represents the expectation of the random vector.

4. The method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU according to claim 3, characterized in that: The step 3) calculates the angular velocity and acceleration of all inertial measurement units according to the estimated angular velocity, the acceleration of the array center, and the zero bias of the accelerometer, specifically: 3.1) The ω obtained in step 2) k , a k Update the attitude, velocity and position; In the "east-north-sky" navigation coordinate system, that is, the n system, the carrier's attitude differential equation is: in, To get the vector The antisymmetric matrix of , "×" represents the antisymmetric matrix of the orientation quantity; is the attitude matrix of system b relative to system n, is the first-order derivative of the attitude matrix, is the attitude rate, Transformed into: Substituting formula (14) into formula (13), we can obtain: In the formula, is the angular velocity output by the gyroscope, i.e., ω in step 2). k , and From the carrier latitude and the velocity in the n system, we can get: In the formula, R M Represents the radius of curvature of the Earth's meridian, R N R represents the radius of curvature of the Earth's circumplex, e represents the reference radius of the earth, e represents the eccentricity of the earth's ellipse; ω ie represents the angular velocity of the earth's rotation, h represents the local altitude, L represents the local latitude, and the speed v in the n system n =[v E v N v U ], where v E 、v N and v U They represent the eastward, northward and celestial components of the carrier velocity respectively; To update the speed: in, Represents the acceleration of ground speed in the navigation coordinate system, is the specific force value measured by the accelerometer, is the Coriolis acceleration obtained by the interaction of the carrier's own acceleration and the earth's rotation angular velocity, is the centripetal acceleration of system n relative to system e, g n represents the local gravitational acceleration; In formula (20), Simple note to do v n , and considering the influence of time t, as follows: represents the acceleration of the ground speed in the navigation coordinate system. m-1 ,t m ] to integrate, we get: Right now: (23); in, and t m-1 and t m The inertial velocity at time is: In the formula, and T=t m -t m-1 n is the specific force velocity increment and harmful acceleration increment during the time period; According to formula (23), the speed expression is: △θ m is the gyro sampling angle increment, and △v m It is the accelerometer sampling specific force velocity increment; The amount of rotation error compensation is called velocity.

5. The method for online estimation of high-precision angular velocity, acceleration and zero bias of an array IMU according to claim 1, characterized in that: The step 4) performs piecewise function fitting on the motion state. If a matching function is fitted, the extended Kalman filter is replaced with a window recursive estimation, which includes the following specific steps: Perform function fitting on a motion state. If a matching function is fitted, replace equation (6) with the following equation (28): Assume that the angular velocity is v, △t is the sampling time, η p is the noise, β is an unknown parameter, and With a window length of l, the least square solution of β is as follows: Calculate the estimated state x k : in: get: State transition matrix Φ (k-n:k-l) for: Represents the covariance matrix:

Citation Information

Cited By

  • Carrier acceleration calculation method with complementary high and low frequency noise characteristics

    CN117849395A