A SINS robust on-the-go alignment method including wild value detection and multiple backtracking filtering

Abstract

The application discloses a SINS robust in-motion alignment method comprising wild value detection and multiple backtracking filtering, comprising the following steps: acquiring and storing the output information of SINS and GNSS in real time, and performing attitude updating; detecting whether the speed information collected by GNSS at the current time is polluted by wild value information, and obtaining a detection result; reconstructing an observation vector according to the detection result and a reference vector; and based on the reference vector and the reconstructed observation vector, using a multiple backtracking filtering method to realize alignment; the application uses a related gradient value design method to isolate the wild value of GNSS output, and continuously corrects the related error in the multiple backtracking filtering process, thereby effectively improving the alignment result reliability and accelerating the alignment speed.

Description

Technical Field

[0001] This invention belongs to the field of strapdown inertial navigation system alignment technology, specifically relating to a robust in-flight alignment method for SINS that includes outlier detection and multiple backtracking filtering. Background Technology

[0002] Strapdown Inertial Navigation System (SINS) is an autonomous navigation system that uses gyroscopes and accelerometers to measure the angular velocity and acceleration of a moving object in real time, thereby calculating the position, velocity, and attitude information of the vehicle. SINS is characterized by high precision and real-time performance and does not rely on external signals, making it widely used in aerospace, marine exploration, and vehicle navigation. Before navigation, SINS requires initial alignment, the accuracy of which is crucial for subsequent navigation. Due to the cumulative effect of SINS errors, any errors incurred during the initial alignment phase will accumulate rapidly over time, leading to a continuous increase in navigation errors. Initial alignment presents a trade-off between alignment time and alignment accuracy. Existing in-flight alignment techniques mainly employ optimization-based alignment (OBA) methods. This approach requires accurate Global Navigation Satellite System (GNSS) data, free of outliers, to assist SINS in alignment. Alignment times often exceed 300 seconds, which may be unsuitable for scenarios requiring rapid alignment in interference-prone environments.

[0003] Traditional in-the-move alignment methods often process SINS data only once in the forward direction, resulting in low data utilization. This makes them unsuitable for situations requiring rapid initial alignment (e.g., within 30 seconds) with SINS. Furthermore, traditional initial alignment algorithms assume complete accuracy of GNSS output, neglecting the influence of outliers. However, due to multipath effects, urban high-rises, and dense forests, GNSS output often contains outliers. In such cases, the performance of traditional initial alignment algorithms is severely impacted, potentially rendering them ineffective.

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

[0005] Purpose of the invention: To address the problems of low alignment accuracy, slow alignment speed, and poor reliability of alignment results when existing in-flight alignment techniques are used in SINS (Short-line In-flight System) applications requiring rapid startup, this invention proposes a robust in-flight alignment method for SINS that includes outlier detection and multiple backtracking filtering. By proposing a design method for relevant gradient values ​​to isolate outliers in the GNSS output, and continuously correcting relevant errors during the multiple backtracking filtering process, the reliability of alignment results is effectively improved, and the alignment speed is accelerated.

[0006] Technical solution: A robust SINS (Simplified In-Mile Attachment) alignment method incorporating outlier detection and multiple backtracking filtering, comprising the following steps:

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

[0008] Step 2: Detect whether the velocity information acquired by 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, a multiple backtracking filtering method is used to achieve alignment;

[0011] In step 1, the pose update specifically includes the following operations:

[0012] Based on the SINS output information, the differential equation of the attitude transformation matrix of the real vehicle coordinate system is solved in real time:

[0013]

[0014] Where b(t) represents the real carrier coordinate system at continuous time t, and ib represents the carrier coordinate system fixed in inertial space at the initial moment. Let represent the attitude transformation matrix from frame b to frame ib over a continuous time t. Let represent the differential form of the attitude transformation matrix of the system under continuous time t. represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system, and (·)× represents the antisymmetric matrix of (·).

[0015] Discretize the continuous time t, denoted by k, and iteratively calculate the attitude transformation matrix of the discretized real vehicle system to achieve attitude update:

