A heading angle real-time fusion method and system

Abstract

This application discloses a real-time heading angle fusion method and system. It acquires output data from a MEMS inertial navigation system and a dual-antenna satellite receiver in real time, updates the heading angle storage sequence and the Z-axis angular velocity storage sequence in real time, performs comprehensive information processing and data fusion, corrects the heading angle of the MEMS inertial navigation system, and updates the values ​​in the heading angle storage sequence. In this application, the MEMS inertial navigation system provides accurate relative angle changes in a short time, resulting in a high output frequency for the fused heading angle. The use of heading angle information from the dual-antenna satellite receiver ensures high accuracy of the fused heading angle over a long period. The comprehensive utilization of the inertial navigation heading angle, the dual-antenna heading angle, the velocity decomposition heading angle, and the angular velocity information of the carrier results in high reliability and stability of the fused heading angle.

Description

Technical Field

[0001] This invention relates to the field of integrated navigation technology, specifically to a method and system for real-time fusion of heading angles. Background Technology

[0002] Micro-Electro-Mechanical Systems (MEMS) integrated navigation systems utilize three MEMS gyroscopes and three MEMS accelerometers as inertial devices. Due to their advantages such as low cost, low power consumption, small size, strong anti-interference ability, and suitability for mass production, they are widely used in industrial control systems, drones, vehicle navigation, and other fields.

[0003] MEMS integrated navigation systems output three angle signals: pitch, roll, and heading. In most attitude angle application scenarios, how to fuse data from multiple sensors to calculate more accurate angles and reduce angle errors is a technical challenge. While pitch and roll errors in MEMS integrated navigation systems can be eliminated by correcting accelerometer signals, heading errors cannot be corrected in this way, and the heading angle drifts over time, accumulating increasingly larger errors.

[0004] To address the heading angle issue, MEMS integrated navigation systems typically consist of two main parts: a MEMS inertial navigation system and a dual-antenna satellite receiver. The MEMS inertial navigation system offers a high heading angle update frequency and can provide accurate angle change information over a short period. However, because MEMS gyroscopes are not sensitive to the Earth's angular velocity, they cannot obtain an accurate initial value for the heading angle, and their error diverges over time. The dual-antenna satellite receiver offers high heading angle accuracy over long periods, but its data update frequency is low and it is susceptible to interference. When there is no satellite signal, the dual-antenna satellite receiver cannot provide heading angle information; when there is a satellite signal, the heading angle provided by the dual-antenna satellite receiver may have a large angle error.

[0005] In existing technologies, to address the heading angle divergence problem, MEMS integrated navigation systems often use the heading angle provided by a dual-antenna satellite receiver for correction. Current solutions directly treat the dual-antenna heading angle as an observation and the heading angle error as a state variable, using a Kalman filter for information fusion to suppress heading angle divergence caused by gyroscope drift. Theoretically, this information fusion method can obtain the optimal estimate of the heading angle error. However, heading angle accuracy heavily depends on the dual-antenna heading angle provided by the dual-antenna satellite receiver; however, the dual-antenna heading angle is inaccurate during vehicle turns and acceleration / deceleration.

[0006] The performance of a Kalman filter depends on the covariance matrix of system noise and observation noise. Inappropriate parameters can lead to slow convergence, oscillation, or even divergence. In such cases, even if the heading angle provided by the dual-antenna satellite receiver is accurate, the Kalman filter may fail to converge to the true value within a short time and cannot effectively correct the heading angle error. Summary of the Invention

[0007] The purpose of this invention is to provide a real-time heading angle fusion method and system to solve the problems in existing related solutions, such as the high heading angle update frequency of MEMS inertial navigation systems, but the error diverging with time, and the high heading angle accuracy of dual-antenna satellite receivers over long periods of time, but the low data update frequency and susceptibility to interference, making it impossible to obtain accurate and true heading angles.

[0008] To achieve the above objectives, embodiments of this application provide a real-time heading angle fusion method, comprising:

[0009] Collect the Z-axis angular velocity and inertial navigation angle of the MEMS inertial navigation system; collect the dual-antenna heading angle, eastward velocity, and northward velocity of the dual-antenna satellite receiver; calculate the velocity decomposition heading angle based on the eastward and northward velocities of the dual-antenna satellite receiver;

[0010] Establish a sliding window to store the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle, and the Z-axis angular velocity, forming a storage sequence;

[0011] By combining the Z-axis angular velocity information of the MEMS inertial navigation system, the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, and the velocity decomposition heading angle are fused to obtain the fused heading angle.

[0012] The fused heading angle is used as the heading angle of the MEMS inertial navigation system.

[0013] The method further includes:

[0014] Calculate the angle error based on the current inertial navigation heading angle and fused heading angle output;

