SINS (Strapdown Inertial Navigation System) robust travel alignment method containing outlier detection and multiple backtracking filtering

Abstract

The invention discloses an SINS (Strapdown Inertial Navigation System) robust travel alignment method containing outlier detection and multiple backtracking filtering, which comprises the following steps of: acquiring and storing output information of an SINS and a GNSS (Global Navigation Satellite System) in real time, and performing attitude updating; detecting whether the speed information collected by the GNSS at the current moment is polluted by the outlier information, and obtaining a detection result; reconstructing an observation vector according to the detection result and the reference vector; based on the reference vector and the reconstructed observation vector, adopting a multiple backtracking filtering method to realize alignment; according to the invention, the design method of the correlation gradient value is used for isolating the outliers output by the GNSS, and meanwhile, the correlation error is continuously corrected in the multi-backtracking filtering process, so that the reliability of the alignment result is effectively improved, and the alignment speed is accelerated.

Description

Technical Field

[0001] The invention belongs to the technical field of strapdown inertial navigation system alignment, and in particular relates to a SINS robust on-the-go alignment method comprising outlier detection and multiple backtracking filtering. Background Art

[0002] The Strapdown Inertial Navigation System (SINS) is an autonomous navigation system that measures the angular velocity and acceleration of a moving object in real time through a gyroscope and an accelerometer, and calculates the position, velocity, and attitude information of the carrier. SINS has the characteristics of high precision and real-time performance, and does not rely on external signals. Therefore, it is widely used in aerospace, ocean exploration, vehicle navigation and other fields. SINS needs to complete initial alignment before navigation. The initial alignment accuracy is crucial for subsequent navigation. Since the error of SINS has a cumulative effect, any error generated in the initial alignment stage will accumulate rapidly over time, resulting in a continuous increase in navigation errors. There is a contradiction between the alignment time and alignment accuracy in the initial alignment. The existing on-the-go alignment technology mainly uses the optimization-based alignment method (OBA). This method requires accurate and outlier-free global navigation satellite system (GNSS) data to assist SINS in completing alignment. The alignment time is often more than 300 seconds, which may not be applicable to the scenario of rapid alignment in an interference environment.

[0003] Traditional on-the-go alignment methods often only perform one forward processing on the data collected by SINS, resulting in low data utilization. Traditional methods are not applicable when SINS needs to complete initial alignment quickly in a short time (e.g., within 30 seconds). On the other hand, in traditional initial alignment algorithms, the information output by GNSS is considered to be completely accurate, and the impact of outliers on GNSS output information is not considered. However, due to the influence of multipath effects, urban high-rise buildings, dense forests, etc., GNSS output information often contains outliers. At this time, the performance of traditional initial alignment algorithms will be seriously affected, and even the alignment task cannot be completed.

[0004] Therefore, in order to improve the initial alignment speed and effectively suppress the interference of outliers, it is necessary to design a SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering. Summary of the invention

[0005] Purpose of the invention: In order to solve the problems of low alignment accuracy and slow alignment speed when the existing on-the-go alignment technology is used for SINS, and poor reliability of alignment results when rapid startup is required, the present invention proposes a SINS robust on-the-go alignment method including outlier detection and multiple back-tracing filtering. A design method for related gradient values ​​is proposed to isolate the outliers output by GNSS, and related errors are continuously corrected in the process of multiple back-tracing filtering, thereby effectively improving the reliability of alignment results and accelerating the alignment speed.

[0006] Technical solution: A SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering, comprising the following steps:

[0007] Step 1: Obtain and store the output information of SINS and GNSS in real time, and perform attitude update;

[0008] Step 2: Detect whether the speed information collected by the GNSS at the current moment is contaminated by outlier information and obtain the detection result;

[0009] Step 3: Reconstruct the observation vector based on the detection results and the reference vector;

[0010] Step 4: Based on the reference vector and the reconstructed observation vector, multiple back-tracing filtering method is used to achieve alignment;

[0011] Among them, in step 1, the posture update described in the above step includes:

[0012] According to the SINS output information, the following real carrier coordinate system attitude transformation matrix differential equation is solved in real time:

[0013]

[0014] Where b(t) represents the real carrier coordinate system at continuous time t, ib represents the carrier coordinate system fixed in the inertial space at the initial moment, represents the attitude transformation matrix from the b system to the ib system at continuous time t, represents the differential form of the attitude transformation matrix of the carrier system under continuous time t, represents the projection of the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system, (·)× represents the antisymmetric matrix of (·);

[0015] Discretize the continuous time t, and the discretized time is represented by k. Iterate the discretized real carrier system attitude transformation matrix to achieve attitude update:

[0016]

[0017] Among them, k is the current moment, k-1 is the previous moment, Obtained by the following formula:

[0018]

[0019] Where E3 represents the 3×3 unit matrix, Δθ1 and Δθ2 represent the angular velocity increments at two consecutive moments output by the gyroscope, Represents the rotation vector of the carrier coordinate system from time k-1 to time k;

[0020] According to the GNSS output information, the following navigation coordinate system attitude transformation matrix differential equation is solved in real time:

[0021]

[0022] Among them, n(t) represents the navigation coordinate system at continuous time t, in represents the navigation coordinate system fixed in the inertial coordinate space at the initial moment, represents the attitude transformation matrix from the n system to the in system at continuous time t, represents the differential form of the attitude transformation matrix of the navigation coordinate system under continuous time t, Represents the projection of the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system;

[0023] Iterate the discretized navigation system attitude transformation matrix to achieve attitude update:

[0024]

[0025] in Calculated by the following formula:

[0026]

[0027] in, is the rotation vector of the navigation system from time k-1 to time k, T s For the update cycle;

[0028] Among them, in step 2, the detection of whether the speed information collected by the GNSS at the current moment is contaminated by wild values ​​to obtain the detection result specifically includes:

[0029] S2_1: Construct the following related gradient values:

[0030]

[0031] In the formula, Represents the constant attitude matrix rotating from the in coordinate system to the ib coordinate system, Represents the observation vector containing outlier information, expressed as: Ξ v Indicates outlier information, β ν,S represents the observation vector, Represents the attitude transformation matrix of the navigation coordinate system n rotating to the in system;

[0032] S2_2: If the relevant gradient value exceeds the preset threshold, it means that the speed information collected by the GNSS at the current moment is contaminated by the outlier information; otherwise, it means that the speed information collected by the GNSS at the current moment is not contaminated by the outlier information;

[0033] Among them, in step 3, the observation vector is reconstructed based on the detection result and the reference vector, and the specific operations include:

[0034] S3_1: Design the following weight function Υ(θ k ):

[0035]

[0036] In the formula, θ k Represents the relevant gradient value, ρ is the preset threshold;

[0037] S3_2: According to the preset threshold ρ and weight function Υ(θ k ) for the observation vector containing outlier information Reconstruct and obtain the reconstructed observation vector It is expressed as:

[0038]

[0039] Wherein, in step 4, the alignment is achieved by using a multiple back-tracing filtering method based on the reference vector and the reconstructed observation vector, and the specific operations include:

[0040] S4_1: Considering the gyroscope measurement error in SINS, the angular velocity information output by the gyroscope is remodeled and expressed as:

[0041]

[0042] in, Indicates the angular velocity information actually output by the gyroscope. It represents the projection of the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system. represents the rotation angular velocity error, b g represents the gyroscope bias, w g represents random noise;

[0043] The corresponding computing carrier system attitude transformation matrix is ​​expressed as:

[0044]

[0045] in, represents the carrier coordinate system obtained by calculation, which is called the calculation carrier system. Represents the differential form of the attitude transformation matrix of the computing carrier system, Indicates from The attitude transformation matrix from the rotation of the system to the ib system;

[0046] S4_2: Real carrier system attitude transformation matrix And calculate the carrier system attitude transformation matrix There is an error between them, which can be expressed as:

[0047]

[0048] in, represents the rotation from b to The attitude transformation matrix of the system, Indicates that the system is calculated from The rotation vector to the real load system b;

[0049] Combined with the above formula, calculate the load system The differential equation of the rotation vector to the real load system b is expressed as follows:

[0050]

[0051] S4_3: Select the coordinate system that causes the calculation carrier The rotation vector of the error with the real carrier coordinate system b and the gyroscope bias b that causes the gyroscope constant error g As a system state variable, it is expressed as:

[0052]

[0053] The reconstructed observation vector Arrange and split it, expressed as:

[0054]

[0055] In the formula, b(t m ) represents the lower limit time of integration t m The real load system at that time, represents the rotation from b(t) to b(t m )’s attitude transformation matrix, Indicates that from b(t m ) Rotate to the posture transformation matrix of ib;

[0056] definition According to the chain rule, the expansion have to:

[0057]

[0058] Represent the reconstructed observation vector using system state variables It is expressed as:

[0059]

[0060] S4_4: Construct the following system measurement equation:

[0061] Z k =H k X k +V k

[0062] in:

[0063]

[0064] S4_5: Use the SINS and GNSS output data and the attitude transformation matrix at the current moment to solve and estimate the current attitude data, perform forward filtering on the current attitude data, estimate the system state variables at the current moment and compensate at the next moment to complete the forward filtering;