[0016]

[0017] Where k represents the current time, and k-1 represents the previous time. The following formula can be used to obtain:

[0018]

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

[0020] Based on GNSS output information, the differential equations of the attitude transformation matrix of the following navigation coordinate system are solved in real time:

[0021]

[0022] Where n(t) represents the navigation coordinate system at continuous time t, and in represents the navigation coordinate system initially fixed in the inertial coordinate space. Let represent the attitude transformation matrix from the n-frame to the in-frame over a continuous time t. Let represent the differential form of the attitude transformation matrix of the navigation coordinate system over continuous time t. This represents the projection of the rotational angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the navigation coordinate system.

[0023] The attitude change is updated by iteratively calculating the discretized navigation frame attitude transformation matrix.

[0024]

[0025] in Calculated using the following formula:

[0026]

[0027] in, Let T be the rotation vector of the navigation frame from time k-1 to time k. s For update cycle;

[0028] In step 2, the specific operation of detecting whether the velocity information acquired by GNSS at the current time is contaminated by outliers and obtaining the detection result includes:

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

[0030]

[0031] In the formula, This represents the constant pose matrix after rotation from the in coordinate system to the ib coordinate system. The observation vector containing outlier information is represented as: Ξ v Represents outlier information, β ν,S Represents the observation vector. This represents the attitude transformation matrix when the navigation coordinate system n is rotated to the in coordinate system;

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

[0033] In step 3, the reconstructing of the observation vector based on the detection results and the reference vector specifically includes the following operations:

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

[0035]

[0036] In the formula, θ k This represents the relevant gradient value, where ρ is a preset threshold.

[0037] S3_2: Based on the preset threshold ρ and weighting function Υ(θ) k ) Observation vectors containing outlier information Reconstruction is performed to obtain the reconstructed observation vector. Represented as:

[0038]

[0039] In step 4, the alignment is achieved using a multiple backtracking filtering method based on the reference vector and the reconstructed observation vector. 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 follows:

[0041]

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

[0043] The corresponding computational attitude transformation matrix of the carrier system is expressed as:

[0044]

[0045] in, The coordinate system of the carrier obtained through calculation is called the computational carrier system. This represents the differential form of the attitude transformation matrix of the computational system. Indicates from The attitude transformation matrix from the ib frame to the ib frame;

[0046] S4_2: Actual vehicle attitude transformation matrix With the calculation of the attitude transformation matrix of the vehicle system There is an error, expressed as:

[0047]

[0048] in, Indicates rotation from the b system to The attitude transformation matrix of the system. Indicates from the computational load system The rotation vector to the real load system b;

[0049] Combining the above formula, calculate the load system. The rotational vector differential equation for the real system b is expressed as follows:

[0050]

[0051] S4_3: Select the coordinate system that causes the computational vehicle Rotation vector of error with the real carrier coordinate system b And the gyroscope bias b that causes constant error in the gyroscope g As a system state variable, it is represented as:

[0052]

[0053] For the reconstructed observation vector The process of organizing and splitting the data is as follows:

[0054]

[0055] In the formula, b(t) m ) represents the time t of the lower limit of integration. m The real-time system, This indicates a rotation from b(t) to b(t). m The attitude transformation matrix, Indicates from b(t) m The pose transformation matrix for rotating to ib;

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

[0057]

[0058] The reconstructed observation vector is represented by system state variables. Represented as:

[0059]

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

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

[0062] in:

[0063]

[0064] S4_5: Solve using SINS and GNSS output data and the attitude transformation matrix at the current moment to estimate the current attitude data, perform forward filtering on the current attitude data, estimate the system state variables at the current moment and compensate for them at the next moment, thus completing the forward filtering;

[0065] S4_6: After the forward filtering is completed, the SINS and GNSS output data are processed. The attitude transformation matrix equation and velocity update equation of the inverse filtering are used to solve the equation and estimate the current attitude data. The current attitude data is then subjected to inverse filtering to estimate the system state variables at the current moment and to compensate for them at the next moment, thus completing the inverse filtering.

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

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

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