[0015] Based on the aforementioned angle error, the data in the storage sequences of inertial navigation heading angle, dual-antenna heading angle, and velocity decomposition heading angle are updated respectively.

[0016] The method further includes:

[0017] The instantaneous value of the Z-axis angular velocity Wz[i] of the MEMS inertial navigation system is collected every sampling period T1, where i=1,2,3…; the sampling period T1 is the data update period of the MEMS inertial navigation system.

[0018] Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system is collected, and the dual-antenna heading angle Dg, eastward velocity Ve, and northward velocity Vn of the dual-antenna satellite receiver are collected; the sampling period T2 is the data update period of the dual-antenna satellite receiver.

[0019] The Z-axis angular velocity Wz within the sampling period T2 is calculated as follows:

[0020] Wz=(Wz[1]+Wz[2]+…Wz[N0]) / N0.

[0021] The method further includes:

[0022] The velocity decomposition heading angle Dv is calculated based on the eastward velocity Ve and northward velocity Vn of the dual-antenna satellite receiver, specifically including:

[0023] Calculate the principal value of the argument of the arctangent function:

[0024]

[0025] Perform quadrant determination:

[0026] If Ve≥0 and Vn≥0, the velocity vector is located in the first quadrant;

[0027] If Ve < 0 and Vn ≥ 0, the velocity vector is located in the second quadrant.

[0028] If Ve < 0 and Vn < 0, the velocity vector is located in the third quadrant.

[0029] If Ve≥0 and Vn<0, the velocity vector is located in the fourth quadrant.

[0030] Calculate the velocity decomposition heading angle Dv:

[0031] When the velocity vector is in the first quadrant: Dv = θ;

[0032] When the velocity vector is in the second or third quadrant: Dv = θ + π;

[0033] When the velocity vector is in the fourth quadrant: Dv = θ + 2π;

[0034] Where π is the ratio of a circle's diameter to its circumference.

[0035] The establishment of a sliding window stores the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle, and the Z-axis angular velocity, forming a storage sequence, including:

[0036] The inertial navigation heading angle storage sequence is Mn[i], the dual-antenna heading angle storage sequence is Mg[i], the velocity decomposition heading angle storage sequence is Mv[i], and the Z-axis angular velocity storage sequence is Mz[i]; where i=1,2…N, there are a total of N storage points;

[0037] Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, the velocity decomposition heading angle Dv, and the Z-axis angular velocity Wz are acquired. The corresponding counter variable Nc is initially set to 1. After each signal acquisition and storage sequence update, the value of Nc increases by 1.

[0038] When the sliding window is not full and Nc≤N, the currently acquired signal is stored in the sliding window:

[0039] Mn[Nc]=Dn, Mg[Nc]=Dg, Mv[Nc]=Dv, Mz[Nc]=Wz;

[0040] When the sliding window is full

[0041] For the inertial navigation heading angle storage sequence, the sliding window data is updated by shifting:

[0042] Mn[i]=Mn[i+1], i=1,2,…,N-1; Mn[N]=Dn;

[0043] For dual-antenna heading angle storage sequences, the sliding window data is updated using a shift method:

[0044] Mg[i]=Mg[i+1], i=1,2,…,N-1; Mg[N]=Dg;

[0045] For the velocity decomposition heading angle storage sequence, the sliding window data is updated by shifting:

[0046] Mv[i]=Mv[i+1], i=1,2,…,N-1; Mv[N]=Dv;

[0047] For the Z-axis angular velocity storage sequence, the sliding window data is updated using a shift method:

[0048] Mz[i]=Mz[i+1], i=1,2,…,N-1; Mz[N]=Wz;

[0049] If the current value of the counter variable Nc is N, then set Nc=1, which means that the counting will start again next time.

[0050] The information from the Z-axis angular velocity of the MEMS inertial navigation system is combined with the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, and the velocity decomposition heading angle to obtain the fused heading angle, including:

[0051] By combining the Z-axis angular velocity information Wz from the MEMS inertial navigation system, data fusion is performed on the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv. The data fusion method is as follows:

[0052]

[0053] in, , , The weighting coefficients for data fusion.

[0054] The method further includes:

[0055] For weighting coefficients , , Perform adaptive adjustments;

[0056] Set the angular velocity judgment threshold E1, the heading angle error judgment threshold E2, and the vector dot product value judgment threshold E3;

[0057] Calculate the error angle and :

[0058]

[0059]

[0060] Where ABS represents absolute value operation;

[0061] Perform a conditional judgment; if none of the following three conditions can be met simultaneously:

[0062] , , ;

[0063] Take weighting coefficients , , ;

[0064] If all three conditions are met simultaneously, calculate the vector dot product values ​​r1 and r2:

[0065]

[0066]

[0067] in, The modulo operation of the inertial navigation azimuth storage sequence Mn[i] is represented as follows:

[0068]