[0065] S4_6: After the forward filtering is completed, the SINS and GNSS output data are processed, and the attitude transformation matrix equation and velocity update equation of the reverse filtering are used to solve them, and the current attitude data is estimated. The current attitude data is reversely filtered, and the system state variables at the current moment are estimated and compensated at the next moment to complete the reverse filtering;

[0066] S4_7: Alignment is achieved by continuously performing forward filtering and reverse filtering.

[0067] Furthermore, before executing step 3, the following steps are also included:

[0068] At continuous time t, the specific force equation in the navigation coordinate system is expressed as:

[0069]

[0070] Among them, V n It represents the velocity vector of the carrier in the navigation coordinate system, It represents the differential of the velocity vector of the carrier in the navigation coordinate system, represents the posture matrix at continuous time t, f b represents the specific force information measured by the accelerometer, e represents the earth coordinate system, represents the projection of the earth's rotation angular velocity in the navigation coordinate system, It represents the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation system, g n Represents the projection of gravity acceleration in the navigation coordinate system;

[0071] The specific force equation in the above navigation coordinate system is integrated in the time period [0, t] and summarized as follows:

[0072]

[0073] Among them, α v is the reference vector, β v is the observation vector, expressed as follows:

[0074]

[0075] The sliding window method is used for the reference vector and observation vector constructed above, and the integration interval is changed to [t m ,t],t m For the variable lower limit time of integration, rewrite as follows:

[0076]

[0077] Furthermore, the posture transformation matrix equation of the reverse filtering is obtained by inverse deduction based on the discretized forward posture matrix update equation, which is expressed as:

[0078]

[0079] in, Represents the reverse process, It represents the attitude transformation matrix from the b system to the n system at time k-1 in the reverse process. It represents the attitude transformation matrix of rotating from the b system to the n system at time k in the reverse process. It represents the actual angular velocity information output by the gyroscope at time k during the reverse process.

[0080] Furthermore, the velocity update equation of the reverse filtering is obtained by inversely deducing the discretized forward velocity update equation, which is expressed as:

[0081]

[0082] in, Represents the reverse process, It represents the velocity vector of the carrier in the navigation coordinate system at time k during the reverse process, represents the specific force information measured by the accelerometer at time k in the reverse process, It represents the projection of the Earth's rotation angular velocity at time k in the reverse process in the navigation coordinate system, It represents the projection of the angular velocity of the navigation coordinate system at time k relative to the earth coordinate system in the navigation system during the reverse process.

[0083] Further, after the forward filtering is completed, the SINS and GNSS output data are processed, and the specific operations include:

[0084] Each time the forward filtering and the reverse filtering are switched, the corresponding data is processed, and the data of the reverse process and the data of the forward process are jointly expressed as:

[0085] in, represents the velocity information at time k in the reverse process, V n (Nk) represents the speed information at time Nk in the forward process, represents the specific force information at time k in the reverse process, f b (Nk) represents the specific force information at time Nk in the forward process, represents the angular velocity information at time k in the reverse process, Indicates the angular velocity information at time Nk during the forward process.

[0086] Furthermore, the initial value of the reverse filtering is set to:

[0087] Furthermore, the estimation of the system state variables at the current moment and compensation at the next moment specifically includes: biasing the gyroscope by b g,k Compensate for the angular velocity of the retrace process Update the attitude transformation matrix in the real carrier coordinate system:

[0088]

[0089] In the formula, It represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system at time k in the reverse process in the carrier coordinate system. Indicates the actual angular velocity information output by the gyroscope at time Nk during the forward process.

[0090] Furthermore, during the reverse filtering process, the system state variables are expressed as:

[0091]

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

[0093] (1) Compared with the traditional OBA method that uses full integral form to construct vectors, the method of the present invention uses the sliding fixed interval method to construct vectors, isolating the time from the start of alignment to t k-mThe error in the measurement information during this time interval reduces the accumulation of inertial device errors;

[0094] (2) The method of the present invention reconstructs the observation vector by designing a related gradient value method, realizes the detection and isolation of GNSS outliers, and improves the reliability of the alignment algorithm in complex scenarios;

[0095] (3) The method of the present invention adopts a multiple backtracking method for alignment. By adopting multiple forward and reverse filtering processes, the equivalent rotation vector error and gyroscope constant bias between the carrier system and the ideal carrier system are continuously compensated, thereby improving data utilization, effectively shortening the alignment time, and making the alignment result more reliable.

[0096] (4) Compared with the traditional single-step back-tracking alignment method, the method of the present invention can further improve data utilization, achieve more comprehensive mining and use of data, and further shorten the alignment time, thereby achieving higher alignment accuracy in a short time. BRIEF DESCRIPTION OF THE DRAWINGS

[0097] Figure 1 A flowchart of a SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering;

[0098] Figure 2 It is a flow chart of the multiple backtracking filtering method;