[0069]

[0070] Among them, V n The velocity vector of the downloaded volume in the navigation coordinate system. The derivative of the velocity vector of the download volume in the navigation coordinate system. Let f represent the attitude matrix at continuous time t. b This indicates the specific force information measured by the accelerometer, where 'e' represents the Earth coordinate system. This represents the projection of the Earth's rotational angular velocity onto the navigation coordinate system. The projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system onto the navigation frame, g n This represents the projection of gravitational acceleration onto the navigation coordinate system.

[0071] Integrating the specific force equation in the above navigation coordinate system over the time interval [0, t], the result is as follows:

[0072]

[0073] Where, α v As the reference vector, β v The observation vector is represented as follows:

[0074]

[0075] Using the sliding window method on the constructed reference vector and observation vector, the integration interval is changed to [t]. m ,t],t m To give a variable lower bound on integration time, it is rewritten as follows:

[0076]

[0077] Furthermore, the attitude transformation matrix equation for the inverse filtering is derived from the discretized forward attitude matrix update equation, and is expressed as:

[0078]

[0079] in, Indicates the reverse process. Let represent the attitude transformation matrix at time k-1 from frame b to frame n during the reverse process. Let represent the attitude transformation matrix at time k from frame b to frame n during the reverse process. This represents the actual angular velocity information output by the gyroscope at time k during the reverse process.

[0080] Furthermore, the velocity update equation for the inverse filter is derived from the discretized forward velocity update equation, and is expressed as:

[0081]

[0082] in, Indicates the reverse process. This represents the velocity vector of the navigation coordinate system at time k during the reverse process. This represents the specific force information measured by the accelerometer at time k during the reverse process. This represents the projection of the Earth's rotational angular velocity at time k during the reverse process onto the navigation coordinate system. This represents the projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system at time k during the reverse process onto the navigation system.

[0083] Furthermore, after the forward filtering is completed, the SINS and GNSS output data are processed, specifically including:

[0084] Each switch between forward and reverse filtering processes the corresponding data, combining the data from the reverse and forward processes into a single representation:

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

[0086] Furthermore, the initial value for the inverse filter is set as follows:

[0087] Furthermore, the estimation of the system state variables at the current moment and the compensation at the next moment specifically includes: biasing the gyroscope by b. g,k Angular velocity during the compensation backtracking process Update the attitude transformation matrix in the real vehicle coordinate system:

[0088]

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

[0090] Furthermore, during the inverse filtering process, the system state variables are represented 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 which uses a full integral form to construct vectors, the method of this invention uses a sliding interval method to construct vectors, which isolates the vectors from the alignment start to t. k-mThe error in the measurement information during this time interval reduces the accumulation of errors in inertial devices;

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

[0095] (3) The method of the present invention uses a multiple backtracking method for alignment. By employing multiple forward and reverse filtering processes, it continuously compensates for the equivalent rotation vector error between the calculated load system and the ideal load system, as well as the constant bias of the gyroscope, thereby improving data utilization, effectively shortening the alignment time, and making the alignment results more reliable.

[0096] (4) The traditional single backtracking alignment method only performs one forward and reverse filtering process. Compared with the traditional single backtracking alignment method, the method of the present invention can further improve the data utilization rate, realize more full mining and use of data, and further shorten the alignment time, and obtain higher alignment accuracy in a short time. Attached Figure Description

[0097] Figure 1 A flowchart of a robust in-journey alignment method for SINS that includes outlier detection and multiple backtracking filtering;

[0098] Figure 2 Here is a flowchart of the multiple backtracking filtering method;

[0099] Figure 3 Schematic diagram of heading angle error when there are no outliers;

[0100] Figure 4 A schematic diagram of roll angle error when there are no outliers;

[0101] Figure 5 A schematic diagram of pitch angle error when there are no outliers;

[0102] Figure 6 This is a schematic diagram of velocity information containing outliers;

