Attitude calculation method in high dynamic environment based on different-plane MEMS IMU array

Abstract

The invention relates to an attitude resolving method in a high dynamic environment based on a different-plane MEMS IMU array, belongs to the technical field of inertial navigation, and particularly relates to an IMU array data fusion method. According to the invention, the plurality of MEMS IMUs are arranged in an array according to a specific different-plane structure, the geometric characteristics of the sensor layout are ingeniously utilized to offset the influence of zero offset and environmental factors, and the measurement precision is improved. The characteristics of the accelerometer array and the gyroscope array are respectively utilized to fuse the angular velocity for the collected inertial data, so that the system can accurately obtain the angular velocity of the system in a high-dynamic environment and even an environment with a super gyroscope range, thereby effectively improving the attitude calculation precision in the high-dynamic environment. According to the method provided by the invention, the attitude calculation precision in the high-dynamic environment is improved, and a new thought is provided for solving high-precision attitude calculation in the high-dynamic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of inertial navigation, and particularly relates to a method for attitude calculation of a non-coplanar array inertial measurement unit in a high-dynamic environment. Background Technique

[0002] Attitude estimation is the core basis in fields such as inertial navigation, industrial control, and robotics. Therefore, accurately obtaining the attitude information of the carrier is not only the key to improving the performance of these technologies but also an important research direction currently. Attitude calculation usually relies on inertial sensors to sense the motion information of the carrier and completes the calculation with the help of a specific algorithm platform. Among the core components that make up the Inertial Measurement Unit (IMU), gyroscopes and accelerometers, as mechatronic products, have significantly lower reliability than other components. At the same time, the accuracy and sensitivity of low-cost Micro Electro Mechanical System (MEMS) inertial sensors are relatively low. Especially in a high-dynamic environment, violent linear motion and angular motion may cause the gyroscope to exceed its range, further affecting the attitude calculation accuracy of the system. Therefore, improving the accuracy and reliability of inertial devices is a research topic with important value.

[0003] By using redundancy technology, increasing a certain number of inertial devices and developing a supporting processor, the reliability of the system can be improved. And by using the layout of a non-planar IMU, the performance can cover all axes by designing the placement position of the IMU, thereby improving the isotropy performance of the navigation system and reducing single-axis drift. Furthermore, it becomes an inertial navigation system with relatively high accuracy and reliability.

[0004] The prior application of the inventor, CN116380078A, a method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment, belongs to the technical field of inertial navigation. The present invention includes the following steps: raw data acquisition; calculating the initial attitude angle; calculating the initial quaternion; constructing an equivalent rotation vector; constructing an attitude change quaternion; performing quaternion update; performing attitude angle update. Different from the traditional equivalent rotation vector algorithm that assumes the carrier motion angular velocity is a polynomial of several degrees, the present invention assumes that the carrier motion angular velocity can be represented by the sum of trigonometric functions with different frequencies. Based on this, an equivalent rotation vector represented by the angular increment and the angular velocity correlation coefficient is derived, and then an attitude calculation method based on this equivalent rotation vector is designed.

[0005] The high-dynamic attitude calculation of patent CN116380078A is an improvement on the traditional attitude calculation algorithm. To address the problem of insufficient correction of non-commutative errors, an equivalent rotation vector is derived by assuming that the carrier motion angular velocity is in the form of the sum of trigonometric functions with different frequencies to achieve high-dynamic attitude calculation.

[0006] The present invention achieves attitude solution in a high-dynamic environment by improving the sensor to obtain more accurate and larger-range angular velocity. First, the differential IMU is used to cancel the influence of zero bias and environmental factors; then, 4 pairs of differential pairs are arranged in a specific non-coplanar structure, and the gyroscope array fuses and outputs the angular velocity. The accelerometer array can calculate the angular velocity through the lever effect. Complementary filtering is used to fuse the two angular velocities, so as to obtain a more accurate and larger-range angular velocity; finally, attitude solution is performed through error quaternion Kalman filtering. Summary of the Invention

[0007] The present invention aims to solve the above problems of the prior art. A method for attitude solution in a high-dynamic environment based on a non-coplanar MEMS IMU array is proposed. The technical solution of the present invention is as follows:

[0008] A method for attitude solution in a high-dynamic environment based on a non-coplanar MEMS IMU array, comprising the following steps:

[0009] Step 1: Collect the original data of the IMU array sensor;

[0010] Step 2: Fuse the angular velocity and acceleration data of four pairs of differential IMUs;

[0011] Step 3: Fuse the IMU array data and output the system angular velocity and acceleration;

[0012] Step 4: Calculate the initial attitude angle and the initial quaternion;

[0013] Step 5: Construct an error quaternion Kalman filter for attitude quaternion update;

[0014] Step 6: Perform attitude angle update.

[0015] Further, in the above Step 1, the original attitude data of the IMU array system is collected, and the right-front-up coordinate system is used as the IMU installation coordinate system for attitude solution; the northeast-up coordinate system is used as the inertial system navigation coordinate system.

[0016] Further, the step of fusing the angular velocity and acceleration data of four pairs of differential IMUs specifically includes:

[0017] The outputs of the Z axes of the two gyroscopes of the differential IMU are:

[0018]

[0019] In the formula, ω z represents the true angular rate of the Z axis; S z1 、S z2 、N z1 、Nz2 , b z1 , b z2 , ε z1 and ε z2 represent the measurement values, scale factor error, cross-coupling error, bias drift, and random noise of the front sensor and the rear sensor, respectively. Therefore, the fusion output of the differential IMU pair can be calculated by the following formula:

[0020]

[0021] Similarly, the outputs of the differential pair IMU gyroscope on the X and Y axes and the accelerometer on the X, Y, and Z axes are:

[0022]

[0023] In the formula, represent the angular velocities of the Y and Z axes of the IMU differential pair, respectively; represent the accelerations of the X, Y, and Z axes of the IMU differential pair, respectively. The subscript F represents the front sensor; the subscript B represents the rear sensor.

[0024] Furthermore, the fusion of the IMU array data in step 3 specifically includes:

[0025] The measurement equation of the gyroscope array is:

[0026]

[0027] ω g =(D T wD) -1 Dwm

[0028] In the formula, is the angular velocity in the system framework; ω g is the angular velocity output by the gyroscope fusion at time t; is the measurement of 4 sensor pairs; D is called the sensor geometric measurement matrix, which is determined according to the placement position of the IMU; w is the weighting matrix; v i is the measurement error.

[0029] Furthermore, the calculation steps of the array angular acceleration in step 3 are calculated by the following method:

[0030] The specific force output by each accelerometer is synthesized by the specific force at the geometric center of the array, the centripetal force, and the rotational torque:

[0031]

[0032] In the formula, f i is the true output of the i-th accelerometer; is the angular acceleration; f ois the acceleration of the array geometric center; r i is the position vector of the i-th accelerometer in the array relative to the geometric center of the array;

[0033] Subtracting the outputs of the two accelerometers can cancel out the central acceleration in the measurement model.

[0034]

[0035] Adjust the angular acceleration term to the other side of the equation by shifting the term:

[0036]

[0037] In the formula, r Δij is the difference between the position vectors of accelerometers numbered i and j in the array relative to the geometric center of the array; since the matrix on the left side of the equation is a singular matrix, the angular acceleration cannot be solved It is necessary to introduce another set of lever arm acceleration differences and solve the two sets of equations in parallel to obtain:

[0038]

[0039] make:

[0040]

[0041] From the above formula, the angular acceleration can be obtained by the least squares method:

[0042]

[0043] The calculation method for calculating the angular velocity of the accelerometer array is as follows:

[0044]

[0045] In the formula, ω a is the angular velocity calculated by lever effect at time t, δT is the sampling interval; ω a-T is the angular velocity calculated by lever effect at the previous moment t-δT; is the angular acceleration calculated by the lever effect at time t.

[0046] When the system angular velocity is less than the set value, the gyroscope array output is directly used. As the system angular velocity; when the angular velocity is greater than the set value, it is output by fusing the gyroscope array The angular velocity calculated based on the accelerometer lever effect formula Get the system angular velocity; when the angular velocity is greater than the set threshold, use the angular velocity calculated based on the accelerometer lever effect formula as the system angular velocity;