[0069] Similarly, This represents the modulus operation of the dual-antenna heading angle storage sequence Mg[i]. The modulo operation represents the velocity decomposition heading angle storage sequence Mv[i] (i=1,2…N), where i=1,2…N;

[0070] The vector dot product operation is as follows:

[0071]

[0072] Similarly, Perform similar calculations;

[0073] When r1 < E3, update the value of r1 to 0.0;

[0074] When r2 < E3, update the value of r2 to 0.0;

[0075] Calculate the weighting coefficients , , :

[0076]

[0077]

[0078]

[0079] Where A is an adjustable coefficient with a value range of [10,20]; B is an adjustable coefficient with a value range of [5,10]; and exp represents the natural exponent operation.

[0080] On the other hand, this application provides a real-time heading angle fusion system, including:

[0081] The data acquisition and processing unit is used to acquire the Z-axis angular velocity and inertial navigation angle of the MEMS inertial navigation system; acquire the dual-antenna heading angle, eastward velocity, and northward velocity of the dual-antenna satellite receiver; and calculate the velocity decomposition heading angle based on the eastward velocity and northward velocity of the dual-antenna satellite receiver.

[0082] The sequence storage unit is used to establish a sliding window to store the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle and the Z-axis angular velocity, forming a storage sequence.

[0083] The data fusion unit is used to combine the Z-axis angular velocity information of the MEMS inertial navigation system to fuse the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, and the velocity decomposition heading angle to obtain the fused heading angle.

[0084] The correction output unit is used to use the fused heading angle as the heading angle of the MEMS inertial navigation system.

[0085] The system also includes:

[0086] The error update unit is used to calculate the angle error based on the current inertial navigation heading angle and fused heading angle output; and to update the data in the storage sequences of inertial navigation heading angle, dual-antenna heading angle and velocity decomposition heading angle according to the angle error.

[0087] The data fusion unit is also used for:

[0088] By combining the Z-axis angular velocity information Wz from the MEMS inertial navigation system, data fusion is performed on the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv. The data fusion method is as follows:

[0089]

[0090] in, , , These are the weighting coefficients for data fusion;

[0091] It also includes the weighting coefficients , , Perform adaptive adjustments:

[0092] Set the angular velocity judgment threshold E1, the heading angle error judgment threshold E2, and the vector dot product value judgment threshold E3;

[0093] Calculate the error angle and :

[0094]

[0095]

[0096] Where ABS represents absolute value operation;

[0097] Perform a conditional judgment; if none of the following three conditions can be met simultaneously:

[0098] , , ;

[0099] Take weighting coefficients , , ;

[0100] If all three conditions are met simultaneously, calculate the vector dot product values ​​r1 and r2:

[0101]

[0102]

[0103] in, The modulo operation of the inertial navigation azimuth storage sequence Mn[i] is represented as follows:

[0104]

[0105] Similarly, This represents the modulus operation of the dual-antenna heading angle storage sequence Mg[i]. The modulo operation represents the velocity decomposition heading angle storage sequence Mv[i] (i=1,2…N), where i=1,2…N;

[0106] The vector dot product operation is as follows:

[0107]

[0108] Similarly, Perform similar calculations;

[0109] When r1 < E3, update the value of r1 to 0.0;

[0110] When r2 < E3, update the value of r2 to 0.0;

[0111] Calculate the weighting coefficients , , :

[0112]

[0113]

[0114]

[0115] Where A is an adjustable coefficient, with a value range of [10, 20]; B is an adjustable coefficient, with a value range of [5, 10]; and exp represents the natural exponent operation.

[0116] The method and system provided in this application acquire output data from a MEMS inertial navigation system and a dual-antenna satellite receiver in real time, update the heading angle storage sequence and the Z-axis angular velocity storage sequence in real time, perform comprehensive information processing and data fusion, correct the heading angle of the MEMS inertial navigation system before outputting it, and update the values ​​in the heading angle storage sequence. In the scheme of this application, the MEMS inertial navigation system provides accurate relative angle changes in a short time, resulting in a high output frequency for the fused heading angle; the heading angle information from the dual-antenna satellite receiver ensures high accuracy of the fused heading angle over a long period; and the comprehensive use of the inertial navigation heading angle, the dual-antenna heading angle, the velocity decomposition heading angle, and the angular velocity information of the carrier results in high reliability and stability of the fused heading angle. Attached Figure Description

[0117] Figure 1 A flowchart illustrating the principle of the real-time heading angle fusion method provided in this application embodiment;

[0118] Figure 2 A schematic diagram of a heading angle fusion process model provided for an embodiment of this application;

[0119] Figure 3 A diagram illustrating the real-time fusion weighting coefficient adaptive adjustment calculation process for heading angle provided in this application embodiment;

[0120] Figure 4 A schematic diagram of the structure of the real-time heading angle fusion system provided in the embodiments of this application. Detailed Implementation