[0103] Figure 7 This is a schematic diagram of the heading angle error when including outliers;

[0104] Figure 8 This is a schematic diagram of roll angle error when outliers are included;

[0105] Figure 9 This is a schematic diagram of pitch angle error when outliers are included. Detailed Implementation

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

[0107] Example 1:

[0108] like Figure 1 As shown, this embodiment proposes a robust SINS (Simplified In-Mile Attachment) alignment method that includes outlier detection and multiple backtracking filtering, mainly comprising the following steps:

[0109] Step 1: Based on the SINS and GNSS output information, and combined with the differential equations of the real-time attitude transformation matrices of the carrier system and the navigation system, solve for the real-time attitude transformation matrices of the carrier system and the navigation system; specific operations include:

[0110] Based on the SINS output information, the differential equations of the attitude transformation matrix of the following real-world vehicle system are solved in real time:

[0111]

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

[0113] The theoretical derivation is performed in continuous time t, but since navigation computers can only process discrete values, the continuous time t is discretized, and the discretized time is represented by k. The attitude transformation matrix of the discretized real vehicle system is then iteratively calculated:

[0114]

[0115] Where k represents the current time, and k-1 represents the previous time. The following formula can be used to obtain:

[0116]

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

[0118] Based on GNSS output information, the differential equations of the attitude transformation matrix of the following navigation coordinate system are solved in real time:

[0119]

[0120] Where n(t) represents the navigation coordinate system at continuous time t, and in represents the navigation coordinate system initially fixed in the inertial coordinate space. Let represent the attitude transformation matrix from the n-frame to the in-frame over a continuous time t. Let represent the differential form of the attitude transformation matrix of the navigation coordinate system over continuous time t. This represents the projection of the rotational angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the navigation coordinate system.

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

[0122]

[0123] in Calculated using the following formula:

[0124]

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

[0126] Step 2: Integrate the transformed navigation system specific force equation using the sliding interval method to construct the improved reference vector and observation vector; 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 The velocity vector of the downloaded volume in the navigation coordinate system. The derivative of the velocity vector of the download volume in the navigation coordinate system. Let f represent the attitude matrix at continuous time t. b This indicates the specific force information measured by the accelerometer, where 'e' represents the Earth coordinate system. This represents the projection of the Earth's rotational angular velocity onto the navigation coordinate system. The projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system onto the navigation frame, g n This represents the projection of gravitational acceleration onto the navigation coordinate system.

[0130] Integrating the specific force equation in the above navigation coordinate system over the time interval [0, t], the result is as follows:

[0131]

[0132] in, Let α represent the constant attitude matrix rotated from the in coordinate system to the ib coordinate system. v As the reference vector, β v The observation vector is represented as follows:

[0133]

[0134] To suppress the accumulation of systematic errors during integration, a sliding window method is used for the constructed vector, changing the integration interval to [t]. m ,t],t m For a variable lower bound time for integration, ensure the integration window size tt. m As a constant value, it can be rewritten as follows:

[0135]

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

[0137] Considering the case where GNSS output contains outliers, the velocity information of GNSS output in complex environments is remodeled:

[0138]

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

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

[0141]

[0142] Reference vector α v,s The construction of the gradient is only related to the specific force information output by the accelerometer and is not affected by outliers in the GNSS output, which is very useful for detecting whether the observation vector contains outliers. Therefore, the following related gradient values ​​are constructed:

[0143]

[0144] When the GNSS output information is accurate and does not contain outliers, the observation vector Strictly equal to a constant attitude matrix With reference vector α v,SThe product of the two terms is used to construct the relevant gradient value, which is then subtracted modulo-1 from each term to illustrate the actual difference between the two terms, i.e., the observed vector. The strength of the wild values ​​included.

[0145] If the above-mentioned gradient values ​​exceed a reasonable range, the velocity information output by the GNSS is considered to be contaminated by outliers. Based on this, a weighting function is designed to construct an improved observation vector. When the outlier intensity is too large, the relevant gradient value θ k The corresponding value increases, while the observed vector decreases. The weight of the reference vector α is increased. ν,S The weights, the weight function Υ(θ) k The following is represented:

[0146]

[0147] Based on the preset threshold ρ and weighting function Υ(θ) k The observation vector containing outlier information is reconstructed to derive an improved observation vector. It is expressed as follows:

[0148]

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

[0150]

[0151] in,

[0152]

[0153] Calculate the fourth-order attitude determination matrix S k The eigenvector corresponding to the smallest eigenvalue is the quaternion form of the constant attitude matrix. Converting the obtained quaternion into matrix form yields the constant attitude matrix.

[0154] Step 4: Construct a system model and perform online compensation for the equivalent rotation vector error and gyroscope bias between the calculated and ideal load systems; specific operations include:

[0155] Considering gyroscope measurement errors, the angular velocity information output by the gyroscope is remodeled:

[0156]

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

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

[0159]

[0160] in, The coordinate system of the carrier obtained through calculation is called the calculated carrier system, and b is the actual carrier coordinate system.

[0161] Real vehicle attitude transformation matrix With the calculation of the attitude transformation matrix of the vehicle system There is an error between them, and the calculation process is as follows:

[0162]

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

[0164] Combining the above formula, calculate the load system. The rotational vector differential equation for the real system b is expressed as follows:

[0165]

[0166] Based on the above derivation, the coordinate system of the computational vehicle is selected. Rotation vector of error with the real carrier coordinate system b And the gyroscope bias b that causes constant error in the gyroscope g As a system state variable, it is represented as follows:

[0167]

[0168] For the improved observation vector The breakdown is as follows:

[0169]

[0170] In the formula, b(t) m ) represents the time t of the lower limit of integration. mThe real-time system, This indicates a rotation from b(t) to b(t). m The attitude transformation matrix, Indicates from b(t) m The attitude transformation matrix for rotating to ib.

[0171] definition According to the chain rule, expand

[0172]

[0173] Representing the improved observation vector using system state variables. It is expressed as follows:

[0174]

[0175] In the formula, The time t represents the lower limit of integration. m The computing system at that time Indicates from The pose transformation matrix for rotating to ib.

[0176] The system measurement equation is constructed using the above formula, and is expressed as follows:

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

[0178] in,

[0179]

[0180] Step 5: Perform the first forward filtering to estimate the error at the current time and compensate for it at the next time. After completing the forward filtering, process the SINS and GNSS data and then begin the reverse filtering. Derive the reverse filtering state equation and compensate for the updated error state variables to achieve iterative correction of related errors using the multiple backtracking filtering method. Specific operations include:

[0181] The attitude matrix equation for the reverse process is derived by inversely using the forward attitude matrix update equation at discrete time points:

[0182]

[0183] in, Indicates the reverse process. Let represent the attitude transformation matrix at time k-1 from frame b to frame n during the reverse process. Let represent the attitude transformation matrix at time k from frame b to frame n during the reverse process. This represents the actual angular velocity information output by the gyroscope at time k during the reverse process.

[0184] The velocity update equation in the reverse process is derived by inversely using the forward velocity update equation at discrete moments:

[0185]

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

[0187] Before implementing the multiple backtracking filtering method, the stored data needs to be processed. Assuming the data length collected by SINS is N, where N is the last moment in the forward process, each switch between forward and reverse filtering requires processing the corresponding data. The data from the reverse process and the data from the forward process are represented together as follows: in, V represents the velocity information at time k during the reverse process. n (Nk) represents the velocity information at time Nk during the forward process. f represents the relative force information at time k during the reverse process. b (Nk) represents the force information at time Nk during the forward process. This indicates the angular velocity information at time k during the reverse process. This represents the angular velocity information at time Nk during the forward process.

[0188] In the multiple backtracking filtering method, the initial values ​​for the inverse filtering process need to be set separately, as follows: That is, the constant attitude matrix in the reverse process is equal to the attitude transformation matrix from the b frame to the n frame at the last moment in the forward process, and the initial velocity in the reverse process is equal to the negative of the velocity at the last moment in the forward process.

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