[0099] Figure 3 Schematic diagram of heading angle error when there is no outlier value;

[0100] Figure 4 Schematic diagram of rolling angle error when there is no wild value;

[0101] Figure 5 Schematic diagram of pitch angle error when there is no wild value;

[0102] Figure 6 It is a schematic diagram of speed information containing wild values;

[0103] Figure 7 Schematic diagram of heading angle error when containing wild values;

[0104] Figure 8 Schematic diagram of rolling angle error when containing wild values;

[0105] Fig. 9 Schematic diagram of pitch angle error when containing wild values. DETAILED DESCRIPTION

[0106] The technical solution of the present invention is now further described in conjunction with the accompanying drawings and embodiments.

[0107] Embodiment 1:

[0108] like Figure 1 As shown, this embodiment proposes a SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering, which mainly includes the following steps:

[0109] Step 1: Based on the output information of SINS and GNSS, the real-time attitude transformation matrix of the carrier system and navigation system is solved by combining the real-time attitude transformation matrix differential equation of the carrier system and navigation system. The specific operations include:

[0110] According to the SINS output information, the following real-life load system attitude transformation matrix differential equation is solved in real time:

[0111]

[0112] Where b(t) represents the real carrier coordinate system at continuous time t, ib represents the carrier coordinate system fixed in the inertial space at the initial moment, represents the attitude transformation matrix rotating from the b system to the ib system at continuous time t, represents the differential form of the attitude transformation matrix of the carrier system under continuous time t, represents the projection of the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system, and (·)× represents the antisymmetric matrix of (·).

[0113] The theoretical derivation is carried out under continuous time t, but since the navigation computer can only process discrete values, the continuous time t is discretized, and the discretized time is represented by k. The discretized real carrier system attitude transformation matrix is ​​iteratively calculated:

[0114]

[0115] Among them, k is the current moment, k-1 is the previous moment, Obtained by the following formula:

[0116]

[0117] Where E3 represents the 3×3 unit matrix, Δθ1 and Δθ2 represent the angular velocity increments at two consecutive moments output by the gyroscope, Represents the equivalent rotation vector of the carrier system from time k-1 to time k.

[0118] According to the GNSS output information, the following navigation coordinate system attitude transformation matrix differential equation is solved in real time:

[0119]

[0120] Among them, n(t) represents the navigation coordinate system at continuous time t, in represents the navigation coordinate system fixed in the inertial coordinate space at the initial moment, represents the attitude transformation matrix from the n system to the in system at continuous time t, represents the differential form of the attitude transformation matrix of the navigation coordinate system under continuous time t, Represents the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system.

[0121] Discretize the continuous time t and iteratively calculate the discretized navigation system attitude transformation matrix:

[0122]

[0123] in Calculated by the following formula:

[0124]

[0125] in, is the equivalent rotation vector of the navigation system from time k-1 to time k, T s For the update cycle.

[0126] Step 2: Integrate the transformed navigation system force equation by the sliding fixed interval method to construct an improved reference vector and observation vector. The specific operations include:

[0127] At continuous time t, the specific force equation in the navigation coordinate system is expressed as follows:

[0128]

[0129] Among them, V n It represents the velocity vector of the carrier in the navigation coordinate system, It represents the differential of the velocity vector of the carrier in the navigation coordinate system, represents the posture matrix at continuous time t, f b represents the specific force information measured by the accelerometer, e represents the earth coordinate system, represents the projection of the earth's rotation angular velocity in the navigation coordinate system, It represents the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation system, g n Represents the projection of gravity acceleration in the navigation coordinate system.

[0130] The specific force equation in the above navigation coordinate system is integrated in the time period [0, t] and summarized as follows:

[0131]

[0132] in, Represents the constant attitude matrix rotated from the in coordinate system to the ib coordinate system, α v is the reference vector, β v is the observation vector, expressed as follows:

[0133]

[0134] In order to suppress the accumulation of system errors during the integration process, the sliding window method is used for the vector constructed above, and the integration interval is changed to [t m ,t],t m is the variable lower limit time of integration, ensuring the integration window size tt m As a constant value, rewrite as follows:

[0135]

[0136] Step 3: Since the reference vector is very stable, a related gradient value is designed based on the reference vector to detect the wild value contained in the GNSS output information, and a weight function is constructed to reconstruct the observation vector. Based on the above reference vector and the reconstructed observation vector, the constant attitude matrix is ​​calculated by the optimization alignment method; the specific operations include:

[0137] Considering the situation where the GNSS output contains outliers, the velocity information of the GNSS output in a complex environment is remodeled:

[0138]

[0139] in, Indicates the speed information output by GNSS, V n represents the actual carrier speed, v Indicates outlier information.

[0140] Observation vector containing outlier information It is expressed as follows:

[0141]

[0142] Reference vector α v,s The construction is only related to the specific force information output by the accelerometer and is not affected by the wild value output by the GNSS. It is very helpful to detect whether the observation vector contains wild values. Therefore, the following related gradient values ​​are constructed:

[0143]

[0144] When the information output by GNSS is accurate and does not contain outliers, the observation vector Strictly equal to the constant attitude matrix With reference vector α v,SThe product of the two items is constructed by taking the modulus of each other to illustrate the actual difference between the two items, that is, the observation vector The strength of the included outliers.

[0145] When the above related gradient values ​​exceed the reasonable range, the velocity information output by GNSS is regarded as being contaminated by wild values. Based on this, a weight function is designed to construct an improved observation vector When the wild value intensity is too large, the relevant gradient value θ k becomes larger accordingly, while reducing the observation vector The weight of the reference vector α is increased ν,S The weight function Υ(θ k ) is expressed as follows:

[0146]

[0147] According to the preset threshold ρ and weight function Υ(θ k ) reconstructs the observation vector containing outlier information and derives the improved observation vector It is expressed as follows:

[0148]

[0149] Using the above improved observation vector and the reference vector α ν,S After discretization, the fourth-order attitude determination matrix S is constructed k , which is expressed as follows:

[0150]

[0151] in,

[0152]

[0153] Calculate the fourth-order attitude determination matrix S k The eigenvector corresponding to the minimum eigenvalue of is the quaternion form of the constant attitude matrix. The constant attitude matrix is ​​obtained by converting the obtained quaternion into a matrix form.

[0154] Step 4: Build a system model and perform online compensation to calculate the equivalent rotation vector error and gyroscope bias between the load system and the ideal load system. The specific operations include:

[0155] Taking into account the gyroscope measurement error, the angular velocity information output by the gyroscope is remodeled:

[0156]

[0157] in, Indicates the angular velocity information actually output by the gyroscope. It represents the projection of the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system. represents the rotation angular velocity error, b g represents the gyroscope bias, w g represents random noise.

[0158] Time-varying attitude transformation matrix of carrier coordinate system The corresponding changes are shown as follows:

[0159]

[0160] in, represents the carrier coordinate system obtained by calculation, called the calculated carrier system, and b is the real carrier coordinate system.

[0161] Real load system attitude transformation matrix And calculate the carrier system attitude transformation matrix There is an error between them, and the calculation process is expressed as follows:

[0162]

[0163] in, represents the rotation from b to The attitude transformation matrix of the system, Indicates that the system is calculated from The rotation vector to the real load system b.

[0164] Combined with the above formula, calculate the load system The differential equation of the rotation vector to the real load system b is expressed as follows:

[0165]

[0166] Based on the above derivation, select the coordinate system that causes the calculation carrier The rotation vector of the error with the real carrier coordinate system b and the gyroscope bias b that causes the gyroscope constant error g As a system state variable, it is expressed as follows:

[0167]

[0168] Improved observation vector The sorting and splitting are as follows:

[0169]

[0170] In the formula, b(t m ) represents the integral lower limit time t mThe real load system at that time, represents the rotation from b(t) to b(t m ), Indicates that from b(t m ) is rotated to the posture transformation matrix of ib.

[0171] definition According to the chain rule, the expansion

[0172]

[0173] Using system state variables to represent the improved observation vector It is expressed as follows:

[0174]

[0175] In the formula, Indicates the lower limit time of integration t m The calculation system at that time, Indicates from Rotate to the posture transformation matrix of ib.

[0176] The above formula is used to construct the system measurement equation, which is expressed as follows:

[0177] Z k =H k X k +V k

[0178] in,

[0179]

[0180] Step 5: Perform the first forward filtering, estimate the error at the current moment and compensate it at the next moment. After completing the forward filtering, process the SINS and GNSS data and start the reverse filtering, derive the reverse filtering state equation and compensate the updated error state variable, and implement the multiple back-tracking filtering method to iteratively correct the related errors. The specific operations include:

[0181] According to the forward attitude matrix update equation at discrete moments, the attitude matrix equation of the reverse process is inversely deduced:

[0182]

[0183] in, Represents the reverse process, It represents the attitude transformation matrix from the b system to the n system at time k-1 in the reverse process. It represents the attitude transformation matrix of rotating from the b system to the n system at time k in the reverse process. It represents the actual angular velocity information output by the gyroscope at time k during the reverse process.

[0184] According to the forward velocity update equation at discrete moments, the velocity update equation in the reverse process is inferred:

[0185]

[0186] in, It represents the velocity vector of the carrier in the navigation coordinate system at time k during the reverse process, represents the specific force information measured by the accelerometer at time k in the reverse process, It represents the projection of the Earth's rotation angular velocity at time k in the reverse process in the navigation coordinate system, It represents the projection of the angular velocity of the navigation coordinate system at time k relative to the earth coordinate system in the navigation system during the reverse process.

