A multi-source data fusion MEMS attitude calibration method

Abstract

The application provides a multi-source data fusion MEMS attitude calibration method, which fully utilizes MEMS gyroscopes, accelerometers and external azimuth attitude information to realize system azimuth attitude calibration. The method realizes long-time precision maintenance of the MEMS attitude in the air and on the ground through software control without increasing any hardware cost and changing the overall framework of the MEMS attitude product, and improves the application scene of the MEMS attitude product.

Description

Technical Field

[0001] The invention belongs to the technical field of MEMS heading attitude calibration, and in particular relates to a MEMS heading attitude calibration method based on multi-source data fusion. Background Art

[0002] As the primary source of aircraft attitude information, the MEMS attitude system provides information such as the aircraft's azimuth, pitch, and bank angles. In battlefield environments, for example, an aircraft may need to maintain azimuth and attitude without satellite navigation information. In these situations, traditional Kalman filter-based navigation methods are ineffective.

[0003] MEMS gyroscopes and accelerometers are the core components of MEMS attitude and heading. Their performance directly affects the performance of MEMS attitude and heading. However, due to the limitations of the micro-electromechanical manufacturing process of domestic MEMS sensors, the performance of MEMS sensors is currently far behind that of fiber optic sensors. Therefore, in the absence of external satellite navigation information, MEMS sensors are currently unable to maintain long-term accuracy. Summary of the Invention

[0004] To address the existing problem of MEMS attitude and heading accuracy remaining unresolved, the present invention provides a multi-source data fusion MEMS attitude and heading calibration method. This method leverages MEMS gyroscopes, gageometers, and external attitude and heading information to achieve system attitude and heading calibration. This method, without increasing hardware costs or changing the overall MEMS attitude and heading product architecture, maintains long-term MEMS attitude and heading accuracy in both airborne and ground conditions through software control, meeting the requirements for MEMS attitude and heading products in multiple application scenarios.

[0005] The technical solution of the present invention is: a MEMS attitude calibration method based on multi-source data fusion, including two parts: azimuth fusion correction and attitude height fusion correction. The azimuth fusion correction includes the following steps:

[0006] Step 1: Receive external azimuth, attitude data and magnetic heading data in real time;

[0007] Step 2: When external azimuth and attitude data are received and the data is valid, the external azimuth data fusion processing flag and the external attitude data fusion processing flag are updated; when magnetic heading data is received and the data is valid, the magnetic heading data fusion processing flag is updated;

[0008] Step 3: Determine whether the external azimuth data fusion processing flag is valid. If the flag is invalid, proceed to step 4; if the flag is valid, proceed to step 5;

[0009] Step four: judge whether the magnetic heading data fusion processing flag is valid, if the flag is invalid, return to step one, if the flag is valid, execute step thirteen;

[0010] Step five: according to the received external azimuth and attitude information, establish the quaternion in the master inertial coordinate system;

[0011] Step six: according to the quaternion information established in step five, establish the attitude matrix in the master inertial coordinate system;

[0012] Step seven: according to the actual installation error value of the master inertial and the attitude and the attitude matrix information established in step six in the master inertial coordinate system, realize the attitude matrix conversion from the master inertial coordinate system to the attitude coordinate system, and compensate the installation error between the MEMS attitude and the master inertial;

[0013] Step eight: according to the attitude matrix calculated in step seven, calculate the converted external azimuth ψ 外 ;

[0014] Step nine: according to the external azimuth ψ 外 calculated in step eight, combine the azimuth ψ 主 calculated by the system itself, and calculate the difference value according to formula (8), if the absolute value of Δψ is greater than zero, execute step ten, otherwise return to step one;

[0015] Δψ=ψ 主 -ψ 外 (8)

[0016] Step ten: the counter starts to count up;

[0017] Step eleven: judge whether the count value reaches the reference threshold, if yes, execute step twelve, otherwise return to step one;

[0018] Step twelve: if the external azimuth ψ 外 in step nine and the azimuth ψ 主 calculated by the system itself are within the set range, and the Δψ calculated in step nine is greater than the reference threshold, execute step twenty-two, otherwise return to step one;

[0019] Step thirteen: calculate the system azimuth 1s increment, wherein, the initial value is zero;

[0020]