[0190]

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

[0192] During the inverse filtering process, the state variables will change accordingly because the data has been processed:

[0193]

[0194] like Figure 2 As shown, the process begins with the first forward filtering, simultaneously storing data from both SINS and GNSS outputs. During the forward filtering process, the rotation vector error and gyroscope bias at the current moment are continuously estimated and fed back to the next moment, ensuring more accurate attitude estimation results. After the first forward filtering process, the stored data is processed and used as data for inverse filtering, which is then subjected to inverse filtering again. This process of forward and inverse filtering is repeated continuously, effectively extending the data range and improving alignment speed.

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

[0196]

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

[0198] Option 1: Add the backtracking method to the traditional OBA algorithm's in-journey alignment method BTOBA (BacktrackingOBA).

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

[0200] Option 3: The indirect in-motion coarse alignment method (IICA) is adopted, which combines modulus matching and adaptive filtering.

[0201] Option 4: The method proposed in this invention is the MBRIA (Multiple Backtracking Robust In-Motion Alignment) method, which uses a sliding window, designs relevant gradient values ​​to detect outliers, and uses backtracking filtering to jointly estimate errors.

[0202] First, assuming no outliers exist in the velocity information provided by GNSS, the four methods are compared. Figures 3 to 5 It can be seen that the alignment results of the BTOBA method are divergent. Table 1 shows the root mean square (RMS) statistics of the attitude error from 15 to 30 seconds. The proposed method has an alignment error of less than 0.8° in all three axes. Taking the heading angle alignment results 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 This indicates velocity information that includes outlier information. From... Figures 7 to 9 As can be seen, the alignment results of the proposed method are stable and do not fluctuate. The BTOBA method, however, fails to converge and cannot complete the in-journey alignment task. The HIMCA method shows continuous fluctuations in alignment results after 30 seconds, indicating slow convergence and a long alignment cycle. The IICA method has a heading angle error of 6.7°, resulting in low alignment accuracy and unreliable alignment results. The proposed MBRIA method demonstrates effective convergence in all three axes, with final alignment errors of 1.2°, 0.27°, and 0.01°, respectively. As shown in Table 2, compared to the method proposed in this invention, the other three methods have significantly lower alignment accuracy. In contrast, the method proposed in this invention effectively improves the accuracy and speed of SINS in-journey alignment.

[0203] Table 1. Statistical characteristics of attitude error during 15–30 s without outliers

[0204]

[0205]

[0206] Table 2. Statistical characteristics of attitude error including outliers from 15 to 30 seconds.

[0207]

Claims