[0187] Before performing the multiple back-tracing filtering method, the stored data needs to be processed. Assuming that the length of the data collected by SINS is N, that is, N is the last moment in the forward process, each time the forward filtering and reverse filtering are switched, the corresponding data needs to be processed. The data of the reverse process and the data of the forward process are jointly expressed as follows: in, represents the velocity information at time k in the reverse process, V n (Nk) represents the speed information at time Nk in the forward process, represents the specific force information at time k in the reverse process, f b (Nk) represents the specific force information at time Nk in the forward process, represents the angular velocity information at time k in the reverse process, Indicates the angular velocity information at time Nk during the forward process.

[0188] In the multiple back-tracing filtering method, the initial value of the reverse filtering process needs to be set separately, and the specific settings are as follows: That is, the constant attitude matrix in the reverse process is equal to the attitude transformation matrix from the b system to the n system at the last moment in the forward process, and the initial velocity in the reverse process is equal to the negative number of the velocity at the last moment in the forward process.

[0189] Starting from the first forward filtering process, the filtering result is used in the next process for error compensation, that is, the gyroscope bias b g (k) Compensation for the angular velocity of the backtracking process The result is used to update the attitude transformation matrix in the carrier coordinate system during the backtracking process:

[0190]

[0191] In the formula, It represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system at time k in the reverse process in the carrier coordinate system. Indicates the actual angular velocity information output by the gyroscope at time Nk during the forward process.

[0192] In the reverse filtering process, since the data is processed, the state variables in the reverse filtering process will change accordingly:

[0193]

[0194] like Figure 2 As shown in the figure, we start with the first forward filtering, store the data output by SINS and GNSS at the same time, and continuously estimate the rotation vector error and gyroscope bias at the current moment during the forward filtering process and feed them back to the next moment to ensure that the attitude estimation result is more accurate. After the first forward filtering process is completed, the stored data is processed and used as the data for reverse filtering, and then the above data is reverse filtered. Then the forward filtering and reverse filtering processes are continuously performed to effectively extend the data and improve the alignment speed.

[0195] In order to verify the effectiveness of the algorithm proposed in this embodiment, simulation analysis is performed in MATLAB. The accelerometer constant bias is set to 0.2 mg, and the accelerometer random walk is set to The gyroscope constant bias is set to 0.4° / s, and the gyroscope random walk is set to The SINS sampling frequency is set to 100 Hz, the GNSS receiver sampling frequency is set to 1 Hz, and the speed information error output by the GNSS is set as follows:

[0196]

[0197] 30 seconds of inertial sensor data are used throughout the alignment process, and multiple control groups are set up in the simulation experiment:

[0198] Solution 1: Add the backtracking method to the traditional OBA algorithm's on-the-go alignment method BTOBA (Backtracking OBA).

[0199] Solution 2: High-Accuracy in-motion coarse alignment (HIMCA) is a method that jointly estimates the attitude matrix, gyroscope constant bias, and accelerometer constant bias.

[0200] Solution 3: Use the module value matching method and the indirect in-motion coarse alignment method IICA (indirect in-motion coarse alignment) with adaptive filtering.

[0201] Solution 4: The method proposed in the present invention is an in-motion alignment method MBRIA (Multiple Backtracking Robust In-Motion Alignment) that uses a sliding window, designs relevant gradient values ​​to detect wild values, and uses backtracking filtering to jointly estimate errors.

[0202] First, the four methods are compared when there are no outliers in the velocity information provided by GNSS. Figures 3 to 5 It can be seen that the comparison results of the BTOBA method are divergent. Table 1 shows the root mean square (RMS) statistics of the attitude error from 15 seconds to 30 seconds. The alignment errors of the proposed method in the three axes are all less than 0.8°. Taking the heading angle alignment result as an example, the accuracy of the proposed method is 83.1% higher than that of the HIMCA method and 49.2% higher than that of the IICA method. Figure 6 Indicates the speed information including outlier information. Figures 7 to 9 It can be seen that the alignment results of the proposed method have no fluctuations and remain stable. The alignment results of the BTOBA method cannot converge and cannot complete the on-the-go alignment task. The alignment results of the HIMCA method continued to fluctuate at 30 seconds, indicating that the method has a slow convergence speed and a long alignment cycle. The heading angle error of the IICA method reached 6.7°, the alignment accuracy was low, and the alignment results were unreliable. The proposed MBRIA method showed effective convergence in all three axes, and the final alignment errors were 1.2°, 0.27°, and 0.01°, respectively. As shown in Table 2, compared with the method proposed in the present invention, the alignment accuracy of the other three methods is significantly lower. In contrast, the method proposed in the present invention effectively improves the accuracy and alignment speed of SINS on-the-go alignment.