[0121] To better understand the present invention, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only for illustration and explanation of the embodiments of the present invention and are not intended to limit the embodiments of the present invention. Various modifications can be made to the embodiments as long as the effects of the present invention are achieved.

[0122] This application proposes a real-time heading angle fusion method and system. It involves real-time acquisition of output data from a MEMS inertial navigation system and a dual-antenna satellite receiver, real-time updating of the heading angle storage sequence and the Z-axis angular velocity storage sequence, comprehensive information processing and data fusion, correction of the heading angle of the MEMS inertial navigation system before output, and updating the values ​​in the heading angle storage sequence.

[0123] A dual-antenna satellite receiver can provide velocity signals, and an approximate heading angle value can be calculated based on the velocity decomposition principle. To combine the best features and compensate for the shortcomings, the inertial heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, and the velocity decomposition heading angle are fused in real time according to certain rules to obtain more accurate heading angle information as the final output.

[0124] Specifically, such as Figure 1 As shown, Figure 1 The flowchart of the real-time heading angle fusion method provided in the embodiments of this application is shown, wherein,

[0125] Step 101: Collect the Z-axis angular velocity and inertial navigation angle of the MEMS inertial navigation system; collect the dual-antenna heading angle, eastward velocity, and northward velocity of the dual-antenna satellite receiver; calculate the velocity decomposition heading angle based on the eastward velocity and northward velocity of the dual-antenna satellite receiver.

[0126] like Figure 2 The diagram shown is a schematic representation of a heading angle fusion process model provided in an embodiment of this application. The process and steps include data acquisition and processing, updating the heading angle storage sequence and the Z-axis angular velocity storage sequence, data fusion, outputting the corrected heading angle, and updating the heading angle sequence according to the angle error. This embodiment of the application uses... Figure 2 Taking the heading angle fusion process shown as an example, the main implementation process of the embodiments of this application is explained.

[0127] In one embodiment of this application, the instantaneous value of the Z-axis angular velocity Wz[i] (i=1,2,3…) of the MEMS inertial navigation system is acquired every sampling period T1. The sampling period T1 is the data update period of the MEMS inertial navigation system, which can be taken as T1=5ms.

[0128] Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system is acquired, and the dual-antenna heading angle Dg, eastward velocity Ve, and northward velocity Vn of the dual-antenna satellite receiver are acquired. Generally, the sampling period T2 is selected as the data update period of the dual-antenna satellite receiver, with the relationship T2=N0*T1 (N0=40).

[0129] The Z-axis angular velocity Wz within the sampling period T2 is calculated as follows:

[0130] Wz=(Wz[1]+Wz[2]+…Wz[N0]) / N0

[0131] The specific steps for calculating the velocity decomposition heading angle Dv based on the eastward velocity Ve and northward velocity Vn of the dual-antenna satellite receiver are as follows:

[0132] Calculate the principal value of the argument of the arctangent function:

[0133]

[0134] Perform quadrant determination:

[0135] If Ve≥0 and Vn≥0, the velocity vector is located in the first quadrant;

[0136] If Ve < 0 and Vn ≥ 0, the velocity vector is located in the second quadrant.

[0137] If Ve < 0 and Vn < 0, the velocity vector is located in the third quadrant.

[0138] If Ve≥0 and Vn<0, the velocity vector is located in the fourth quadrant.

[0139] Step 3: Calculate the velocity decomposition heading angle Dv:

[0140] When the velocity vector is in the first quadrant:

[0141] Dv=θ

[0142] When the velocity vector is located in the second or third quadrant:

[0143] Dv=θ+π

[0144] When the velocity vector is in the fourth quadrant:

[0145] Dv=θ+2π

[0146] Where π is the ratio of a circle's diameter to its circumference.

[0147] Step 102: Establish a sliding window to store the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle, and the Z-axis angular velocity, forming a storage sequence.

[0148] In one embodiment of this application, a sliding window is established to form a storage sequence. The inertial navigation heading angle storage sequence is Mn[i] (i=1,2…N), with a total of N storage points. The dual-antenna heading angle storage sequence is Mg[i] (i=1,2…N), with a total of N storage points. The velocity decomposition heading angle storage sequence is Mv[i] (i=1,2…N), with a total of N storage points; the Z-axis angular velocity storage sequence is Mz[i] (i=1,2…N), with a total of N storage points, where N is preferably 50.

[0149] Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, the velocity decomposition heading angle Dv, and the Z-axis angular velocity Wz are acquired. The corresponding counter variable Nc is initially set to 1. After each signal acquisition and storage sequence update, the value of Nc increases by 1.

[0150] When the sliding window is not full and Nc≤N, the currently acquired signal is stored in the sliding window:

[0151] Mn[Nc]=Dn

[0152] Mg[Nc]=Dg

[0153] Mv[Nc]=Dv