1. A robust SINS (Simplified In-Mile Attachment) alignment method incorporating outlier detection and multiple backtracking filtering, characterized in that: Includes the following steps: Step 1: Acquire and store the output information of SINS and GNSS in real time, and perform attitude updates; Step 2: Detect whether the velocity information acquired by 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, a multiple backtracking filtering method is used to achieve alignment; In step 1, the pose update specifically includes the following operations: Based on the SINS output information, the differential equation of the attitude transformation matrix of the real vehicle coordinate system is solved in real time: Where b(t) represents the real carrier coordinate system at continuous time t, and ib represents the carrier coordinate system fixed in inertial space at the initial moment. Let represent the attitude transformation matrix from frame b to frame ib over a continuous time t. Let represent the differential form of the attitude transformation matrix of the system under continuous time t. represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system, and (·)× represents the antisymmetric matrix of (·). Discretize the continuous time t, denoted by k, and iteratively calculate the attitude transformation matrix of the discretized real vehicle system to achieve attitude update: Where k represents the current time, and k-1 represents the previous time. The following formula can be used to obtain: Where E3 represents a 3×3 identity matrix, and Δθ1 and Δθ2 represent the angular velocity increments at two consecutive moments in the gyroscope output. This represents the rotation vector of the carrier coordinate system from time k-1 to time k; Based on GNSS output information, the differential equations of the attitude transformation matrix of the following navigation coordinate system are solved in real time: Where n(t) represents the navigation coordinate system at continuous time t, and in represents the navigation coordinate system initially fixed in the inertial coordinate space. Let represent the attitude transformation matrix from the n-frame to the in-frame over a continuous time t. Let represent the differential form of the attitude transformation matrix of the navigation coordinate system over continuous time t. This represents the projection of the rotational angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the navigation coordinate system. The attitude change is updated by iteratively calculating the discretized navigation frame attitude transformation matrix. in Calculated using the following formula: in, Let T be the rotation vector of the navigation frame from time k-1 to time k. s For update cycle; In step 2, the specific operation of detecting whether the velocity information acquired by GNSS at the current time is contaminated by outliers and obtaining the detection result includes: S2_1: Construct the following related gradient values: In the formula, This represents the constant pose matrix after rotation from the in coordinate system to the ib coordinate system. The observation vector containing outlier information is represented as: Ξ v Represents outlier information, β v,S Represents the observation vector. The attitude transformation matrix represents the rotation from the navigation coordinate system n to the in coordinate system; α v,s Indicates the reference vector; S2_2: If the relevant gradient value exceeds the preset threshold, it means that the velocity information collected by GNSS at the current time is contaminated by outlier information; otherwise, it means that the velocity information collected by GNSS at the current time is not contaminated by outlier information. In step 3, the reconstructing of the observation vector based on the detection results and the reference vector specifically includes the following operations: S3_1: Design the following weight function γ(θ) k ): In the formula, θ k This represents the relevant gradient value, where ρ is a preset threshold. S3_2: Based on the preset threshold ρ and weighting function γ(θ) k ) Observation vectors containing outlier information Reconstruction is performed to obtain the reconstructed observation vector. Represented as: In step 4, the alignment is achieved using a multiple backtracking filtering method based on the reference vector and the reconstructed observation vector. 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 follows: in, This represents the actual angular velocity information output by the gyroscope. This represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system. Indicates the rotational angular velocity error, b g Indicates gyroscope bias, w g Indicates random noise; The corresponding computational attitude transformation matrix of the carrier system is expressed as: in, The coordinate system of the carrier obtained through calculation is called the computational carrier system. This represents the differential form of the attitude transformation matrix of the computational system. Indicates from The attitude transformation matrix from the ib frame to the ib frame; S4_2: Actual vehicle attitude transformation matrix With the calculation of the attitude transformation matrix of the vehicle system There is an error, expressed as: in, Indicates rotation from the b system to The attitude transformation matrix of the system. Indicates from the computational load system The rotation vector to the real load system b; Combining the above formula, calculate the load system. The rotational vector differential equation for the real system b is expressed as follows: S4_3: Select the coordinate system that causes the computational vehicle Rotation vector of error with the real carrier coordinate system b And the gyroscope bias b that causes constant error in the gyroscope g As a system state variable, it is represented as: For the reconstructed observation vector The process of organizing and splitting the data is as follows: In the formula, b(t) m ) represents the time t of the lower limit of integration. m The real-time system, This indicates a rotation from b(t) to b(t). m The attitude transformation matrix, Indicates from b(t) m The pose transformation matrix for rotating to ib; f b This indicates the specific force information measured by the accelerometer; definition According to the chain rule, expand have to: The reconstructed observation vector is represented by system state variables. Represented as: S4_4: Construct the following system measurement equations: Z k =H k X k +V k in: S4_5: Solve using SINS and GNSS output data and the attitude transformation matrix at the current moment to estimate the current attitude data, perform forward filtering on the current attitude data, estimate the system state variables at the current moment and compensate for them at the next moment, thus completing the forward filtering; S4_6: After the forward filtering is completed, the SINS and GNSS output data are processed. The attitude transformation matrix equation and velocity update equation of the inverse filtering are used to solve the equation and estimate the current attitude data. The current attitude data is then subjected to inverse filtering to estimate the system state variables at the current moment and to compensate for them at the next moment, thus completing the inverse filtering. S4_7: Alignment is achieved by continuously performing forward and reverse filtering.