[0203] Table 1 Statistical characteristics of attitude error when there is no outlier value for 15 to 30 seconds

[0204]

[0205]

[0206] Table 2 Statistical characteristics of attitude error when containing wild values ​​from 15 to 30 seconds

[0207]

Claims

1. A SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering, characterized in that: The following steps are involved: Step 1: Obtain and store the output information of SINS and GNSS in real time, and perform attitude update; Step 2: Detect whether the speed information collected by the GNSS at the current moment is contaminated by outlier information and obtain the detection result; Step 3: Reconstruct the observation vector based on the detection results and the reference vector; Step 4: Based on the reference vector and the reconstructed observation vector, multiple back-tracing filtering method is used to achieve alignment; Among them, in step 1, the posture update described in the above step includes: According to the SINS output information, the following real carrier coordinate system attitude transformation matrix differential equation is solved in real time: Where b(t) represents the real carrier coordinate system at continuous time t, ib represents the carrier coordinate system fixed in the inertial space at the initial moment, represents the attitude transformation matrix from the b system to the ib system at continuous time t, represents the differential form of the attitude transformation matrix of the carrier system under continuous time t, represents the projection of the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system, (·)× represents the antisymmetric matrix of (·); Discretize the continuous time t, and the discretized time is represented by k. Iterate the discretized real carrier system attitude transformation matrix to achieve attitude update: Among them, k is the current moment, k-1 is the previous moment, Obtained by the following formula: Where E3 represents the 3×3 unit matrix, Δθ1 and Δθ2 represent the angular velocity increments at two consecutive moments output by the gyroscope, Represents the rotation vector of the carrier coordinate system from time k-1 to time k; According to the GNSS output information, the following navigation coordinate system attitude transformation matrix differential equation is solved in real time: Among them, n(t) represents the navigation coordinate system at continuous time t, in represents the navigation coordinate system fixed in the inertial coordinate space at the initial moment, represents the attitude transformation matrix from the n system to the in system at continuous time t, represents the differential form of the attitude transformation matrix of the navigation coordinate system under continuous time t, Represents the projection of the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system; Iterate the discretized navigation system attitude transformation matrix to achieve attitude update: in Calculated by the following formula: in, is the rotation vector of the navigation system from time k-1 to time k, T s For the update cycle; Among them, in step 2, the detection of whether the speed information collected by the GNSS at the current moment is contaminated by wild values ​​to obtain the detection result specifically includes: S2_1: Construct the following related gradient values: In the formula, Represents the constant attitude matrix rotating from the in coordinate system to the ib coordinate system, Represents the observation vector containing outlier information, expressed as: Ξ v Indicates outlier information, β v,S represents the observation vector, Represents the attitude transformation matrix of the navigation coordinate system n rotating to the in system; S2_2: If the relevant gradient value exceeds the preset threshold, it means that the speed information collected by the GNSS at the current moment is contaminated by the outlier information; otherwise, it means that the speed information collected by the GNSS at the current moment is not contaminated by the outlier information; Among them, in step 3, the observation vector is reconstructed based on the detection result and the reference vector, and the specific operations include: S3_1: Design the following weight function In the formula, Represents the relevant gradient value, ρ is the preset threshold; S3_2: According to the preset threshold ρ and weight function For the observation vector containing outlier information Reconstruct and obtain the reconstructed observation vector It is expressed as: Wherein, in step 4, the alignment is achieved by using a multiple back-tracing filtering method based on the reference vector and the reconstructed observation vector, and the specific operations include: S4_1: Considering the gyroscope measurement error in SINS, the angular velocity information output by the gyroscope is remodeled and expressed as: in, Indicates the angular velocity information actually output by the gyroscope. It represents the projection of the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system. represents the rotation angular velocity error, b g represents the gyroscope bias, w g represents random noise; The corresponding computing carrier system attitude transformation matrix is ​​expressed as: in, represents the carrier coordinate system obtained by calculation, which is called the calculation carrier system. Represents the differential form of the attitude transformation matrix of the computing carrier system, Indicates from The attitude transformation matrix from the rotation of the system to the ib system; S4_2: Real carrier system attitude transformation matrix And calculate the carrier system attitude transformation matrix There is an error between them, which can be expressed as: in, represents the rotation from b to The attitude transformation matrix of the system, Indicates that the system is calculated from The rotation vector to the real load system b; Combined with the above formula, calculate the load system The differential equation of the rotation vector to the real load system b is expressed as follows: S4_3: Select the coordinate system that causes the calculation carrier The rotation vector of the error with the real carrier coordinate system b and the gyroscope bias b that causes the gyroscope constant error g As a system state variable, it is expressed as: The reconstructed observation vector Arrange and split it, expressed as: In the formula, b(t m ) represents the lower limit time of integration t m The real load system at that time, represents the rotation from b(t) to b(t m )’s attitude transformation matrix, Indicates that from b(t m ) Rotate to the posture transformation matrix of ib; definition According to the chain rule, the expansion have to: Represent the reconstructed observation vector using system state variables It is expressed as: S4_4: Construct the following system measurement equation: Z k =H k X k +V k in: S4_5: Use the SINS and GNSS output data and the attitude transformation matrix at the current moment to solve and estimate the current attitude data, perform forward filtering on the current attitude data, estimate the system state variables at the current moment and compensate at the next moment to complete the forward filtering; S4_6: After the forward filtering is completed, the SINS and GNSS output data are processed, and the attitude transformation matrix equation and velocity update equation of the reverse filtering are used to solve them, and the current attitude data is estimated. The current attitude data is reversely filtered, and the system state variables at the current moment are estimated and compensated at the next moment to complete the reverse filtering; S4_7: Alignment is achieved by continuously performing forward filtering and reverse filtering.