[0154] Mz[Nc]=Wz

[0155] When the sliding window is full, for the inertial navigation angle storage sequence, update the sliding window data by shifting:

[0156] Mn[i]=Mn[i+1], i=1,2,…,N-1

[0157] Mn[N]=Dn

[0158] When the sliding window is full, for the dual-antenna heading angle storage sequence, the sliding window data is updated by shifting:

[0159] Mg[i] = Mg[i+1], i = 1, 2, ..., N-1

[0160] Mg[N]=Dg

[0161] When the sliding window is full, update the sliding window data by shifting the velocity decomposition heading angle storage sequence:

[0162] Mv[i]=Mv[i+1], i=1,2,…,N-1

[0163] Mv[N]=Dv

[0164] When the sliding window is full, update the sliding window data by shifting the Z-axis angular velocity storage sequence:

[0165] Mz[i] = Mz[i+1], i = 1, 2, ..., N-1

[0166] Mz[N]=Wz

[0167] If the current value of the counter variable Nc is N, then set Nc=1, which means the counting will start again next time.

[0168] Step 103: Combining the Z-axis angular velocity information of the MEMS inertial navigation system, the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, and the velocity decomposition heading angle are fused to obtain the fused heading angle.

[0169] In one embodiment of this application, the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv are fused together with the Z-axis angular velocity information Wz of the MEMS inertial navigation system. The data fusion formula is as follows:

[0170]

[0171] in, , , The weighting coefficients for data fusion.

[0172] Specifically, the weighting coefficients can be adaptively adjusted; see [link / reference]. Figure 3 . Figure 3 This illustration shows a calculation process diagram for real-time fusion weighted coefficient adaptive adjustment of heading angle provided in an embodiment of this application, wherein...

[0173] Set an angular velocity judgment threshold E1, preferably 5 degrees per second; set a heading angle error judgment threshold E2, preferably 1.0 degree (i.e. 0.0174533 radians); set a vector dot product value judgment threshold E3, preferably 0.5.

[0174] Weighting coefficients , , The specific steps for adaptive adjustment followed by data fusion are as follows:

[0175] Step 31: Calculate the error angle and :

[0176]

[0177]

[0178] Here, ABS represents absolute value operation.

[0179] Step 32: Perform a conditional check. If the following three conditions cannot be met simultaneously:

[0180] Condition 1:

[0181] Condition 2:

[0182] Condition 3:

[0183] Take weighting coefficients

[0184]

[0185]

[0186]

[0187] If all three conditions are met, proceed to step 33; otherwise, proceed to step 35.

[0188] Step 33: Calculate the vector dot product values ​​r1 and r2:

[0189]

[0190]

[0191] in, The modulo operation represents the storage sequence of inertial navigation azimuth angles Mn[i] (i=1,2…N), i.e.

[0192]

[0193] akin, This represents the modulus operation of the dual-antenna heading angle storage sequence Mg[i] (i=1,2…N). The modulo operation represents the velocity decomposition heading angle storage sequence Mv[i] (i=1,2…N).

[0194] The vector dot product operation is defined as follows:

[0195]

[0196] It performs similar calculations.

[0197] When r1 < E3, update the value of r1 to 0.0.

[0198] When r2 < E3, update the value of r2 to 0.0.

[0199] Step 34: Calculate the weighting coefficients , , :

[0200]

[0201]

[0202]

[0203] Where A is an adjustable coefficient with a value range of [10,20]; B is an adjustable coefficient with a value range of [5,10]; and exp represents the natural exponent operation.

[0204] Step 35: Perform data fusion calculation of heading angle:

[0205] .

[0206] Step 104: Use the fused heading angle as the heading angle of the MEMS inertial navigation system.

[0207] In one embodiment of this application, the heading angle D obtained by data fusion calculation is closer to the actual heading angle of the system and is used as the corrected heading angle output of the system at the current moment.

[0208] In one embodiment of this application, a process for updating the heading angle sequence based on the angle error is also included.

[0209] Calculate the angle error based on the current inertial navigation heading angle Dn and the corrected heading angle output D:

[0210]

[0211] Update the data in the inertial navigation heading angle storage sequence by adding Mn[i]+ (i=1,2…N) is stored as the latest inertial navigation heading angle sequence for subsequent use.

[0212] Update the data in the dual-antenna heading angle storage sequence by adding Mg[i]+ (i=1,2…N) is stored as the latest dual-antenna heading angle sequence for subsequent use.

[0213] Update the data in the velocity decomposition heading angle storage sequence by adding Mv[i]+ (i=1,2…N) is stored as the latest velocity decomposition heading angle sequence for subsequent use.