[0047]

[0048] Wherein, is the angular velocity of the IMU array system; K is the adjustable parameter of the angular velocity complementary filter; FSR is the range of the gyroscope; is the angular velocity at the previous moment;

[0049] The acceleration of the i-th pair of IMUs converted to the geometric center of the system is:

[0050]

[0051] By converting the acceleration data of the four pairs of IMUs to the geometric center and averaging them, the final acceleration of the system is obtained.

[0052] Furthermore, calculating the initial attitude angle in step 4 specifically includes: the pitch angle θ0 and roll angle γ0 in the initial attitude angle are calculated from the data of the accelerometer within the first set of attitude update periods, and the calculation formulas are:

[0053]

[0054] Wherein, arctan is the arctangent function, and the calculation result range is [-π / 2, π / 2]; arctan2 is the four-quadrant arctangent function, and the calculation result range is [-π, π]. The unit of the initial attitude angle is radians; a x , a y , a z respectively represent the IMU array acceleration information in the vehicle coordinate system;

[0055] Said one attitude update period refers to a time period composed of two sampling points, that is, attitude calculation is performed every two samplings.

[0056] Furthermore, the initial quaternion in step 4 is calculated in the following manner:

[0057] Denote the transformation matrix from the vehicle coordinate system (b system) to the navigation coordinate system (n system) derived by the coordinate rotation method as:

[0058]

[0059] Wherein, γ, θ, and ψ respectively represent the roll angle, pitch angle, and heading angle; represents the coordinate transformation matrix from the b system to the n system; T 11 , T 12 , T 13 , T 21 , T 22 , T 23 , T 31 , T32 , T 33 are the abbreviations of the expressions of the corresponding elements in the matrix respectively;

[0060] Meanwhile, the transformation matrix from the self-carrier coordinate system to the navigation coordinate system represented by quaternions is denoted as:

[0061]

[0062] where q0, q1, q2, q3 are the four real elements of the quaternion q = q0 + q1i + q2j + q3k, satisfying the relation: i, j, k respectively represent the unit vectors along the three-dimensional coordinate system;

[0063] The transformation matrices represented by the two methods are equal. By combining the two matrices, the four elements of the initial quaternion can be obtained as:

[0064]

[0065] where sign is the sign function, which outputs +1 when the input is positive and -1 when the input is negative.

[0066] Furthermore, in step 5, the attitude quaternion update by the error quaternion Kalman filter is constructed by the following steps:

[0067] Establish the system state update equation:

[0068]

[0069] where x is the state vector; is the nominal state; δx is the error state; is the nominal estimated quaternion; δq is the error quaternion; b g is the gyroscope zero bias; δb g is the error gyroscope zero bias; δα = [δγ δθ δψ] T is a rotation vector, whose components are the roll angle, pitch angle and yaw angle respectively; ||δα|| represents the modulus of δα;

[0070] According to the error quaternion differential equation and the gyroscope drift mathematical model, the following error quaternion Kalman filter state transition model can be established:

[0071] δx k = φ k,k-1 δx k-1 + w k-1

[0072]

[0073] Wherein, δT is the sampling interval time; I is the identity matrix.

[0074] Furthermore, the output of the accelerometer is the observable of the error quaternion Kalman filter:

[0075] Z k = Hx k + v k

[0076]

[0077] Wherein, Z k represents the system measurement; H represents the system measurement matrix; v k represents the system measurement noise;

[0078] The measurement noise matrix R k is:

[0079] R k = E(v k v k T ) = R a + R z

[0080] R z = c 2 ||f - g b || 2

[0081] Wherein, f is the acceleration of the IMU array; R a is the accelerometer measurement noise covariance matrix; R z is the adaptively adjusted covariance matrix; c is the system adjustable parameter.

[0082] Furthermore, in the step 5, the measurement noise matrix is compensated by using the adaptively adjusted covariance matrix, and the error quaternion update value is obtained by using the error quaternion Kalman filter, the true attitude quaternion is calculated, and the attitude angle is obtained by attitude calculation according to the quaternion method.

[0083] The advantages and beneficial effects of the present invention are as follows:

[0084] The present invention proposes to arrange multiple MEMSIMUs into an array in a specific non-planar structure, and utilize the redundant information of multiple sensors to improve the accuracy and reliability of attitude solution through data fusion. First, the geometric characteristics of the sensor layout are utilized to offset the influence of zero bias and environmental factors on the differential IMU data through differential processing, thereby improving the measurement accuracy. Secondly, the output of the gyroscope array is complementary filtered with the angular velocity calculated based on the accelerometer lever effect formula, so that the system can still ensure the angular velocity estimation accuracy of the system even in an environment where the gyroscope is out of range. Finally, the error quaternion Kalman filter is used to achieve continuous and accurate attitude estimation under extreme conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] Figure 1 It is a general flow chart of the IMU array data fusion attitude solution method in a high dynamic environment provided by the preferred embodiment of the present invention.

[0086] Figure 2 It is a schematic diagram of the installation position of the IMU array sensor and the coordinates of the sensor in a non-orthogonal frame.

[0087] Figure 3 A side view of the differential IMU hardware components and a view from the front sensor.

[0088] Figure 4 It is a coordinate diagram of the front and rear sensors of the differential IMU. DETAILED DESCRIPTION

[0089] The following will describe the technical solutions in the embodiments of the present invention in detail in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention.

[0090] The technical solution of the present invention to solve the above technical problems is:

[0091] The technical solution of the present invention to solve the above technical problems is to refer to Figure 1 As shown in the figure, a method for solving attitude calculation using IMU array data fusion in a high dynamic environment is constructed, which includes the following steps:

[0092] Step 1: Collect raw data from IMU array sensors;

[0093] Step 2: Fuse the angular velocity and acceleration data of the four pairs of differential IMU pairs;

[0094] Step 3: Fuse the IMU array data to output the system angular velocity acceleration;

[0095] Step 4: Calculate the initial attitude angle and initial quaternion;

[0096] Step 5: Construct the error quaternion Kalman filter to update the attitude quaternion;

[0097] Step 6: Update the attitude angle.

[0098] In this embodiment, in the said Step 1, the placement position of the IMU array sensor is as Figure 2 shown. Install the IMU array system on the carrier to be measured, and collect the carrier motion acceleration information and angular velocity information through the internal inertial sensors of the IMU, where the acceleration information is obtained by the accelerometer and the angular velocity information is obtained by the gyroscope.

[0099] In this embodiment, in the said Step 1, the IMU installation coordinate system is the right front up coordinate system, that is, the X b axis of the IMU points to the right side of the carrier, the Y b axis points to the front of the carrier, and the Z b axis points to the skyward direction of the carrier; the inertial system navigation coordinate system is the northeast sky coordinate system, that is, the X n axis of the navigation coordinate system points to the geographical east direction, the Y n axis points to the geographical north direction, and the Z n axis points to the geographical skyward direction.

[0100] In this embodiment, in the said Step 2, the differential IMU performs data fusion, and the hardware placement of each pair of differential IMUs is as Figure 3 shown. The outputs of the Z axes of the two gyroscopes are:

[0101]

[0102] In the formula, ω z represents the true angular rate of the Z axis; S z1 、S z2 、N z1 、N z2 、b z1 、b z2 、ε z1 and ε z2 respectively represent the measurement values of the front sensor and the rear sensor, the scale factor error, the cross-coupling error, the bias drift and the random noise. Therefore, the fusion output of the differential IMU pair can be calculated by the following formula:

[0103]

[0104] Similarly, the outputs of the X and Y axes of the differential pair IMU gyroscopes and the X, Y, and Z axes of the accelerometers are:

[0105]

[0106] In the formula, respectively represent the angular velocities of the Y and Z axes of the IMU differential pair; They respectively represent the accelerations of the IMU differential pairs on the X, Y, and Z axes. The subscript F represents the front sensor; the subscript B represents the rear sensor.

[0107] In this embodiment, the step 3 array IMU measurement fusion is constructed by the following steps:

[0108] Step 3.1: IMU array gyroscope angular velocity measurement fusion;

[0109] Step 3.2: IMU array accelerometer estimates angular acceleration, angular velocity, and acceleration;

[0110] Step 3.3: IMU array system fuses angular velocity.