2. The SINS robust in-journey alignment method comprising outlier detection and multiple backtracking filtering according to claim 1, characterized in that: Before performing step 3, the following steps are also included: In continuous time t, the specific force equation in the navigation coordinate system is expressed as: Among them, V n The velocity vector of the downloaded volume in the navigation coordinate system. The derivative of the velocity vector of the download volume in the navigation coordinate system. Let f represent the attitude matrix at continuous time t. b This indicates the specific force information measured by the accelerometer, where 'e' represents the Earth coordinate system. This represents the projection of the Earth's rotational angular velocity onto the navigation coordinate system. The projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system onto the navigation frame, g n This represents the projection of gravitational acceleration onto the navigation coordinate system. Integrating the specific force equation in the above navigation coordinate system over the time interval [0, t], the result is as follows: Where, α v As the reference vector, β v The observation vector is represented as follows: Using the sliding window method on the constructed reference vector and observation vector, the integration interval is changed to [t]. m ,t],t m To give a variable lower bound on integration time, it is rewritten as follows:

3. The SINS robust in-journey alignment method according to claim 2, comprising outlier detection and multiple backtracking filtering, is characterized in that: The attitude transformation matrix equation for the inverse filtering is derived from the discretized forward attitude matrix update equation, and is expressed as: in, Indicates the reverse process. Let represent the attitude transformation matrix at time k-1 from frame b to frame n during the reverse process. Let represent the attitude transformation matrix at time k from frame b to frame n during the reverse process. This represents the actual angular velocity information output by the gyroscope at time k during the reverse process.

4. The SINS robust in-journey alignment method including outlier detection and multiple backtracking filtering according to claim 3, characterized in that: The velocity update equation for the inverse filter is derived by reversing the discretized forward velocity update equation, and is expressed as: in, Indicates the reverse process. This represents the velocity vector of the navigation coordinate system at time k during the reverse process. This represents the specific force information measured by the accelerometer at time k during the reverse process. This represents the projection of the Earth's rotational angular velocity at time k during the reverse process onto the navigation coordinate system. This represents the projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system at time k during the reverse process onto the navigation system.

5. The SINS robust in-journey alignment method according to claim 1, comprising outlier detection and multiple backtracking filtering, is characterized in that: After the forward filtering is completed, the SINS and GNSS output data are processed, including the following operations: Each switch between forward and reverse filtering processes the corresponding data, combining the data from the reverse and forward processes into a single representation: in, V represents the velocity information at time k during the reverse process. n (Nk) represents the velocity information at time Nk during the forward process. f represents the relative force information at time k during the reverse process. b (Nk) represents the force information at time Nk during the forward process. This indicates the angular velocity information at time k during the reverse process. This represents the angular velocity information at time Nk during the forward process.

6. The SINS robust in-journey alignment method according to claim 1, comprising outlier detection and multiple backtracking filtering, is characterized in that: The initial values ​​for the back filter are set as follows: the constant attitude matrix during the back process is equal to the attitude transformation matrix from the b-frame to the n-frame at the last moment during the forward process, and the initial velocity during the back process is equal to the negative of the velocity at the last moment during the forward process.

7. The SINS robust in-journey alignment method comprising outlier detection and multiple backtracking filtering according to claim 1, characterized in that: The estimation of the system state variables at the current moment and the compensation at the next moment specifically includes: biasing the gyroscope by b. g,k Angular velocity during the compensation backtracking process Update the attitude transformation matrix in the real vehicle coordinate system: In the formula, This represents the projection of the rotational angular velocity of the carrier coordinate system relative to the inertial coordinate system at time k during the reverse process onto the carrier coordinate system. This represents the actual angular velocity information output by the gyroscope at time Nk during the forward process.

8. The SINS robust in-journey alignment method according to claim 1, comprising outlier detection and multiple backtracking filtering, is characterized in that: During the inverse filtering process, the system state variables are represented as follows:

Images