[0021] Step fourteen: calculate the magnetic heading 1s increment, wherein, ψ MHDG0 the initial value is zero;

[0022] Δψ MHDG =ψ MHDG -ψ MHDG0(10)

[0023] Step fifteen: according to the system azimuth angle increment calculated in step thirteen and the magnetic heading increment calculated in step fourteen, the difference between the magnetic heading rate of change and the system azimuth angle rate of change is calculated according to formula (11);

[0024] Δψ1= Δψ MHDG - Δψ HDG (11)

[0025] Step sixteen: the current system azimuth angle and magnetic heading information are saved for use in the next 1s calibration according to formula (12) and formula (13);

[0026] ψ HDG0 = ψ 主 (12)

[0027] ψ MHDG0 = ψ MHDG (13)

[0028] Step seventeen: it is judged whether the magnetic heading fusion processing condition is met, if yes, step eighteen is executed, otherwise, step one is returned, wherein the magnetic heading fusion processing condition is that the difference between the azimuth angle rate of change and the magnetic heading angle rate of change, the system attitude angle, the three-axis angular velocity and the three-axis body acceleration are all less than the set threshold value;

[0029] Step eighteen: the counter starts to count up;

[0030] Step nineteen: it is judged whether the count value reaches the reference threshold value, if yes, step twenty is executed, otherwise, step one is returned;

[0031] Step twenty: the magnetic heading and azimuth angle error is calculated according to formula (14);

[0032] Δψ = ψ MHDG - ψ 主 (14)

[0033] Step twenty-one: the error between the magnetic heading and the system azimuth angle calculated in step twenty is converted to between ±180°;

[0034] Step twenty-two: the system azimuth angle is calibrated according to formula (15); wherein λ is a calibration parameter;

[0035] ψ 主 = ψ 主 - λ Δψ (15)

[0036] Step twenty-three: the system quaternion of the MEMS attitude and heading is established by using the calibrated system azimuth angle and the attitude angle;

[0037] Step twenty-four: according to the system quaternion of the MEMS attitude established in step twenty-three, a system attitude matrix of the MEMS attitude is established, and after the step is finished, the azimuth fusion mechanism processing flow is finished;

[0038] The attitude height fusion correction comprises the following steps:

[0039] Step 1: real-time receiving external azimuth, attitude data, height and acceleration data;

[0040] Step 2: when the attitude data is received and the data is valid, the external attitude data fusion processing flag is updated; when the external height data is received and the data is valid, the height valid flag is updated;

[0041] Step 3: the height lock flag system is defaulted as FALSE, and it is judged whether the height lock flag is FALSE, if yes, step 4 is executed, otherwise step 6 is executed;

[0042] Step 4: judging the height valid flag, if the height valid flag is valid, step 5 is executed, otherwise step 6 is executed;

[0043] Step 5: the system height is initialized, and the height lock flag value is set as TRUE, steps 3 to 5 together realize the height lock, and only in the system once, that is, the storage of the initial height value of the system is completed;

[0044] Step 6: judging whether the acceleration fusion processing condition meets the requirement, if yes, step 8 is executed, if not, step 7 is executed; wherein the acceleration fusion processing condition is that the three-axis angular rate and the current 1s combined acceleration are within the reference threshold;

[0045] Step 7: judging whether the external attitude fusion processing flag is valid, if yes, step 14 is executed, if not, returning to step 1;

[0046] Step 8: according to the acceleration information 基 and the gravity acceleration information g, the reference pitch angle θ 基 and the reference roll angle γ are calculated;

[0047]

[0048]

[0049] Step 9: judging whether the reference angle values calculated in step 8 meet the reference threshold range of the accelerometer fusion correction, if yes, step 10 is executed, otherwise returning to step 1;

[0050] Step 10: according to the reference attitude calculated in step 8, combining the attitude information θ主 and γ 主 , calculate the attitude error value:

[0051] Δθ = θ 基 - θ 主 (21)

[0052] Δγ = γ 基 - γ 主 (22)

[0053] Step 11: Determine whether the attitude error value calculated in step 10 is greater than the calibration amount per beat. If it is greater than the calibration amount per beat, execute step 12, otherwise return to step 1;

[0054] Step 12: Determine the number of positive and negative direction calibrations according to the attitude error value calculated in step 10;