[0214] In this embodiment, when updating the heading angle storage sequence and the Z-axis angular velocity storage sequence, a sliding window is established to form a storage sequence. The inertial navigation heading angle storage sequence is Mn[i] (i=1,2…N) with a total of N storage points, the dual-antenna heading angle storage sequence is Mg[i] (i=1,2…N) with a total of N storage points, the velocity decomposition heading angle storage sequence is Mv[i] (i=1,2…N) with a total of N storage points, and the Z-axis angular velocity storage sequence is Mz[i] (i=1,2…N) with a total of N storage points. N is preferably 50. Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, the velocity decomposition heading angle Dv, and the Z-axis angular velocity Wz are acquired. When the sliding window is not full, the currently acquired signal is stored in the sliding window. When the sliding window is full, the sliding window data is updated by shifting.

[0215] During data fusion, the Z-axis angular velocity information Wz of the MEMS inertial navigation system is combined with the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv to perform data fusion. The data fusion formula is as follows: , , , These are the weighting coefficients for data fusion, which are adaptively adjusted based on the characteristics of the stored sequences.

[0216] Update the heading angle sequence based on the angle error, and calculate the angle error between the corrected heading angle output D and the inertial navigation heading angle Dn at the current moment. Update the data in the inertial navigation angle storage sequence by adding Mn[i]+ (i=1,2…N) is used as the latest inertial navigation heading angle storage sequence. The data in the dual-antenna heading angle storage sequence is updated, and Mg[i]+ (i=1,2…N) is used as the latest dual-antenna heading angle storage sequence. The data in the velocity decomposition heading angle storage sequence is updated by adding Mv[i]+ (i=1,2…N) is the latest velocity decomposition heading angle storage sequence.

[0217] This application embodiment also provides a complete real-time heading angle fusion system, such as... Figure 4 As shown, it includes:

[0218] The data acquisition and processing unit 21 is used to acquire the Z-axis angular velocity and the inertial navigation angle of the MEMS inertial navigation system; acquire the dual-antenna heading angle, eastward velocity, and northward velocity of the dual-antenna satellite receiver; and calculate the velocity decomposition heading angle based on the eastward velocity and northward velocity of the dual-antenna satellite receiver.

[0219] The sequence storage unit 22 is used to establish a sliding window to store the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle and the Z-axis angular velocity, respectively, forming a storage sequence;

[0220] The data fusion unit 23 is used to combine the Z-axis angular velocity information of the MEMS inertial navigation system to perform data fusion on the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, and the velocity decomposition heading angle to obtain the fused heading angle.

[0221] The correction output unit 24 is used to use the fused heading angle as the heading angle of the MEMS inertial navigation system.

[0222] The system also includes:

[0223] Error update unit 25 is used to calculate the angle error based on the current inertial navigation heading angle and fused heading angle output; and to update the data in the inertial navigation heading angle, dual-antenna heading angle and velocity decomposition heading angle storage sequence according to the angle error.

[0224] The data fusion unit 23 is further configured to:

[0225] By combining the Z-axis angular velocity information Wz from the MEMS inertial navigation system, data fusion is performed on the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv. The data fusion method is as follows:

[0226]

[0227] in, , , These are the weighting coefficients for data fusion;

[0228] It also includes the weighting coefficients , , Perform adaptive adjustments:

[0229] Set the angular velocity judgment threshold E1, the heading angle error judgment threshold E2, and the vector dot product value judgment threshold E3;

[0230] Calculate the error angle and :

[0231]

[0232]

[0233] Where ABS represents absolute value operation;

[0234] Perform a conditional judgment; if none of the following three conditions can be met simultaneously:

[0235] , , ;

[0236] Take weighting coefficients , , ;

[0237] If all three conditions are met simultaneously, calculate the vector dot product values ​​r1 and r2:

[0238]

[0239]

[0240] in, The modulo operation of the inertial navigation azimuth storage sequence Mn[i] is represented as follows:

[0241]

[0242] Similarly, This represents the modulus operation of the dual-antenna heading angle storage sequence Mg[i]. The modulo operation represents the velocity decomposition heading angle storage sequence Mv[i] (i=1,2…N), where i=1,2…N;

[0243] The vector dot product operation is as follows:

[0244]

[0245] Similarly, Perform similar calculations;

[0246] When r1 < E3, update the value of r1 to 0.0;

[0247] When r2 < E3, update the value of r2 to 0.0;

[0248] Calculate the weighting coefficients , , :

[0249]

[0250]

[0251]

[0252] Where A is an adjustable coefficient with a value range of [10,20]; B is an adjustable coefficient with a value range of [5,10]; and exp represents the natural exponent operation.

[0253] Those skilled in the art will understand that embodiments of this application can be provided as methods, apparatus, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0254] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0255] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0256] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0257] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0258] Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, like read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0259] Computer-readable media include both permanent and non-permanent, removable and non-removable media that can store information by any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic magnetic disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0260] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, or they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0261] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0262] The above are merely embodiments of this application and are not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