2. A SINS robust on-the-go alignment method comprising outlier detection and multiple backtracking filtering according to claim 1, characterized in that: Before executing step 3, the following steps are also included: At continuous time t, the specific force equation in the navigation coordinate system is expressed as: Among them, V n It represents the velocity vector of the carrier in the navigation coordinate system, It represents the differential of the velocity vector of the carrier in the navigation coordinate system, represents the posture matrix at continuous time t, f b represents the specific force information measured by the accelerometer, e represents the earth coordinate system, represents the projection of the earth's rotation angular velocity in the navigation coordinate system, It represents the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation system, g n Represents the projection of gravity acceleration in the navigation coordinate system; The specific force equation in the above navigation coordinate system is integrated in the time period [0, t] and summarized as follows: Among them, α v is the reference vector, β v is the observation vector, expressed as follows: The sliding window method is used for the reference vector and observation vector constructed above, and the integration interval is changed to [t m ,t],t m For the variable lower limit time of integration, rewrite as follows:

3. A SINS robust on-the-go alignment method comprising outlier detection and multiple back-tracing filtering according to claim 1, characterized in that: The posture transformation matrix equation of the reverse filtering is obtained by inverse deduction based on the discretized forward posture matrix update equation, and is expressed as: in, Represents the reverse process, It represents the attitude transformation matrix from the b system to the n system at time k-1 in the reverse process. It represents the attitude transformation matrix of rotating from the b system to the n system at time k in the reverse process. It represents the actual angular velocity information output by the gyroscope at time k during the reverse process.

4. A SINS robust on-the-go alignment method comprising outlier detection and multiple back-tracing filtering according to claim 1, characterized in that: The velocity update equation of the reverse filter is obtained by inverse derivation based on the discretized forward velocity update equation, which is expressed as: in, Represents the reverse process, It represents the velocity vector of the carrier in the navigation coordinate system at time k during the reverse process, represents the specific force information measured by the accelerometer at time k in the reverse process, It represents the projection of the Earth's rotation angular velocity at time k in the reverse process in the navigation coordinate system, It represents the projection of the angular velocity of the navigation coordinate system at time k relative to the earth coordinate system in the navigation system during the reverse process.

5. The SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering according to claim 1, characterized in that: After the forward filtering is completed, the SINS and GNSS output data are processed, and the specific operations include: Each time the forward filtering and the reverse filtering are switched, the corresponding data is processed, and the data of the reverse process and the data of the forward process are jointly expressed as: in, represents the velocity information at time k in the reverse process, V n (Nk) represents the speed information at time Nk in the forward process, represents the specific force information at time k in the reverse process, f b (Nk) represents the specific force information at time Nk in the forward process, represents the angular velocity information at time k in the reverse process, Indicates the angular velocity information at time Nk during the forward process.

6. A SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering according to claim 1, characterized in that: The initial value of the reverse filter is set to:

7. A SINS robust on-the-go alignment method including outlier detection and multiple back-tracing filtering according to claim 1, characterized in that: The estimating of the system state variables at the current moment and compensating at the next moment specifically includes: biasing the gyroscope Compensate for the angular velocity of the retrace process Update the attitude transformation matrix in the real carrier coordinate system: In the formula, It represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system at time k in the reverse process in the carrier coordinate system. Indicates the actual angular velocity information output by the gyroscope at time Nk during the forward process.

8. A SINS robust on-the-go alignment method including outlier detection and multiple backtracking filtering according to claim 1, characterized in that: During the reverse filtering process, the system state variables are expressed as:

Images