[0055] Step 13: Determine whether the number of positive and negative direction calibrations determined in step 12 meets the reference threshold. If it does, execute step 27, otherwise return to step 1;

[0056] Step 14: The ground mode calibration flag system is set to FALSE by default. Determine whether the ground mode calibration flag system is FALSE. If it is, execute step 15, otherwise return to step 1;

[0057] Step 15: Calculate the height difference value according to the system initial height value h 初 locked in step 5, combined with the system height value h 主 received in real time. Determine whether the height difference value is greater than the reference threshold. If it is greater, execute step 16, otherwise return to step 1;

[0058] Δh = h 主 - h 初 (23)

[0059] Step 16: Set the ground calibration flag to TRUE;

[0060] Step 17: Determine whether the ground calibration flag is set to TRUE. If it is TRUE, execute step 18, otherwise return to step 1;

[0061] Step 18: Lock the received external orientation and attitude information;

[0062] Step 19: Establish the quaternion in the master inertial coordinate system according to the external orientation and attitude information locked in step 18;

[0063] Step 20: Establish the attitude matrix in the master inertial coordinate system according to the quaternion information established in step 19;

[0064] Step 21: According to the actual installation error value of the master inertial and the attitude and the attitude matrix information in the master inertial coordinate system, the attitude matrix conversion from the master inertial coordinate system to the attitude and position coordinate system is realized, and the installation error between the MEMS attitude and position and the master inertial is compensated.

[0065] Step 22: According to the attitude matrix calculated in step 21 , the converted external attitude angle is calculated.

[0066] Step 23: According to the external attitude angle calculated in step 22, combined with the attitude angle θ 主 , γ 主 calculated by the system itself, the difference value is calculated according to formulas (26) and (27):

[0067] Δθ=θ 主 -θ 外 (26)

[0068] Δγ=γ 主 -γ 外 (27)

[0069] Step 24: The counter starts to count up;

[0070] Step 25: If the count value meets the reference threshold, step 26 is executed, otherwise, step 1 is returned;

[0071] Step 26: If the external attitude angle in step 23 and the attitude angle calculated by the system itself are within the set range, and Δθ and Δγ calculated in step 23 are greater than the reference threshold, step 27 is executed, otherwise, step 1 is returned.

[0072] Step 27: The system attitude angle is calibrated according to formulas (28) and (29); wherein μ and κ are calibration parameters:

[0073] θ 主 =θ 主 -μΔθ (28)

[0074] γ 主 =γ 主 -κΔγ (29)

[0075] Step 28: According to the calibrated system attitude angle and the system azimuth angle obtained in step 27, the system quaternion of the MEMS attitude and position is established.

[0076] Step 29: The system attitude matrix of the MEMS attitude and position is established according to the system quaternion obtained in step 28; after this step, the attitude fusion mechanism processing flow is ended.

[0077] Further, in step five of the azimuth fusion correction and step 19 of the attitude and height fusion correction, the formula for establishing the quaternion in the master inertial coordinate system is:

[0078]

[0079] where ψ1, γ1 is the received external azimuth and attitude information.

[0080] Further, in step six of the azimuth fusion correction and step 20 of the attitude and height fusion correction, the formula for establishing the attitude matrix is:

[0081]

[0082]

[0083] Further, in step seven of the azimuth fusion correction and step 21 of the attitude and height fusion correction, the attitude matrix conversion from the master inertial coordinate system to the navigation and attitude coordinate system is implemented according to formulas (4) and (5):

[0084]

[0085]

[0086] where p0 is the pitch installation error value.

[0087] Further, in step 23 of the azimuth fusion correction and step 28 of the attitude and height fusion correction, the formula for establishing the system quaternion of the MEMS navigation and attitude is:

[0088]

[0089] where ψ 主 , γ 主 is the system azimuth and attitude angle.

[0090] Further, in step 24 of the azimuth fusion correction and step 29 of the attitude and height fusion correction, the formula for establishing the system attitude matrix of the MEMS navigation and attitude is:

[0091]

[0092]

[0093] Further, in step eight of the azimuth fusion correction, the formula for calculating the converted external azimuth is:

[0094]

[0095]

[0096] Further, in the step 22 of the attitude height fusion correction, the external attitude angle calculation formula is:

[0097]

[0098] θ 外 = sin -1 (T 32 ) (24)

[0099]

[0100] Inventive effects

[0101] The technical effects of the present application are that the present application makes full use of MEMS gyroscopes, accelerometers, external azimuth attitude and height information, designs corresponding control mechanisms, uses height information to determine the different states of the product on the ground and in the air, and can determine different calibration methods according to different situations, thereby realizing the long-time precision maintenance of MEMS attitude. The precision maintenance of the MEMS attitude in the absence of external satellite navigation information is solved. BRIEF DESCRIPTION OF DRAWINGS

[0102] Fig. 1 It is a general design architecture diagram

[0103] Fig. 2 It is a heading fusion processing flowchart

[0104] Fig. 3 It is an attitude fusion processing flowchart DETAILED DESCRIPTION

[0105] Referring to Figs. 1-3 , the present application mainly includes two functional modules: azimuth fusion processing module and attitude fusion processing module. The principle of the azimuth fusion processing module is to calibrate the azimuth angle in real time according to the magnetic heading information and external azimuth information; the principle of the attitude fusion processing module is to calibrate the attitude data in real time according to the MEMS accelerometer data, external attitude data and height information.

[0106] The processing flow of the above two modules will be described in detail below.

[0107] I. Azimuth angle fusion mechanism processing: realize the fusion processing of external azimuth angle, magnetic heading and system azimuth angle, including the following steps:

[0108] Step 1: Real-time receive external azimuth angle, attitude data and magnetic heading data;

[0109] Step two: when receiving the external azimuth, attitude data and the data is valid, update the external azimuth data fusion processing flag and the external attitude data fusion processing flag; when receiving the magnetic heading data and the data is valid, update the magnetic heading data fusion processing flag;

[0110] Step three: judge whether the external azimuth data fusion processing flag is valid, if the flag is invalid, execute step four; if the flag is valid, execute step five-step twelve and step twenty-two-step twenty-four;

[0111] Step four: judge whether the magnetic heading data fusion processing flag is valid, if the flag is invalid, return to step one, if the flag is valid, execute step thirteen-step twenty-one and step twenty-two-step twenty-four;

[0112] Step five: according to the received external azimuth and attitude information, establish the quaternion in the master inertial coordinate system by formula (1), wherein ψ1、 γ1 is the received external azimuth and attitude information;

[0113]

[0114] Step six: according to the quaternion information established in step five, establish the attitude matrix in the master inertial coordinate system by formula (2), (3);

[0115]

[0116]

[0117] Step seven: according to the actual installation error value of the master inertial and the attitude and the attitude matrix information in the master inertial coordinate system established in step six, realize the attitude matrix conversion from the master inertial coordinate system to the attitude coordinate system according to formula (4), (5), and compensate the installation error between the MEMS attitude and the master inertial, wherein p0 is the installation error value;

[0118]

[0119]

[0120] Step eight: according to the attitude matrix calculated in step seven, calculate the converted external azimuth ψ 外 according to formula (6), formula (7);

[0121]

[0122]

[0123] Step nine: according to the external azimuth ψ外 , the azimuth angle ψ 主 , the difference is calculated according to formula (8), if the absolute value of Δψ is greater than zero, step ten is executed, otherwise, step one is returned;

[0124] Δψ = ψ 主 - ψ 外 (8)

[0125] Step ten: the counter starts to count up;

[0126] Step eleven: it is judged whether the count value reaches the reference threshold, if yes, step twelve is executed, otherwise, step one is returned;

[0127] Step twelve: if the external azimuth angle ψ 外 and the azimuth angle ψ 主 calculated by the system itself in step nine are within the set range, and Δψ calculated in step nine is greater than the reference threshold, step twenty-two is executed, otherwise, step one is returned;

[0128] Step thirteen: the system azimuth angle 1s increment is calculated according to formula (9), wherein, ψ HDG0 is zero initially;

[0129] Δψ HDG = ψ 主 - ψ HDG0 (9)

[0130] Step fourteen: the magnetic heading 1s increment is calculated according to formula (10), wherein, ψ MHDG0 is zero initially;

[0131] Δψ MHDG = ψ MHDG - ψ MHDG0 (10)