[0263] The accompanying drawings illustrate several block diagrams and / or flowcharts. It should be understood that some blocks, or combinations thereof, in the block diagrams and / or flowcharts can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that when executed by the processor, these instructions can create means for implementing the functions / operations described in these block diagrams and / or flowcharts. The technology of this application can be implemented in hardware and / or software (including firmware, microcode, etc.). Alternatively, the technology of this application can take the form of a computer program product stored on a computer-readable storage medium, which can be used by or in conjunction with an instruction execution system.

Claims

1. A real-time heading angle fusion method, characterized in that, include: Collect the Z-axis angular velocity and inertial navigation angle of the MEMS inertial navigation system; Collect the heading angle, eastward velocity, and northward velocity of the dual-antenna satellite receiver; The velocity decomposition heading angle is calculated based on the eastward and northward velocities of the dual-antenna satellite receiver; Establish a sliding window to store the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle, and the Z-axis angular velocity, forming a storage sequence; The Z-axis angular velocity in the stored sequence is compared with a preset judgment threshold. Based on the comparison result, the weight coefficients of the inertial navigation heading angle, the dual-antenna heading angle, and the velocity decomposition heading angle are adaptively adjusted. The three are then weighted and fused to obtain the fused heading angle. The fused heading angle is used as the heading angle of the MEMS inertial navigation system.

2. The method according to claim 1, characterized in that, The method further includes: Calculate the angle error based on the current inertial navigation heading angle and fused heading angle output; Based on the aforementioned angle error, the data in the storage sequences of inertial navigation heading angle, dual-antenna heading angle, and velocity decomposition heading angle are updated respectively.

3. The method according to claim 1, characterized in that, The method further includes: The instantaneous value of the Z-axis angular velocity Wz[i] of the MEMS inertial navigation system is collected every sampling period T1, where i=1,2,3…; the sampling period T1 is the data update period of the MEMS inertial navigation system. Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system is collected, and the dual-antenna heading angle Dg, eastward velocity Ve, and northward velocity Vn of the dual-antenna satellite receiver are collected; the sampling period T2 is the data update period of the dual-antenna satellite receiver. The Z-axis angular velocity Wz within the sampling period T2 is calculated as follows: Wz=(Wz[1]+Wz[2]+…Wz[N0]) / N0.

4. The method according to claim 3, characterized in that, The method further includes: The velocity decomposition heading angle Dv is calculated based on the eastward velocity Ve and northward velocity Vn of the dual-antenna satellite receiver, specifically including: Calculate the principal value of the argument of the arctangent function: ； Perform quadrant determination: If Ve≥0 and Vn≥0, the velocity vector is located in the first quadrant; If Ve < 0 and Vn ≥ 0, the velocity vector is located in the second quadrant. If Ve < 0 and Vn < 0, the velocity vector is located in the third quadrant. If Ve≥0 and Vn<0, the velocity vector is located in the fourth quadrant. Calculate the velocity decomposition heading angle Dv: When the velocity vector is in the first quadrant: Dv = θ; When the velocity vector is in the second or third quadrant: Dv = θ + π; When the velocity vector is in the fourth quadrant: Dv = θ + 2π; Where π is the ratio of a circle's diameter to its circumference.

5. The method according to claim 4, characterized in that, The establishment of a sliding window stores the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle, and the Z-axis angular velocity, forming a storage sequence, including: The inertial navigation heading angle storage sequence is Mn[i], the dual-antenna heading angle storage sequence is Mg[i], the velocity decomposition heading angle storage sequence is Mv[i], and the Z-axis angular velocity storage sequence is Mz[i]; where i=1,2…N, there are a total of N storage points; Every sampling period T2, the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, the velocity decomposition heading angle Dv, and the Z-axis angular velocity Wz are acquired. The corresponding counter variable Nc is initially set to 1. After each signal acquisition and storage sequence update, the value of Nc increases by 1. When the sliding window is not full and Nc≤N, the currently acquired signal is stored in the sliding window: Mn[Nc]=Dn, Mg[Nc]=Dg, Mv[Nc]=Dv, Mz[Nc]=Wz; When the sliding window is full For the inertial navigation heading angle storage sequence, the sliding window data is updated by shifting: Mn[i]=Mn[i+1], i=1,2,…,N-1; Mn[N]=Dn; For dual-antenna heading angle storage sequences, the sliding window data is updated using a shift method: Mg[i]=Mg[i+1], i=1,2,…,N-1; Mg[N]=Dg; For the velocity decomposition heading angle storage sequence, the sliding window data is updated by shifting: Mv[i]=Mv[i+1], i=1,2,…,N-1; Mv[N]=Dv; For the Z-axis angular velocity storage sequence, the sliding window data is updated using a shift method: Mz[i]=Mz[i+1], i=1,2,…,N-1; Mz[N]=Wz; If the current value of the counter variable Nc is N, then set Nc=1, which means that the counting will start again next time.