[0111] In this embodiment, the installation of the IMU in the step 3.1 is as Figure 2 shown. S i is the unit vector of the sensitive axis of the i-th gyroscope and can be expressed as:

[0112] S i = cosα i ·cosβ i i + cosα i ·sinβ i j + sinα i k

[0113] In the formula, α i , β i are the installation angles of the i-th gyroscope relative to the reference body coordinate system (X b , Y b , Z b ); the symbols i, j, k are the unit vectors along the corresponding axes of the reference body coordinate system. The sensor measurement equation is:

[0114]

[0115] Our system contains 4 groups of paired sensor data, identified by 1, 2, 3, 4. The measurement equation of the original gyroscope system can be expressed as follows:

[0116]

[0117] ω g = (D T wD) -1 Dwm

[0118] In the formula, is the angular velocity in the system framework; ω g is the angular velocity output by the gyroscope fusion at time t; is the measurement of the four sensor pairs; D is called the sensor geometry measurement matrix, which is determined by the placement of the IMU; w is the weighting matrix; v i is the measurement error.

[0119] In this embodiment, the calculation steps of the IMU array accelerometer estimating angular acceleration, angular velocity, and acceleration in step 3.2 are calculated by the following method:

[0120] The specific force output by each accelerometer is synthesized by the specific force at the geometric center of the array and the centripetal force and torque:

[0121]

[0122] In the formula, f i is the true output of the i-th accelerometer; is the angular acceleration; f o is the acceleration of the array geometric center; r i is the position vector of the i-th accelerometer in the array relative to the geometric center of the array.

[0123] Subtracting the outputs of the two accelerometers can cancel out the central acceleration in the measurement model:

[0124]

[0125] Adjust the angular acceleration term to the other side of the equation by shifting the term:

[0126]

[0127] In the formula, r Δij is the difference in position vector of accelerometers numbered i and j in the array relative to the geometric center of the array. Since the matrix on the left side of the equation is a singular matrix, the angular acceleration cannot be directly solved by this formula. To overcome this limitation, another set of lever arm acceleration differences needs to be introduced, and the two sets of equations are solved simultaneously to obtain:

[0128]

[0129] make:

[0130]

[0131] From the above formula, the angular acceleration can be obtained by the least squares method:

[0132]

[0133] For an array system consisting of four pairs of IMUs, after obtaining the system angular velocity, the final angular acceleration estimate of the system can be obtained by calculating the average value of the differential IMU angular velocity data.

[0134] The calculation method for estimating the angular velocity by the accelerometer array is as follows:

[0135]

[0136] In the formula, ω a is the angular velocity calculated by the lever effect at time t, δT is the sampling interval time; ω a-T is the angular velocity calculated by the lever effect at the previous time t - δT; is the angular acceleration calculated by the lever effect at time t.

[0137] When the system angular velocity is less than the set value, the output of the gyroscope array is directly used as the system angular velocity; when the angular velocity is greater than the set value, by fusing the output of the gyroscope array and the angular velocity calculated by solving the accelerometer lever effect formula the system angular velocity is obtained; when the angular velocity is greater than the set threshold, the angular velocity calculated by the accelerometer lever effect formula is used as the system angular velocity;

[0138]

[0139] In the formula, is the angular velocity of the IMU array system; K is the adjustable parameter of the angular velocity complementary filter; FSR is the range of the gyroscope; is the angular velocity at the previous moment.

[0140] The acceleration of the i-th pair of IMU accelerations converted to the system geometric center is:

[0141]

[0142] By converting the acceleration data of the four pairs of IMUs to the geometric center and averaging them, the final acceleration of the system is obtained.

[0143] In this embodiment, calculating the initial attitude angle in step 4 specifically includes: the pitch angle θ0 and roll angle γ0 in the initial attitude angle are calculated from the accelerometer data within the first set of attitude update periods, and the calculation formulas are:

[0144]

[0145] In the formula, arctan is the arctangent function, and the calculation result range is [-π / 2, π / 2], arctan2 is the four-quadrant arctangent function, and the calculation result range is [-π, π]. The unit of the initial attitude angle is radians, a x , a y , a zThey respectively represent the IMU array acceleration information in the carrier coordinate system.