[0132] Step fifteen: according to the system azimuth angle increment calculated in step thirteen and the magnetic heading increment calculated in step fourteen, the difference between the magnetic heading change rate and the system azimuth angle change rate is calculated according to formula (11);

[0133] Δψ1 = Δψ MHDG - Δψ HDG (11)

[0134] Step sixteen: the current system azimuth angle and magnetic heading information are saved for the next 1s calibration according to formula (12) and formula (13);

[0135] ψ HDG0 = ψ 主 (12)

[0136] ψ MHDG0 = ψMHDG (13)

[0137] Step seventeen: judge whether the magnetic heading fusion processing condition is met, if yes, execute step eighteen, otherwise return to step one, wherein the magnetic heading fusion processing condition is that the difference between the azimuth rate of change and the magnetic heading angle rate of change, the system attitude angle, the three-axis angular rate and the three-axis body acceleration are all less than the set threshold value;

[0138] Step eighteen: the counter starts to count up;

[0139] Step nineteen: judge whether the count value reaches the reference threshold value, if yes, execute step twenty, otherwise return to step one;

[0140] Step twenty: calculate the magnetic heading and system azimuth angle error according to formula (14);

[0141] Δψ=ψ MHDG -ψ 主 (14)

[0142] Step twenty-one: convert the error between the magnetic heading and the system azimuth angle calculated in step twenty to between ±180°;

[0143] Step twenty-two: calibrate the system azimuth angle according to formula (15); wherein λ is the calibration parameter.

[0144] ψ 主 =ψ 主 -λΔψ (15)

[0145] Step twenty-three: use the calibrated system azimuth angle and the attitude angle ψ 主 、 γ 主 to establish the system quaternion of the MEMS attitude according to formula (16);

[0146]

[0147] Step twenty-four: establish the system attitude matrix of the MEMS attitude according to formula (17) and (18). After this step, the azimuth fusion mechanism processing flow ends.

[0148]

[0149]

[0150] II. Attitude fusion processing mechanism: to realize the fusion processing of the external attitude, height, acceleration data and the system attitude information. The processing flow includes the following steps:

[0151] Step one: real-time receive the external azimuth angle, attitude data, height and acceleration data;

[0152] Step two: update the external attitude data fusion processing flag when receiving the attitude data and the data is valid; update the height valid flag when receiving the external height data and the data is valid;

[0153] Step three: the height lock flag system is defaulted to FALSE; determine whether the height lock flag is FALSE, if yes, execute step four, otherwise execute step six;

[0154] Step four: determine the height valid flag, if the height valid flag is valid, execute step five, otherwise execute step six;

[0155] Step five: assign the initial value to the system height and set the height lock flag value to TRUE, steps three to five collectively realize the height lock, and only in the system once, that is, complete the storage of the initial height value of the system;

[0156] Step six: determine whether the acceleration fusion processing condition meets the requirements, if yes, execute steps eight-thirteen and twenty-seven-twenty-nine; if not, execute step seven; wherein the acceleration fusion processing condition is that the three-axis angular rate and the current 1s combined acceleration are within the reference threshold;

[0157] Step seven: determine whether the external attitude fusion processing flag is valid, if yes, execute steps fourteen-twenty-six and twenty-seven-twenty-nine, if not, return to step one;

[0158] Step eight: calculate the reference pitch angle θ 基 according to formula (19) based on the acceleration information and the gravity acceleration information g 基 ;

[0159]

[0160]

[0161] Step nine: determine whether the reference angle values calculated in step eight meet the reference threshold range of the accelerometer fusion correction, if yes, execute step ten, otherwise return to step one;

[0162] Step ten: calculate the attitude error values according to formulas (21), (22) based on the reference attitude calculated in step eight, combined with the attitude information θ 主 and γ 主 calculated by the system itself:

[0163] Δθ = θ 基 - θ 主 (21)

[0164] Δγ = γ 基 -γ 主 (22)

[0165] Step eleven: determine whether the attitude error value calculated in step ten is greater than the calibration amount per beat, if greater than the calibration amount per beat, execute step twelve, otherwise return to step one;

[0166] Step twelve: determine the number of positive and negative direction calibrations according to the attitude error value calculated in step ten;

[0167] Step thirteen: determine whether the number of positive and negative direction calibrations determined in step twelve meets the reference threshold, if it meets, execute step twenty-seven, otherwise return to step one;