6. The method according to claim 5, characterized in that, The process involves comparing the Z-axis angular velocity in the stored sequence with a preset judgment threshold, adaptively adjusting the weight coefficients of the inertial navigation heading angle, the dual-antenna heading angle, and the velocity decomposition heading angle based on the comparison result, and then weighting and fusing the three to obtain the fused heading angle, including: By combining the Z-axis angular velocity information Wz of the MEMS inertial navigation system, data fusion is performed on the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv. The data fusion method is as follows: ； in, , , The weighting coefficients for data fusion.

7. The method according to claim 6, characterized in that, The method further includes: For weighting coefficients , , Perform adaptive adjustments; Set the angular velocity judgment threshold E1, the heading angle error judgment threshold E2, and the vector dot product value judgment threshold E3; Calculate the error angle and : ； ； Where ABS represents absolute value operation; Perform a conditional judgment; if none of the following three conditions can be met simultaneously: 、 、 ； Take weighting coefficients , , ; If all three conditions are met simultaneously, calculate the vector dot product values ​​r1 and r2: ； ； in, The modulo operation of the inertial navigation azimuth storage sequence Mn[i] is represented as follows: ； Similarly, This represents the modulus operation of the dual-antenna heading angle storage sequence Mg[i]. The modulus operation represents the velocity decomposition heading angle storage sequence Mv[i], where i=1,2…N; The vector dot product operation is as follows: ； Similarly, Perform similar calculations; When r1 < E3, update the value of r1 to 0.0; When r2 < E3, update the value of r2 to 0.0; Calculate the weighting coefficients , , : ； ； ； Where A is an adjustable coefficient with a value range of [10,20]; B is an adjustable coefficient with a value range of [5,10]; and exp represents the natural exponent operation.

8. A real-time heading angle fusion system, characterized in that, include: The data acquisition and processing unit is used to acquire the Z-axis angular velocity and the inertial navigation angle of the MEMS inertial navigation system. Collect the heading angle, eastward velocity, and northward velocity of the dual-antenna satellite receiver; calculate the velocity decomposition heading angle based on the eastward and northward velocities of the dual-antenna satellite receiver; The sequence storage unit is used to establish a sliding window to store the inertial navigation heading angle of the MEMS inertial navigation system, the dual-antenna heading angle of the dual-antenna satellite receiver, the velocity decomposition heading angle and the Z-axis angular velocity, forming a storage sequence. The data fusion unit is used to compare the Z-axis angular velocity in the stored sequence with a preset judgment threshold, adaptively adjust the weight coefficients of the inertial navigation heading angle, the dual-antenna heading angle and the velocity decomposition heading angle according to the comparison result, and perform weighted fusion of the three to obtain the fused heading angle. A correction output unit is used to use the fused heading angle as the heading angle of the MEMS inertial navigation system.

9. The system according to claim 8, characterized in that, The system also includes: The error update unit is used to calculate the angle error based on the current inertial navigation heading angle and fused heading angle output; and to update the data in the storage sequences of inertial navigation heading angle, dual-antenna heading angle and velocity decomposition heading angle according to the angle error.

10. The system according to claim 8, characterized in that, The data fusion unit is also used for: By combining the Z-axis angular velocity information Wz of the MEMS inertial navigation system, data fusion is performed on the inertial navigation heading angle Dn of the MEMS inertial navigation system, the dual-antenna heading angle Dg of the dual-antenna satellite receiver, and the velocity decomposition heading angle Dv. The data fusion method is as follows: ； in, , , These are the weighting coefficients for data fusion; It also includes the weighting coefficients , , Perform adaptive adjustments: Set the angular velocity judgment threshold E1, the heading angle error judgment threshold E2, and the vector dot product value judgment threshold E3; Calculate the error angle and : ； ； Where ABS represents absolute value operation; Perform a conditional judgment; if none of the following three conditions can be met simultaneously: 、 、 ； Take weighting coefficients , , ; If all three conditions are met simultaneously, calculate the vector dot product values ​​r1 and r2: ； ； in, The modulo operation of the inertial navigation azimuth storage sequence Mn[i] is represented as follows: ； Similarly, This represents the modulus operation of the dual-antenna heading angle storage sequence Mg[i]. The modulus operation represents the velocity decomposition heading angle storage sequence Mv[i], where i=1,2…N; The vector dot product operation is as follows: ； Similarly, Perform similar calculations; When r1 < E3, update the value of r1 to 0.0; When r2 < E3, update the value of r2 to 0.0; Calculate the weighting coefficients , , : ； ； ； Where A is an adjustable coefficient with a value range of [10,20]; B is an adjustable coefficient with a value range of [5,10]; and exp represents the natural exponent operation.

Images