[0146] The attitude update cycle refers to a time period consisting of two sampling points, that is, the attitude solution is performed once every two samplings.

[0147] In this embodiment, the initial quaternion in step 4 is calculated by the following method:

[0148] The transformation matrix from the carrier coordinate system (b system) to the navigation coordinate system (n system) derived from the coordinate system rotation method is:

[0149]

[0150] Where γ, θ, and ψ represent the roll angle, pitch angle, and heading angle, respectively. represents the coordinate transformation matrix from b system to n system; T 11 、T 12 、T 13 、T 21 、T 22 、T 23 、T 31 、T 32 、T 33 The matrices are A brief note of the formula for the corresponding element in .

[0151] At the same time, the transformation matrix from the vehicle coordinate system to the navigation coordinate system represented by the quaternion is recorded as:

[0152]

[0153] Where q0, q1, q2, q3 are the four real elements of the quaternion q = q0 + q1i + q2j + q3k, satisfying the relationship: i, j, and k represent unit vectors along the three-dimensional coordinate system.

[0154] The transformation matrices expressed in the two ways are equal. By combining the two matrices, the four elements of the initial quaternion can be obtained as follows:

[0155]

[0156] Where sign is the sign function, which outputs +1 when the input is a positive number and -1 when the input is a negative number.

[0157] In this embodiment, in step 5, the error quaternion Kalman filter performs attitude quaternion update according to the following steps:

[0158] Step 5.1: Initialization;

[0159] Step 5.2: Error Kalman filter nominal state update;

[0160] Step 5.3: Error state update of the error Kalman filter;

[0161] Step 5.4: Update of the error Kalman filter;

[0162] Step 5.5: Quaternion update

[0163] In this embodiment, in the said Step 5.1, initialize the error state value and the bias error of the gyroscope:

[0164]

[0165] In this embodiment, in the said Step 5.2, the steps of the nominal state update of the error Kalman filter are as follows:

[0166] The rotation vector for each sampling interval is:

[0167] δa = [(ω t + ω t+1 ) / 2 - b g · δT

[0168] Nominal state quaternion update:

[0169]

[0170] Zero bias update in the nominal state:

[0171]

[0172] In this embodiment, in the said Step 5.3, the steps of the error state update of the error Kalman filter are as follows:

[0173] The error state transition model is:

[0174] δx k = φ k,k-1 δx k-1

[0175]

[0176] Update of the prior covariance matrix:

[0177]

[0178] In the formula, P k-1 is the covariance of the previous moment, is the current prior covariance, and Q k is the system process noise matrix.

[0179] In this embodiment, in the said Step 5.4, the steps of the update of the error Kalman filter are as follows:

[0180] Update the system measurement matrix to:

[0181]

[0182]

[0183] Error Kalman filter gain update:

[0184]

[0185] R k = E(v k v k T ) = R a + R z

[0186] R z = c 2 ||f - g b || 2

[0187] Where f is the acceleration of the IMU array; R a is the covariance matrix of the accelerometer measurement noise; R z is the adaptively adjusted covariance matrix; c is the adjustable parameter of the system.

[0188] Measurement update error state:

[0189]

[0190] Posterior covariance matrix update:

[0191]

[0192] In this embodiment, in step 5.5, the updated value of the error quaternion is obtained, and the updated true quaternion value is obtained from Obtain the updated true quaternion value.

[0193] In this embodiment, in step 6, the attitude angle is updated as follows:

[0194] The coordinate transformation matrix from the body coordinate system to the navigation coordinate system can be represented by the quaternion method or by the coordinate system rotation method. The matrices represented by the two are equal. Therefore, the corresponding relationship between the attitude angle and the quaternion is:

[0195] θ = arcsin(2(q2q3 + q0q1))

[0196]

[0197] The systems, devices, modules or units illustrated in the above embodiments can be specifically implemented by computer chips or by products with certain functions.

[0198] It should also be noted that the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, commodity or device comprising a series of elements not only includes those elements but also includes other elements not explicitly listed, or further includes elements inherent to such process, method, commodity or device. Without further limitations, an element defined by the statement "comprising an..." does not exclude the existence of additional identical elements in the process, method, commodity or device comprising the said element.

[0199] These above embodiments should be understood as only illustrative of the present invention and not restrictive of the scope of protection of the present invention. After reading the content recorded in the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A method for attitude calculation in a high-dynamic environment based on a heterogeneous MEMS IMU array, characterized in that, It includes the following steps: Step 1: Fuse the angular velocity and acceleration data of four pairs of differential IMU pairs; Step 2: Fuse the IMU array data to output the system angular velocity and acceleration.

2. The attitude solution method in a high-dynamic environment based on a non-coplanar MEMS IMU array according to claim 1, characterized in that, In the above Step 1, the IMU installation coordinate system is the right front upper coordinate system; the inertial system navigation coordinate system is the northeast sky coordinate system. Fusing the angular velocity and acceleration data of four pairs of differential IMU pairs specifically includes: The outputs of the Z axes of the two gyroscopes of the differential IMU pair are: where ω z represents the true angular rate of the Z-axis; S z1 and S z2 and N z1 and N z2 and b z1 and b z2 and ε z1 and ε z2 represent the measured values of the front sensor and the rear sensor, scale factor error, cross-coupling error, bias drift, and random noise, respectively. Therefore, the angular velocity fusion output of the differential IMU pair can be calculated by the following formula: Similarly, the outputs of the X, Y axes of the differential pair IMU gyroscopes and the X, Y, Z axes of the accelerometers are: In the formula, respectively represent the angular velocities of the IMU differential pair about the Y and Z axes; respectively represent the accelerations of the IMU differential pair about the X, Y, and Z axes; the subscript F represents the front sensor; the subscript B represents the rear sensor.

3. A method for attitude calculation in a high-dynamic environment based on a skew MEMS IMU array according to claim 1, characterized in that, In the above Step 2, fusing the IMU array data specifically includes: The measurement equation of the gyroscope array is: ω g =(D T wD) -1 Dwm In the formula, is the angular velocity in the system framework; ω g is the angular velocity output by gyroscope fusion at time t; is the measurement of 4 sensor pairs; D is called the sensor geometric measurement matrix, which is determined according to the placement position of the IMU; w is the weighting matrix; v i is the measurement error. The calculation steps of the array angular acceleration are calculated by the following method: The specific force output by each accelerometer is synthesized by the specific force at the geometric center of the array, the centripetal force, and the rotational torque: where f i is the true output of the i-th accelerometer; is the angular acceleration; f o is the acceleration of the array geometric center; r i is the position vector of the i-th accelerometer relative to the array geometric center. Subtracting the outputs of the two accelerometers can cancel the central acceleration in the measurement model: Adjust the angular acceleration term to the other side of the equation by transposing: In the formula, r Δij is the difference between the position vectors of accelerometers numbered i and j in the array relative to the geometric center of the array. Since the matrix on the left side of the equation is a singular matrix, the angular acceleration cannot be solved. It is necessary to introduce another set of lever arm acceleration differences and solve the two sets of equations in parallel to obtain: Let: From the above formula, the angular acceleration can be obtained by the least squares method as: The calculation method for the accelerometer array to deduce the angular velocity is as follows: where ω a is the angular velocity calculated by the leverage effect at time t; δT is the sampling interval time; ω a-T is the angular velocity calculated by the leverage effect at the previous moment t - δT; is the angular acceleration calculated by the leverage effect at time t; When the system angular velocity is less than the set value, directly adopt the output of the gyroscope array as the system angular velocity; when the angular velocity is greater than the set value, by fusing the output of the gyroscope array and the angular velocity calculated based on the accelerometer lever effect formula to obtain the system angular velocity; when the angular velocity is greater than the set threshold, use the angular velocity calculated based on the accelerometer lever effect formula as the system angular velocity. In the formula, is the angular velocity of the IMU array system; K is the adjustable parameter of the angular velocity complementary filter; FSR is the range of the gyroscope; is the angular velocity at the previous moment; The acceleration of the i-th pair of IMU converted to the geometric center of the system is: By converting the acceleration data of the four pairs of IMUs to the geometric center and averaging them, the final acceleration of the system can be obtained.

Images