[0168] Step fourteen: the ground mode calibration flag system is defaulted to FALSE, determine whether the ground mode calibration flag system is FALSE, if it is, execute step fifteen, otherwise return to step one;

[0169] Step fifteen: calculate the height difference value according to formula (23) based on the system initial height value h 初 locked in step five combined with the system height value h 主 received in real time, determine whether the height difference value is greater than the reference threshold, if it is greater, execute step sixteen, otherwise return to step one;

[0170] Δh = |h 主 -h 初 | (23)

[0171] Step sixteen: set the ground calibration flag to TRUE;

[0172] Step seventeen: determine whether the ground calibration flag is TRUE, if it is TRUE, execute step eighteen, otherwise return to step one;

[0173] Step eighteen: lock the received external orientation and attitude information;

[0174] Step nineteen: establish the quaternion in the master inertial coordinate system using formula (1) based on the external orientation and attitude information locked in step eighteen, wherein ψ1、 γ1is the received external orientation angle and attitude information;

[0175]

[0176] Step twenty: establish the attitude matrix in the master inertial coordinate system using formula (2) and (3) based on the quaternion information established in step nineteen;

[0177]

[0178]

[0179] Step twenty-one: according to the actual installation error value of the master inertial and the attitude and the attitude matrix information under the master inertial coordinate system, the attitude matrix conversion under the master inertial coordinate system to the attitude coordinate system is realized according to formula (4), (5), and the installation error between the MEMS attitude and the master inertial is compensated, wherein p0 is the installation error value;

[0180]

[0181]

[0182] Step twenty-two: according to the attitude matrix calculated in step twenty-one The converted external attitude angle is calculated according to formula (6), formula (24), (25);

[0183]

[0184] θ 外 = sin -1 (T 32 ) (24)

[0185]

[0186] Step twenty-three: according to the external attitude angle calculated in step twenty-two, combined with the attitude angle θ 主 , γ 主 calculated by the system itself, the difference value is calculated according to formula (26), (27);

[0187] Δθ=θ 主 -θ 外 (26)

[0188] Δγ=γ 主 -γ 外 (27)

[0189] Step twenty-four: the counter starts to count up;

[0190] Step twenty-five: if the count value meets the reference threshold value, step twenty-six is executed, otherwise step one is returned;

[0191] Step twenty-six: if the external attitude angle in step twenty-three and the attitude angle calculated by the system itself are within the set range, and Δθ, Δγ calculated in step twenty-three are greater than the reference threshold value, step twenty-seven is executed, otherwise step one is returned;

[0192] Step twenty-seven: the system attitude angle is calibrated according to formula (28), (29); wherein μ, κ are calibration parameters.

[0193] θ 主 = θ 主 - μΔθ (28)

[0194] γ 主 = γ 主 - κΔγ (29)

[0195] Step twenty-eight: according to the calibrated system attitude angle and the system azimuth angle obtained in step twenty-seven, the system quaternion of the MEMS attitude is established according to formula (16);

[0196]

[0197] Step twenty-nine: the system attitude matrix of the MEMS attitude is established according to formula (17) and (18). After this step, the attitude fusion mechanism processing flow is ended.

[0198]

[0199]

Claims

1. A MEMS attitude calibration method based on multi-source data fusion, characterized in that: Including azimuth fusion correction and attitude The two parts of the state are highly integrated and revised. Azimuth fusion correction includes the following steps: Step 1: Receive external azimuth, attitude data and magnetic heading data in real time; Step 2: When external azimuth and attitude data are received and the data is valid, the external azimuth data fusion processing flag and the external attitude data fusion processing flag are updated; when magnetic heading data is received and the data is valid, the magnetic heading data fusion processing flag is updated; Step 3: Determine whether the external azimuth data fusion processing flag is valid. If the flag is invalid, proceed to step 4; if the flag is valid, proceed to step 5; Step 4: Determine whether the magnetic heading data fusion processing flag is valid. If the flag is invalid, return to step 1. If the flag is valid, execute step 13. Step 5: Based on the received external azimuth and attitude information, establish the quaternion in the principal inertial coordinate system; Step 6: Based on the quaternion information established in step 5, establish the attitude matrix in the principal inertial coordinate system; Step 7: Based on the actual installation error between the master inertial coordinate system and the attitude and heading coordinate system and the attitude matrix information in the master inertial coordinate system established in step 6, the attitude matrix conversion between the master inertial coordinate system and the attitude and heading coordinate system is realized to compensate for the installation error between the MEMS attitude and heading coordinate system. Step 8: Calculate the converted external orientation angle based on the attitude matrix calculated in step 7 ; Step 9: External azimuth calculated according to step 8 , combined with the azimuth angle calculated by the system itself , calculate the difference according to formula (8), if If the absolute value is greater than zero, execute step 10, otherwise return to step 1; （8） Step 10: The counter starts to count up; Step 11: Determine whether the count value reaches the reference threshold, if so, proceed to step 12, otherwise return to step 1; Step 12: If the external azimuth in step 9 And the azimuth calculated by the system itself Within the set range, and the value calculated in step nine If the value is greater than the reference threshold, then execute step 22, otherwise return to step 1; Step 13: Calculate the 1s increment of the system azimuth, where: The initial value is zero; （9） Step 14: Calculate the 1s increment of magnetic heading, where: Initial value is zero （10） Step 15: Based on the system azimuth increment calculated in step 13 and the magnetic heading increment calculated in step 14, calculate the difference between the magnetic heading change rate and the system azimuth change rate according to formula (11); （11） Step 16: Save the current system azimuth and magnetic heading information according to formula (12) and formula (13) for use in the next 1s calibration; （12） = （13） Step 17: Determine whether the magnetic heading fusion processing conditions are met. If so, proceed to step 18; otherwise, return to step 1. The magnetic heading fusion processing conditions are: the difference between the azimuth angle change rate and the magnetic heading angle change rate, the system attitude angle, the three-axis angular rate, and the three-axis body acceleration are all less than the set threshold value; Step 18: The counter starts to count up; Step 19: Determine whether the count value reaches the reference threshold, then execute step 20, otherwise return to step 1; Step 20: Calculate the magnetic heading and azimuth errors according to formula (14); （14） Step 21: Convert the error between the magnetic heading calculated in step 20 and the system azimuth to within ±180°; Step 22: Calibrate the system azimuth according to formula (15); where is the calibration parameter; （15） Step 23: Use the calibrated system azimuth and attitude angles to create the system quaternion of the MEMS heading attitude. Step 24: Based on the system quaternion of the MEMS attitude established in step 23, the system attitude matrix of the MEMS attitude is established. After this step, the azimuth fusion mechanism processing flow is completed; the attitude altitude fusion correction includes the following steps: Step 1: Receive external azimuth, attitude data, altitude and acceleration data in real time; Step 2: When the attitude data is received and the data is valid, the external attitude data fusion processing flag is updated; when the external height data is received and the data is valid, the height valid flag is updated; Step 3: The altitude lock flag system defaults to FALSE. Determine whether the altitude lock flag is FALSE. If so, go to step 4; otherwise, go to step 6. Step 4: Determine the height validity flag. If the height validity flag is valid, go to step 5; otherwise, go to step 6. Step 5: Assign an initial value to the system altitude and set the altitude lock flag value to TRUE. Steps 3 to 5 together implement altitude lock and are only performed once in the system to complete the storage of the system's initial altitude value. Step 6: Determine whether the acceleration fusion processing conditions meet the requirements. If so, proceed to step 8; if not, proceed to step 7. The acceleration fusion processing conditions are that the three-axis angular rate and the current 1s combined acceleration are within the reference threshold. Step 7: Determine whether the external posture fusion processing flag is valid. If valid, execute step 14; if invalid, return to step 1; Step 8: Based on the acceleration information 、 、 And gravitational acceleration information g, calculate the reference pitch angle and the reference roll angle ; （19） （20） Step 9: Determine whether the reference angle values ​​calculated in step 8 all meet the reference threshold range of the accelerometer fusion correction. If so, execute step 10; otherwise, return to step 1. Step 10: Based on the reference posture calculated in step 8, combined with the posture information solved by the system itself and , calculate the attitude error value: （21） （22） Step 11: Determine whether the posture error value calculated in step 10 is greater than the calibration amount per beat. If so, execute step 12; otherwise, return to step 1. Step 12: Determine the number of positive and negative calibrations based on the attitude error value calculated in step 10; Step 13: Based on the positive and negative direction calibration times determined in step 12, determine whether the positive and negative direction calibration times meet the reference threshold. If so, proceed to step 27; otherwise, return to step 1. Step 14: The ground mode calibration flag system defaults to FALSE. Determine whether the ground mode calibration flag system is FALSE. If so, execute step 15. Otherwise, return to step 1. Step 15: Lock the system's initial altitude value according to step 5 , combined with the system altitude value received in real time , calculate the height difference, and determine whether the height difference is greater than the reference threshold. If so, execute step 16, otherwise return to step 1; （23） Step 16: Set the ground calibration flag to TRUE; Step 17: Determine whether the ground calibration flag is set to TRUE. If it is TRUE, execute step 18, otherwise return to step 1; Step 18: Lock the received external position and attitude information; Step 19: Create the quaternion in the principal inertial coordinate system based on the external orientation and attitude information locked in step 18; Step 20: Based on the quaternion information created in step 19, establish the attitude matrix in the principal inertial coordinate system; Step 21: Based on the actual installation error between the master inertial coordinate system and the heading attitude and the attitude matrix information in the master inertial coordinate system, the attitude matrix conversion from the master inertial coordinate system to the heading attitude coordinate system is realized to compensate for the installation error between the MEMS heading attitude and the master inertial coordinate system. Step 22: Based on the posture matrix calculated in step 21 , calculate the external attitude angle after conversion; Step 23: Based on the external attitude angle calculated in step 22, combined with the attitude angle calculated by the system itself 、 , calculate the difference according to formula (26) and (27): （26） （27） Step 24: The counter starts to count up; Step 25: If the count value meets the reference threshold, execute step 26; otherwise, return to step 1; Step 26: If the external attitude angle in step 23 and the attitude angle calculated by the system itself are both within the set range, and the attitude angle calculated in step 23 is 、 If the value is greater than the reference threshold, execute step 27, otherwise return to step 1; Step 27: Calibrate the system attitude angle according to formulas (28) and (29); 、 are the calibration parameters: （28） （29） Step 28: Based on the calibrated system attitude angle and system azimuth angle obtained in step 27, establish the system quaternion of the MEMS heading; Step 29: Establish the system attitude matrix of MEMS heading according to the system quaternion obtained in step 28; after this step, the attitude fusion mechanism processing flow ends.

2. A MEMS attitude calibration method based on multi-source data fusion as claimed in claim 1, characterized in that: In step 5 of the azimuth fusion correction and step 19 of the attitude and altitude fusion correction, the formula for establishing the quaternion in the principal inertial coordinate system is: in is the received external azimuth and attitude information.

3. A MEMS attitude calibration method based on multi-source data fusion as claimed in claim 2, characterized in that: In step 6 of the azimuth fusion correction and step 20 of the attitude height fusion correction, the formula for establishing the attitude matrix is: 。 4. The MEMS attitude calibration method based on multi-source data fusion according to claim 1, characterized in that: In step 7 of the azimuth fusion correction and step 21 of the attitude altitude fusion correction, the attitude matrix conversion from the principal inertial coordinate system to the heading attitude coordinate system is realized according to formulas (4) and (5): （4） （5） in, Pitch installation error value.

5. The MEMS attitude calibration method based on multi-source data fusion as claimed in claim 1, characterized in that: In step 23 of the azimuth fusion correction and step 28 of the attitude altitude fusion correction, the system quaternion formula for establishing the MEMS heading attitude is: in are the system azimuth and attitude angles.

6. The MEMS attitude calibration method based on multi-source data fusion as claimed in claim 1, characterized in that: In step 24 of the azimuth fusion correction and step 29 of the attitude altitude fusion correction, the system attitude matrix of the MEMS attitude is established as follows: 。 7. The MEMS attitude calibration method based on multi-source data fusion according to claim 1, characterized in that: In step eight of the azimuth fusion correction, the formula for calculating the converted external azimuth is: 。 8. The MEMS attitude calibration method based on multi-source data fusion as claimed in claim 1, characterized in that: In step 22 of the posture height fusion correction, the external posture angle calculation formula is: （6） （24） （25）。

Images