Initial alignment method of moving base of integrated navigation system and related device

Through the combination of Kalman filtering and robust weight function, the problems of IMU zero bias and GNSS field value in the combined SINS and GNSS navigation system are solved, which improves the accuracy and speed of the initial alignment of the moving base, and achieves a more stable Carrier pose matrix estimation.

CN118913320APending Publication Date: 2024-11-08XI AN JIAOTONG UNIV +1
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411236114.6
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-04
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

The existing combined navigation system of SINS and Global Navigation Satellite System (GNSS) ignores the zero-biased cumulative error of inertial measurement elements (IMU) and the GNSS receiver is susceptible to external use during the initial alignment of the moving base. The field value problems caused by interference affect the positioning accuracy and convergence speed.

Method used

Using the Kalman filtering method based on error-free observation vector model and reference vector model, the prediction model of real state and error state is established by combining IMU output and GNSS output, and a robust weight function is constructed to suppress IMU zero bias and GNSS field value. , optimize the direction cosine matrix of the initial navigation system and carrier system to realize the determination of the real-time carrier attitude matrix.

Benefits of technology

Effectively suppress the IMU zero-biased cumulative error and GNSS field value, improve the initial alignment accuracy and convergence speed of the dynamic base of the combined navigation system, and enhance the robustness of the system.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118913320A_ABST
    Figure CN118913320A_ABST
Patent Text Reader

Abstract

The invention belongs to the field of strapdown inertial navigation systems, and discloses an initial alignment method of a moving base of an integrated navigation system and a related device.The method comprises the steps that firstly, a state prediction model and a state measurement model of a real state are constructed, and a prediction model and a measurement model of an error state and a prediction model of a nominal state are obtained; receiving IMU output to carry out priori estimation of an error state, an error state covariance matrix and a nominal state; gNSS output is received to update a direction cosine matrix from an initial navigation system to a navigation system, and an observation vector is constructed; building a robust weight function through expansion module value matching; then constructing a robust attitude determination matrix, and solving a nominal direction cosine matrix from the initial navigation system to the initial carrier system through an optimization method; carrying out posteriori on the error state and the error state covariance matrix and posteriori estimation of an estimated nominal state, and combining posteriori estimation of a direction cosine matrix from the initial carrier system to the carrier system, a nominal direction cosine matrix from the initial navigation system to the initial carrier system and a direction cosine matrix from the initial navigation system to the navigation system; the real-time carrier attitude matrix is obtained to control the initial alignment of the moving base of the integrated navigation system, the IMU zero offset accumulative error can be inhibited, the outliers in the output of the GNSS receiver can be detected and removed, and the precision and convergence speed of the initial alignment of the moving base of the integrated navigation system are further effectively improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of strapdown inertial navigation systems, and relates to a moving base initial alignment method of an integrated navigation system and a related device. Background Art

[0002] Strapdown Inertial Navigation System (SINS) has superior autonomy and is widely used in robot positioning. However, if SINS works alone, it will produce a large cumulative error, which will affect the positioning accuracy. Using the Global Navigation Satellite System (GNSS) to assist SINS for integrated navigation can suppress the SINS cumulative error and thus improve the positioning accuracy. Initial alignment is required before SINS and GNSS are combined for navigation.

[0003] However, the current dynamic base initial alignment method of the SINS and GNSS integrated navigation system ignores two problems. One is the accumulated bias error of the SINS inertial measurement unit (IMU) during the alignment process, and the other is that the GNSS receiver is susceptible to external interference such as obstacles and multipath effects, resulting in wild values ​​in the measurement. These two problems seriously affect the convergence speed and accuracy of the initial alignment. Summary of the invention

[0004] The purpose of the present invention is to overcome the above-mentioned shortcomings of the prior art and provide a method for initial alignment of a moving base of an integrated navigation system and related devices.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions:

[0006] In a first aspect, the present invention provides a method for initial alignment of a moving base of an integrated navigation system, comprising:

[0007] Based on the error-free observation vector model and the reference vector model, the IMU output is modeled and the reference vector, the direction cosine matrix from the initial load system to the load system and the IMU measurement zero bias are used as state variables to establish the state prediction model and state measurement model of the real state. According to the relationship between the real state, the error state and the nominal state, the error state prediction model and measurement model, as well as the nominal state prediction model are obtained.

[0008] Receive the IMU output, and obtain the error state, the error state covariance matrix and the prior estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until a new frame of GNSS output is received; and when a new frame of GNSS output is received, update the direction cosine matrix from the initial navigation system to the navigation system according to the GNSS output, and construct an observation vector;

[0009] Model the GNSS output and construct a robust weight function through extended modulus matching based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector, and the observation vector measurement value;

[0010] The robust attitude determination matrix is ​​constructed by using the robust weight function, the measured value of the observation vector and the prior estimate of the nominal reference vector, and the nominal direction cosine matrix from the initial navigation system to the initial load system is solved by the optimization method according to the robust attitude determination matrix;

[0011] According to the observation model of the error state, the error state and the error state covariance matrix are estimated a posteriori through the Kalman filter measurement update formula and the robust weight function, and the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state to complete the a posteriori estimate of the nominal state; and the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system are combined to obtain the real-time carrier attitude matrix;

[0012] The initial alignment of the moving base is controlled according to the real-time carrier attitude matrix.

[0013] Optionally, the error-free observation vector model and the reference vector model are derived based on the specific force equation and the chain rule of the direction cosine matrix; according to the specific force equation and the chain rule of the direction cosine matrix, the following is obtained:

[0014]

[0015] in, represents the differential of the navigation system velocity; Represents the direction cosine matrix from the navigation coordinate system to the initial navigation system; Represents the direction cosine matrix from the initial navigation system to the initial load system; represents the direction cosine matrix from the initial load system to the load system; f b Represents the true value of the accelerometer measurement; It represents the projection of the angular velocity of the Earth system relative to the inertial system in the navigation system; represents the projection of the rotational angular velocity of the navigation system relative to the Earth system in the navigation system; × represents the vector cross multiplication operation; v n Indicates the navigation system speed; g n It represents the projection of gravitational acceleration in the navigation system;

[0016] Multiply both sides of the above equation by Integrating both sides simultaneously, Taking it out from the integral, we get:

[0017]

[0018] Among them, β represents the observation vector; α represents the reference vector; represents the direction cosine matrix from the initial navigation system to the navigation system; × represents the vector cross multiplication operation;

[0019] The IMU output is modeled and the reference vector, the direction cosine matrix from the initial load system to the load system, and the IMU measurement zero bias are used as state variables to establish a state prediction model and a state measurement model of the real state, and according to the relationship between the real state, the error state, and the nominal state, the error state prediction model and the measurement model, as well as the nominal state prediction model include:

[0020] Model the IMU output and obtain the IMU output model:

[0021]

[0022] in, represents the accelerometer measurement value; b a Indicates the accelerometer measurement zero bias; n a represents the accelerometer measuring Gaussian white noise; Represents the gyroscope measurement value; Indicates the true value of gyroscope measurement; b g Indicates the gyroscope measurement bias; n g represents the gyroscope measurement Gaussian white noise;

[0023] Define the true state as:

[0024]

[0025] Wherein, T represents the transpose operation;

[0026] The nominal state is defined as:

[0027]

[0028] in, represents the nominal reference vector; represents the nominal direction cosine matrix from the initial load system to the load system; Indicates the nominal accelerometer measurement bias; Indicates the nominal gyro measurement bias;

[0029] Define the error state as:

[0030]

[0031] Among them, δα represents the error reference vector; δθ represents the attitude error vector; δb a Indicates the error accelerometer measurement bias; δb gIndicates the error gyro measurement bias;

[0032] The relationship between the true state, nominal state and error state is established as:

[0033]

[0034] Where exp(·) represents the exponential function; δθ× represents the antisymmetric matrix of the attitude error vector;

[0035] Among them, exp(δθ×) can be calculated by the following formula:

[0036]

[0037] Where I3 represents the identity matrix of 3 rows and 3 columns; ||δθ|| represents the 2-norm of the attitude error vector;

[0038] According to the model of IMU measurement output and the IMU zero bias model as a random walk process, the prediction model of the real state can be obtained as follows:

[0039]

[0040] in, represents the differential of the reference vector; Represents the differential of the direction cosine matrix from the initial load system to the load system; represents the gyroscope measurement value; (a)× represents the antisymmetric matrix of vector a; represents the differential of the accelerometer measurement bias; Represents the differential of the gyroscope measurement bias;

[0041] The prediction model for the nominal state is defined as:

[0042]

[0043] in, represents the differential of the nominal reference vector; represents the differential of the nominal direction cosine matrix of the initial load system to the load system; represents the gyroscope measurement value; (a)× represents the antisymmetric matrix of vector a; Represents the differential of the nominal accelerometer measurement bias; 0 3×1 represents a 3D zero vector; represents the differential of the nominal gyro measurement bias;

[0044] According to the differential equations of the true state and the nominal state, and the relationship between the true state, the nominal state and the error state, the prediction model of the error state is obtained:

[0045]

[0046] in, represents the differential of the error reference vector; Represents the accelerometer measurement value; represents the differential of the attitude error vector; Represents the gyroscope measurement value; Indicates the nominal gyro measurement bias; represents the differential of the error accelerometer measurement bias; represents the differential of the error gyro measurement bias; define i as the index value of the IMU measurement value, and define Δt s is the IMU measurement period, and the discretized error state prediction model can be obtained as follows:

[0047] δx i+1 =F i δx i +w i

[0048] Among them, δx i+1 Indicates t i+1 Error state at time δx i Indicates t i The error state at the moment; represents the state noise vector; F i Indicates t i The state transfer matrix at time;

[0049] Define k as the index value of the GNSS measurement value, and define Δt g For the GNSS measurement cycle, the observation model of the error state is constructed through the vector observer:

[0050] z k =H k δx k +n v,k

[0051] in, Indicates t k =kΔt g The observed value at time; Indicates t k The observation matrix at time; Indicates t k Error state at time n v,k Indicates t k The measurement noise vector at time; Indicates t k The observation vector constructed from the GNSS measured velocity at the time; Indicates t kThe direction cosine matrix from the initial navigation system to the initial load system obtained by the optimization method at each moment; Indicates t k Nominal reference vector at time; 0 3×9 Represents a zero matrix with 3 rows and 9 columns.

[0052] Optionally, the receiving IMU output and obtaining the error state, the error state covariance matrix and the prior estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until the GNSS output of a new frame is received includes:

[0053] Based on the established state prediction model of the error state, the IMU output is received and the error state and the error state covariance matrix are estimated a priori through the Kalman filter prediction update formula:

[0054]

[0055] in, Indicates t i+1 A priori estimate of the error state at the moment; F i Indicates t i The state transfer matrix at time; Indicates t i A priori estimate of the error state at that moment; Indicates t k-1 A priori estimate of the error state at that moment; Indicates t k-1 A posteriori estimate of the error state at that moment; Indicates t i+1 A priori estimate of the error state covariance matrix at each moment; Indicates t i A priori estimate of the error state covariance matrix at the moment; Q i Indicates t i+1 The state noise covariance matrix at the moment; Indicates t k-1 A priori estimate of the error state covariance matrix at each moment; Indicates t k-1 The posterior estimate of the error state covariance matrix at the moment;

[0056] The a priori estimate of the nominal state is given by:

[0057]

[0058] in, Indicates t i+1 A priori estimate of the nominal reference vector at time instant; Indicates t i A priori estimate of the nominal reference vector at time instant; Indicates t i The a priori estimate of the direction cosine matrix from the initial carrier system to the carrier system at time Δt s Represents the IMU sampling interval; I3 represents the identity matrix of 3 rows and 3 columns; Indicates t i Gyroscope measurement value at the moment; Indicates t i A priori estimate of the nominal gyro measurement bias at time; (a)× represents the antisymmetric matrix of vector a; Indicates t i Accelerometer measurement value at the moment; Indicates t i A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A priori estimate of the nominal reference vector at time instant; Indicates t k-1 A posteriori estimate of the nominal reference vector at time instant; Indicates t i+1 The a priori estimate of the direction cosine matrix from the initial load system to the load system at time; exp(·) represents the exponential function; Indicates t k-1 A priori estimate of the direction cosine matrix from the initial load system to the load system at time instant; Indicates t k-1 The a posteriori estimate of the direction cosine matrix from the initial load system to the load system at time instant; Indicates t i+1 A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A posteriori estimate of the nominal accelerometer measurement bias at time instant; Indicates t i+1 A priori estimate of the nominal gyro measurement bias at that moment; Indicates t k-1 A priori estimate of the nominal gyro measurement bias at that moment; Indicates t k-1 A posteriori estimate of the nominal gyro measurement bias at time instant;

[0059] The error state, error state covariance matrix and prior estimation of the nominal state are performed with the arrival of the IMU output until a new frame of GNSS output arrives, and the error reference vector, the covariance matrix of the error reference vector and the prior estimation of the nominal reference vector are obtained.

[0060] Optionally, when receiving a new frame of GNSS output, updating the direction cosine matrix from the initial navigation system to the navigation system according to the GNSS output, and constructing the observation vector includes:

[0061] Receive GNSS output and construct observation vector:

[0062]

[0063] in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k The nominal reference vector at time instant; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t k GNSS velocity measurement value at the moment; Indicates the GNSS velocity measurement value at the initial moment; Indicates t j The direction cosine matrix from the initial navigation system to the navigation system at time Δt g represents the GNSS sampling interval; I3 represents the identity matrix of 3 rows and 3 columns; Indicates t j The projection of the angular velocity of the navigation system relative to the inertial system at the moment in the navigation system; (a)× represents the antisymmetric matrix of vector a; Indicates t j The projection of the earth's rotation angular velocity at the moment in the navigation system; Indicates t j The projection of the gravitational acceleration at the moment in the navigation system; Indicates t j+1 The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l+1 The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l The projection of the angular velocity of the navigation system relative to the inertial system at the time in the navigation system; Δt g represents the GNSS sampling interval; exp(·) represents the exponential function.

[0064] Optionally, the modeling of the GNSS output and constructing a robust weight function through extended modulus matching based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector and the observation vector measurement value includes:

[0065] Model the GNSS output and obtain the GNSS output model:

[0066]

[0067] in, Indicates t k GNSS measurement speed at the moment; v n Indicates t k The true speed of time; Indicates t k GNSS measurement velocity error value at the moment;

[0068] Substituting the GNSS measured velocity into the observation vector yields:

[0069]

[0070] in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; β k Indicates t k The true value of the observation vector at each moment; δβ k Indicates t k The error observation vector at time; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time;

[0071] The vector modulus square error is defined as:

[0072]

[0073] Among them, ε k Indicates t k The square error of the modulus value at the moment; Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time ||·|| 2 Represents the modulus square operation;

[0074] Substituting into the error relationship, we can get the normalized modulus square error:

[0075]

[0076] in, Indicates t k Normalized modulus square error at time; Var[·] represents variance operation; ||·|| 2 Represents the modulus square operation; Indicates t kThe observation vector constructed using the GNSS velocity measurement at each moment; T represents the transpose operation; σ k Indicates t k The standard deviation of the GNSS velocity noise at the moment; Indicates t k A priori estimate of the time error reference vector; Indicates t k A priori estimate of the nominal reference vector at time instant; Represented by the matrix The matrix consisting of the first three rows and first three columns of ;

[0077] Based on Huber robust filtering theory, a robust weight function is constructed by normalizing the square error of the modulus value:

[0078]

[0079] in, represents the robust weight function; γ represents the Gaussian threshold; sgn(·) represents the sign function.

[0080] Optionally, the step of constructing a robust attitude determination matrix using a robust weight function, an observation vector measurement value, and a priori estimated value of a nominal reference vector, and solving a nominal direction cosine matrix from an initial navigation system to an initial carrier system by an optimization method according to the robust attitude determination matrix includes:

[0081] Construct the robust pose determination matrix:

[0082]

[0083] Among them, Y k Indicates t k The robust attitude determination matrix at time ; Y k-1 Indicates t k-1 The robust attitude determination matrix at time Δt g Indicates the GNSS sampling interval; Robust weight function representing the normalized modulus square error; Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time instant;

[0084] in, and The expression is as follows:

[0085]

[0086] in, Indicates t kThe observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time; T represents the transpose operation; (a)× represents the antisymmetric matrix of vector a;

[0087] Solving for Y k The eigenvector corresponding to the minimum eigenvalue of is obtained, and the quaternion corresponding to the direction cosine matrix of the optimal initial navigation system to the initial load system is obtained. Then, through the conversion formula of the quaternion and the direction cosine matrix, t is solved. k The nominal direction cosine matrix from the initial navigation system to the initial load system at time

[0088] Optionally, the error state and the error state covariance matrix are estimated a posteriori by using the Kalman filter measurement update formula and the robust weight function according to the observation model of the error state, and the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state to complete the a posteriori estimate of the nominal state; and the real-time carrier attitude matrix is ​​obtained by combining the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system, including:

[0089] The posterior estimation of the error state and the error state covariance matrix is ​​performed by the following formula:

[0090]

[0091] Among them, H k Indicates t k The observation matrix at time; Indicates t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time 0; 3×9 represents a zero matrix with 3 rows and 9 columns; z k Indicates t k The observed value at time; Indicates t k The prior estimate of the error state vector at the moment K k Indicates t k The Kalman gain matrix at time t; Indicates t k A priori estimate of the covariance matrix of the error state vector at each moment; R represents the robust weight function of the normalized squared error of the modulus; k Indicates t k The measurement noise matrix at time; Indicates t k A posteriori estimate of the error state vector at the moment; Indicates t kThe posterior estimate of the covariance matrix of the error state vector at the moment I 12 Represents the identity matrix with 12 rows and 12 columns;

[0092] according to get:

[0093]

[0094] in, Indicates t k The a posteriori estimate of the time error reference vector; (m:n) represents the operation of taking the m to n elements of the vector; Indicates t k A posteriori estimate of the moment error attitude angle vector; Indicates t k A posteriori estimate of the accelerometer bias at the instant of error; Indicates t k A posteriori estimate of the time error gyro bias;

[0095] The posterior estimate of the error state is injected into the prior estimate of the nominal state, thus completing the posterior estimate of the nominal state:

[0096]

[0097]

[0098] in, Indicates t k A posteriori estimate of the nominal reference vector at time instant; Indicates t k A priori estimate of the nominal reference vector at time instant; Indicates t k A posteriori estimate of the time error reference vector; Indicates t k The a posteriori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t; Indicates t k A priori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t; Indicates t k A posteriori estimate of the moment error attitude angle vector; Indicates t k The a posteriori estimate of the nominal accelerometer measurement bias at time t; Indicates t k A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k The a posteriori estimate of the error accelerometer measurement bias at time instant; Indicates t k The a posteriori estimate of the nominal gyro measurement bias at time t; Indicates t k A priori estimate of the nominal gyro measurement bias at time instant; Indicates t k The a posteriori estimate of the gyro measurement bias error at time Set to zero vector and enter the next round of state estimation;

[0099] The real-time carrier posture matrix is ​​obtained by the following formula:

[0100]

[0101] in, Indicates t k The carrier posture matrix at the moment; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time; Indicates t k The a posteriori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t.

[0102] In a second aspect, the present invention provides a moving base initial alignment system for an integrated navigation system, comprising:

[0103] A model building module is used to model the IMU output based on the error-free observation vector model and the reference vector model, and to establish a state prediction model and a state measurement model of the real state by taking the reference vector, the direction cosine matrix from the initial load system to the load system, and the IMU measurement zero bias as state variables, and to obtain the prediction model and measurement model of the error state and the prediction model of the nominal state according to the relationship between the real state, the error state, and the nominal state;

[0104] A data processing module is used to receive the IMU output, and obtain the error state, the error state covariance matrix and the prior estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until a new frame of GNSS output is received; and when a new frame of GNSS output is received, update the direction cosine matrix from the initial navigation system to the navigation system according to the GNSS output, and construct an observation vector;

[0105] A state update module is used to model the GNSS output and construct a robust weight function through extended modulus matching based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector, and the observation vector measurement value;

[0106] A solution module is used to construct a robust attitude determination matrix using a robust weight function, an observation vector measurement value and a priori estimated value of a nominal reference vector, and solve a nominal direction cosine matrix from an initial navigation system to an initial load system by an optimization method according to the robust attitude determination matrix;

[0107] The attitude matrix determination module is used to perform a posteriori estimation of the error state and the error state covariance matrix according to the observation model of the error state through the Kalman filter measurement update formula and the robust weight function, and inject the a posteriori estimation of the error state into the a priori estimation of the nominal state to complete the a posteriori estimation of the nominal state; and combine the a posteriori estimation of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system to obtain the real-time carrier attitude matrix;

[0108] The alignment control module is used to control the initial alignment of the moving base according to the real-time carrier posture matrix.

[0109] In a third aspect of the present invention, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method for initial alignment of a moving base of the above-mentioned combined navigation system when executing the computer program.

[0110] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method for initial alignment of a moving base of the above-mentioned combined navigation system are implemented.

[0111] Compared with the prior art, the present invention has the following beneficial effects:

[0112] The method for initial alignment of a moving base of an integrated navigation system of the present invention firstly takes a reference vector, a direction cosine matrix from an initial carrier system to a carrier system and an IMU measurement zero bias as state variables based on an error-free observation vector model and a reference vector model, constructs a state prediction model and a state measurement model of a real state, and obtains a prediction model and a measurement model of an error state and a prediction model of a nominal state according to the relationship between the real state, the error state and the nominal state; then receives an IMU output, and obtains an error state, an error state covariance matrix and a priori estimate of the nominal state through a Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until a GNSS output of a new frame is received; then receives a GNSS output, updates the direction cosine matrix from the initial navigation system to the navigation system and constructs an observation vector; then models the GNSS output, and constructs a robust weight by extended modulus matching based on an error reference vector, an error reference vector covariance matrix, a priori estimate of the nominal reference vector and a measurement value of the observation vector. value function; then, a robust attitude determination matrix is ​​constructed by using the robust weight function, the measurement value of the observation vector and the prior estimate of the nominal reference vector, and the nominal direction cosine matrix from the initial navigation system to the initial carrier system is solved by the optimization method according to the robust attitude determination matrix; finally, according to the observation model of the error state, the error state and the error state covariance matrix are estimated a posteriori by using the Kalman filter measurement update formula and the robust weight function, and the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state, thereby completing the a posteriori estimate of the nominal state, and combining the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system, the real-time carrier attitude matrix is ​​obtained to guide the initial alignment of the moving base of the integrated navigation system, which can suppress the accumulated error of the IMU zero bias, and can detect and remove the wild values ​​in the output of the GNSS receiver, thereby effectively improving the accuracy and convergence speed of the initial alignment of the moving base of the integrated navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0113] Figure 1 This is a flow chart of a method for initial alignment of a moving base of an integrated navigation system according to an embodiment of the present invention.

[0114] Figure 2 This is a diagram of the simulated motion trajectory of a carrier according to an embodiment of the present invention.

[0115] Figure 3 Graph showing reference values ​​of the pitch angle and roll angle of a carrier according to an embodiment of the present invention.

[0116] Figure 4 This is a reference value diagram of the heading angle of the carrier according to an embodiment of the present invention.

[0117] Figure 5FIG. 4 is a diagram of simulated velocity output of a GNSS receiver according to an embodiment of the present invention.

[0118] Figure 6 4 is a gyroscope bias estimation curve diagram according to an embodiment of the present invention.

[0119] Figure 7 4 is a graph showing the zero bias estimation curve of an accelerometer according to an embodiment of the present invention.

[0120] Figure 8 4 is a comparison diagram of pitch angle alignment error curves according to an embodiment of the present invention.

[0121] Fig. 9 1 is a comparison diagram of roll angle alignment error curves according to an embodiment of the present invention.

[0122] Fig.10 1 is a comparison diagram of heading angle alignment error curves according to an embodiment of the present invention.

[0123] Fig.11 The present invention is a block diagram of the structure of the dynamic base initial alignment system of the integrated navigation system according to the embodiment of the present invention. DETAILED DESCRIPTION

[0124] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0125] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0126] The present invention is further described in detail below in conjunction with the accompanying drawings:

[0127] See also Figure 1In one embodiment of the present invention, a method for initial alignment of a moving base of an integrated navigation system is provided, which can be effectively applied to an integrated navigation system based on SINS and GNSS, and overcomes the defects of low convergence speed and accuracy of initial alignment of a moving base of the current integrated navigation system.

[0128] Specifically, the method for initial alignment of a moving base of an integrated navigation system of the present invention comprises the following steps:

[0129] S1: Based on the error-free observation vector model and the reference vector model, the IMU output is modeled and the reference vector, the direction cosine matrix from the initial carrier system to the carrier system and the IMU measurement zero bias are used as state variables to establish the state prediction model and state measurement model of the real state. According to the relationship between the real state, the error state and the nominal state, the prediction model and measurement model of the error state and the prediction model of the nominal state are obtained.

[0130] S2: Receive the IMU output, and obtain the error state, error state covariance matrix and prior estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until the GNSS output of a new frame is received; and when the GNSS output of a new frame is received, update the direction cosine matrix from the initial navigation system to the navigation system according to the GNSS output, and construct the observation vector.

[0131] S3: Model the GNSS output and construct a robust weight function through extended modulus matching based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector, and the observation vector measurement value.

[0132] S4: A robust attitude determination matrix is ​​constructed using the robust weight function, the measurement value of the observation vector and the prior estimate of the nominal reference vector, and the nominal direction cosine matrix from the initial navigation system to the initial load system is solved by the optimization method based on the robust attitude determination matrix.

[0133] S5: According to the observation model of the error state, the error state and the error state covariance matrix are estimated a posteriori through the Kalman filter measurement update formula and the robust weight function, and then the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state to complete the a posteriori estimate of the nominal state. The real-time carrier attitude matrix is ​​obtained by combining the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system.

[0134] S6: Control the initial alignment of the moving base according to the real-time carrier posture matrix.

[0135] The method for initial alignment of a moving base of an integrated navigation system of the present invention firstly takes a reference vector, a direction cosine matrix from an initial carrier system to a carrier system and an IMU measurement zero bias as state variables based on an error-free observation vector model and a reference vector model, constructs a prediction model of a real state and a state measurement model, and obtains a prediction model and a measurement model of an error state and a prediction model of a nominal state according to the relationship between the real state, the error state and the nominal state; then receives an IMU output, obtains an error state, an error state covariance matrix and a priori estimation of a nominal state according to the prediction model of the error state and the nominal state through a Kalman filter prediction update formula, and receives a GNSS output of a new frame; then receives a GNSS output; S output, update the direction cosine matrix from the initial navigation system to the navigation system and construct the observation vector; then model the GNSS output, based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector and the measurement value of the observation vector, construct a robust weight function about the normalized modulus square error through extended modulus matching; then construct a robust attitude determination matrix, and solve the nominal direction cosine matrix from the initial navigation system to the initial carrier system through the optimization method; finally, the error state and the error state covariance matrix are estimated a posteriori through the Kalman filter measurement update formula and the robust weight function, and then the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state, thereby completing the a posteriori estimate of the nominal state. Combined with the a posteriori estimation of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system, the real-time carrier attitude matrix is ​​obtained to control the initial alignment of the moving base of the integrated navigation system. It can suppress the accumulated error of IMU zero bias and detect and remove wild values ​​in the GNSS receiver output, thereby effectively improving the accuracy and convergence speed of the initial alignment of the moving base of the integrated navigation system.

[0136] The IMU zero bias is used as a state, modeled, and estimated. In the prior estimation of the nominal reference vector, the nominal direction cosine matrix from the initial load system to the load system, the nominal IMU zero bias value is compensated, thereby effectively suppressing the cumulative error caused by the IMU zero bias. When the GNSS receiver output has wild values, it will cause There are also outliers, when the normalized modulus square error exceeds the preset Gaussian threshold, making will be a small amount, thus effectively reducing The weight of , plays a role in suppressing outliers. The robust weight function improves the z k The measurement covariance R k , reduce z k The weight in the posterior estimate of the error state, thereby effectively suppressing The outliers in the error state estimation can be eliminated to achieve the effect of resisting the interference of outliers.

[0137] In a possible implementation, the error-free observation vector model and the reference vector model are derived based on the specific force equation and the chain rule of the direction cosine matrix.

[0138] Specifically, from the attitude update equation, we know

[0139]

[0140] in, Represents the differential of the direction cosine matrix from the initial load system to the load system; Represents the direction cosine matrix from the initial load system to the load system; represents the true value measured by the gyroscope; (a)× represents the antisymmetric matrix of vector a; Represents the differential of the direction cosine matrix from the initial navigation system to the navigation system; Represents the direction cosine matrix from the initial navigation system to the navigation system; It represents the projection of the angular velocity of the navigation system relative to the inertial system under the navigation system.

[0141] From the specific force equation and the chain rule of direction cosine matrices, we know that:

[0142]

[0143] in, represents the differential of the navigation system velocity; Represents the direction cosine matrix from the navigation coordinate system to the initial navigation system; represents the direction cosine matrix from the initial navigation system to the initial load system; f b Represents the true value of the accelerometer measurement; It represents the projection of the angular velocity of the Earth system relative to the inertial system in the navigation system; represents the projection of the rotational angular velocity of the navigation system relative to the Earth system in the navigation system; × represents the vector cross multiplication operation; v n Indicates the navigation system speed; g n Represents the projection of gravitational acceleration in the navigation system.

[0144] Multiply both sides of the above equation by Integrating both sides simultaneously, Taking it out from the integral and sorting it out, we can get:

[0145]

[0146] Among them, β represents the observation vector; α represents the reference vector.

[0147] In a possible implementation, the IMU output is modeled and the reference vector, the direction cosine matrix from the initial load system to the load system, and the IMU measurement zero bias are used as state variables to establish a state prediction model and a state measurement model of the real state, and according to the relationship between the real state, the error state, and the nominal state, the error state prediction model and the measurement model, as well as the nominal state prediction model are obtained, including:

[0148] Model the IMU output and obtain the IMU output model:

[0149]

[0150] in, represents the accelerometer measurement value; b a Indicates the accelerometer measurement zero bias; n a represents the accelerometer measuring Gaussian white noise; Represents the gyroscope measurement value; Indicates the true value of gyroscope measurement; b g Indicates the gyroscope measurement bias; n g represents the gyroscope measurement Gaussian white noise.

[0151] Define the true state as:

[0152]

[0153] Among them, b a Indicates the accelerometer measurement bias; b g Indicates the gyroscope measurement bias; T indicates the transpose operation.

[0154] The nominal state is defined as:

[0155]

[0156] in, represents the nominal reference vector; represents the nominal direction cosine matrix from the initial load system to the load system; Indicates the nominal accelerometer measurement bias; Represents the nominal gyro measurement bias.

[0157] Define the error state as:

[0158]

[0159] Among them, δα represents the error reference vector; δθ represents the attitude error vector; δb a Indicates the error accelerometer measurement bias; δb g Represents the error gyro measurement bias.

[0160] Therefore, the relationship between the true state, nominal state and error state is established as:

[0161]

[0162] Where exp(·) represents the exponential function; δθ× represents the antisymmetric matrix of the attitude error vector.

[0163] Among them, exp(δθ×) can be calculated by the following formula:

[0164]

[0165] Among them, I3 represents the identity matrix of 3 rows and 3 columns; ||δθ|| represents the 2-norm of the attitude error vector.

[0166] According to the model of IMU measurement output and the IMU zero bias model as a random walk process, the prediction model of the real state can be obtained as follows:

[0167]

[0168] in, represents the differential of the reference vector; Represents the differential of the direction cosine matrix from the initial load system to the load system; represents the gyroscope measurement value; (a)× represents the antisymmetric matrix of vector a; represents the differential of the accelerometer measurement bias; Represents the differential of the gyroscope measured bias.

[0169] The prediction model for the nominal state is defined as:

[0170]

[0171]

[0172] in, represents the differential of the nominal reference vector; represents the differential of the nominal direction cosine matrix of the initial load system to the load system; represents the gyroscope measurement value; (a)× represents the antisymmetric matrix of vector a; Represents the differential of the nominal accelerometer measurement bias; 0 3×1 represents a 3D zero vector; Represents the differential of the nominal gyro measurement bias.

[0173] According to the true and nominal state differential equations and the relationship between the true, nominal and error states, the following error state prediction model can be derived:

[0174]

[0175] in, represents the differential of the error reference vector; × represents the vector cross multiplication operation; represents the differential of the attitude error vector; Represents the gyroscope measurement value; represents the differential of the error accelerometer measurement bias; Represents the differential of the gyro measurement bias.

[0176] Define i as the index value of the IMU measurement value, and define Δt s is the IMU measurement period. Discretizing the above error state differential equation, the discretized prediction model of the error state can be obtained as follows:

[0177] δx i+1 =F i δx i +w i

[0178] Among them, δx i+1 Indicates t i+1 Error state at time δx i Indicates t i The error state at the moment; represents the state noise vector.

[0179] Among them, F i Indicates t i The state transfer matrix at time is expressed as:

[0180]

[0181]

[0182] Where I3 represents the identity matrix with 3 rows and 3 columns; Δt s Indicates the IMU sampling interval; Indicates t i The nominal direction cosine matrix from the initial load system to the load system at time; Indicates t i Gyroscope measurement at the moment; Indicates t i The nominal gyro measurement bias at time; (a)× represents the antisymmetric matrix of vector a; Indicates t i Accelerometer measurement at the moment; Indicates t i The nominal accelerometer measurement bias at time.

[0183] Define k as the index value of the GNSS measurement value, and define Δt gFor the GNSS measurement cycle, the observation model of the error state is constructed through the vector observer:

[0184] z k =H k δx k +n v,k

[0185] in, Indicates t k =kΔt g The observed value at time; Indicates t k The observation matrix at time; Indicates t k Error state at time n v,k Indicates t k The measurement noise vector at time; Indicates t k The observation vector constructed from the GNSS measured velocity at the time; Indicates t k The direction cosine matrix from the initial navigation system to the initial load system obtained by the optimization method at each moment; Indicates t k Nominal reference vector at time; 0 3×9 Represents a zero matrix with 3 rows and 9 columns.

[0186] In a possible implementation, the receiving IMU output and obtaining the error state, the error state covariance matrix and the prior estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until the GNSS output of a new frame is received includes:

[0187] Based on the established state prediction model of the error state, the IMU output is received and the error state and the error state covariance matrix are estimated a priori through the Kalman filter prediction update formula:

[0188]

[0189] in, Indicates t i+1 A priori estimate of the error state at the moment; F i Indicates t i The state transfer matrix at time; Indicates t i A priori estimate of the error state at that moment; Indicates t k-1 A priori estimate of the error state at that moment; Indicates t k-1 A posteriori estimate of the error state at that moment; Indicates t i+1Prior estimate of the error state covariance matrix at the moment; P i - Indicates t i A priori estimate of the error state covariance matrix at the moment; Q i Indicates t i+1 The state noise covariance matrix at the moment; Indicates t k-1 A priori estimate of the error state covariance matrix at each moment; Indicates t k-1 The posterior estimate of the error state covariance matrix at the moment;

[0190] At the same time, a priori estimation of the nominal state is performed:

[0191]

[0192] in, Indicates t i+1 A priori estimate of the nominal reference vector at time instant; Indicates t i A priori estimate of the nominal reference vector at time instant; Indicates t i A priori estimate of the nominal direction cosine matrix from the initial load system to the load system at time Δt s Represents the IMU sampling interval; I3 represents the identity matrix of 3 rows and 3 columns; Indicates t i Gyroscope measurement value at the moment; Indicates t i A priori estimate of the nominal gyro measurement bias at time; (a)× represents the antisymmetric matrix of vector a; Indicates t i Accelerometer measurement value at the moment; Indicates t i A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A priori estimate of the nominal reference vector at time instant; Indicates t k-1 A posteriori estimate of the nominal reference vector at time instant; Indicates t i+1 The a priori estimate of the direction cosine matrix from the initial load system to the load system at time; exp(·) represents the exponential function; Indicates t k-1 A priori estimate of the direction cosine matrix from the initial load system to the load system at time instant; Indicates t k-1 The a posteriori estimate of the direction cosine matrix from the initial load system to the load system at time instant; Indicates t i+1 A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A posteriori estimate of the nominal accelerometer measurement bias at time instant; Indicates t i+1 A priori estimate of the nominal gyro measurement bias at that moment; Indicates t k-1 A priori estimate of the nominal gyro measurement bias at that moment; Indicates t k-1 The a posteriori estimate of the nominal gyro measurement zero bias at the moment; in the a priori estimate of the nominal reference vector, the nominal direction cosine matrix from the initial load system to the load system, the nominal IMU zero bias value is compensated, thereby effectively suppressing the cumulative error caused by the IMU zero bias.

[0193] The prior estimation of the error state and the nominal state is carried out with the arrival of the IMU output until a new frame of GNSS output arrives (at this time, the prior estimation of the nominal state and the error state are recursively extended to t k time), and make a posteriori estimation.

[0194] In a possible implementation, when receiving a new frame of GNSS output, updating the direction cosine matrix from the initial navigation system to the navigation system according to the GNSS output, and constructing the observation vector includes:

[0195] Receive GNSS output and construct observation vector:

[0196]

[0197] in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k The nominal reference vector at time instant; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t k GNSS velocity measurement value at the moment; Indicates the GNSS velocity measurement value at the initial moment; Indicates t j The direction cosine matrix from the initial navigation system to the navigation system at time Δt g represents the GNSS sampling interval; I3 represents the identity matrix of 3 rows and 3 columns; Indicates t j The projection of the angular velocity of the navigation system relative to the inertial system at the moment in the navigation system; (a)× represents the antisymmetric matrix of vector a; Indicates tj The projection of the earth's rotation angular velocity at the moment in the navigation system; Indicates t j The projection of the gravitational acceleration at the moment in the navigation system; Indicates t j+1 The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l+1 The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l The projection of the angular velocity of the navigation system relative to the inertial system at the time in the navigation system; Δt g Indicates the GNSS sampling interval.

[0198] In a possible implementation, the modeling of the GNSS output and constructing a robust weight function by extended modulus matching based on an error reference vector, an error reference vector covariance matrix, a priori estimates of a nominal reference vector, and an observation vector measurement value includes:

[0199] Model the GNSS output and obtain the GNSS output model:

[0200]

[0201] in, Indicates t k GNSS measurement speed at the moment; v n Indicates t k The true speed of time; Indicates t k The GNSS measured velocity error value at the moment.

[0202] Substituting the GNSS measured velocity into the observation vector yields:

[0203]

[0204] in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; β k Indicates t k The true value of the observation vector at each moment; δβ k Indicates t k The error observation vector at time; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time.

[0205] Since GNSS receivers are easily affected by external environments such as obstacles and multipath effects, outliers may appear in the output sometimes. The extended modulus matching method can effectively detect and remove outliers and improve the robustness of the system.

[0206] The vector modulus square error is defined as:

[0207]

[0208] Among them, ε k Indicates t k The square error of the modulus value at the moment; Table |t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time ||·|| 2 Represents a modulo square operation.

[0209] Substituting into the error relationship, we can get:

[0210]

[0211] Among them, ε k Indicates t k The square error of the modulus at the moment; β k Indicates t k The true value of the observation vector at the moment; α k Indicates t k The true value of the reference vector at time ||·|| 2 represents the modulus square operation; T represents the transpose operation; δβ k Indicates t k The error observation vector at time; Indicates t k A priori estimate of the instant error reference vector.

[0212] From the relationship between the observation vector and the reference vector, we can get:

[0213]

[0214] Among them, β k Indicates t k The true value of the observation vector at the moment; α k Indicates t k The true value of the reference vector at the moment; It represents the true value of the direction cosine matrix from the initial navigation system to the initial load system; T represents the transpose operation.

[0215] Substituting the relationship between the observation vector and the reference vector into the error relationship, we can get:

[0216]

[0217] Among them, ε k Indicates t k The square error of the modulus at the moment; β k Indicates t k The true value of the observation vector at the moment; α k Indicates t k The true value of the reference vector at time instant.

[0218] Continuing to use the relationship between the observation vector and the reference vector and omitting the second-order small quantity, the above formula can be simplified to:

[0219]

[0220] Among them, ε k Indicates t k The square error of the modulus at the moment; β k Indicates t k The true value of the observation vector at the moment; α k Indicates t k The true value of the reference vector at the moment; δβ k Indicates t k The error observation vector at time; Indicates t k A priori estimate of the time error reference vector; Indicates t k A priori estimate of the nominal reference vector at time instant; Represents the true value of the direction cosine matrix from the initial carrier system to the initial navigation system.

[0221] Solving the expectation and variance of the squared error of the modulus value yields:

[0222]

[0223] Where E[·] represents the expected operation; Var[·] represents the variance operation; σ k Indicates t k The standard deviation of the GNSS velocity noise at the time.

[0224] The normalized modulus square error is defined as:

[0225]

[0226] in, Indicates t k Normalized modulus square error at time; Var[·] represents variance operation; ||·|| 2 Represents the modulus square operation; Indicates t kThe observation vector constructed using the GNSS velocity measurement at each moment. T represents the transpose operation; σ k Indicates t k The standard deviation of the GNSS velocity noise at the moment; Indicates t k A priori estimate of the time error reference vector; Indicates t k A priori estimate of the nominal reference vector at time instant; Represented by the matrix The matrix consists of the first three rows and first three columns of .

[0227] Based on Huber robust filtering theory, the normalized modulus square error can be used to construct the weight function:

[0228]

[0229] in, represents the robust weight function; Indicates t k The normalized square error of the modulus at the time instant; γ represents the Gaussian threshold; sgn(·) represents the sign function.

[0230] In a possible implementation, the method of constructing a robust attitude determination matrix using a robust weight function, an observation vector measurement value, and a priori estimated value of a nominal reference vector, and solving a nominal direction cosine matrix from an initial navigation system to an initial load system by an optimization method according to the robust attitude determination matrix includes:

[0231] Construct the robust pose determination matrix:

[0232]

[0233] Among them, Y k Indicates t k The robust attitude determination matrix at time ; Y k-1 Indicates t k-1 The robust attitude determination matrix at time Δt g Indicates the GNSS sampling interval; Robust weight function representing the normalized modulus square error; Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at the moment. When the GNSS receiver output has wild values, it will cause There are also outliers, when the normalized modulus square error exceeds the preset Gaussian threshold, making will be a small amount, thus effectively reducing The weight of plays the role of suppressing outliers.

[0234] and The expression is as follows:

[0235]

[0236] in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time; T represents the transpose operation; (a)× represents the antisymmetric matrix of vector a.

[0237] Solving for Y k The eigenvector corresponding to the minimum eigenvalue of is the quaternion corresponding to the direction cosine matrix of the optimal initial navigation system to the initial carrier system. Through the conversion formula of quaternion and direction cosine matrix, we can solve t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time

[0238] In a possible implementation, the error state and the error state covariance matrix are estimated a posteriori by using the Kalman filter measurement update formula and the robust weight function according to the observation model of the error state, and then the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state, thereby completing the a posteriori estimate of the nominal state, and combining the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system, to obtain the real-time carrier attitude matrix, including:

[0239] Make a posteriori estimates of the error state and error state covariance matrix:

[0240]

[0241] Among them, H k Indicates t k The observation matrix at time; Indicates t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time 0; 3×9 represents a zero matrix with 3 rows and 9 columns; z k Indicates t k The observed value at time; Indicates t k The prior estimate of the error state vector at the moment K k Indicates t k The Kalman gain matrix at time t; P k - Indicates t kA priori estimate of the covariance matrix of the error state vector at each moment; Indicates t k Robust weight function of the normalized modulus square error at each time step; R k Indicates t k The measurement noise matrix at time; Indicates t k A posteriori estimate of the error state vector at the moment; Indicates t k The posterior estimate of the covariance matrix of the error state vector at the moment I 12 Represents a 12-row 12-column identity matrix. The robust weight function is used to improve the inclusion of outliers. z k The measurement covariance R k , reduce z k The weight in the posterior estimate of the error state, thereby effectively suppressing The outliers in the error state estimation can be eliminated to achieve the effect of resisting the interference of outliers.

[0242] according to You can get:

[0243]

[0244] in, Indicates t k The a posteriori estimate of the time error reference vector; (m:n) represents the operation of taking the m to n elements of the vector; Indicates t k A posteriori estimate of the moment error attitude angle vector; Indicates t k A posteriori estimate of the accelerometer bias at the instant of error; Indicates t k A posteriori estimate of the time error gyro bias.

[0245] Inject the posterior estimate of the error state into the prior estimate of the nominal state to complete the posterior estimate of the nominal state:

[0246]

[0247] in, Indicates t k A posteriori estimate of the nominal reference vector at time instant; Indicates t k A priori estimate of the nominal reference vector at time instant; Indicates t k A posteriori estimate of the time error reference vector; Indicates t k The a posteriori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t; Indicates t k A priori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t; Indicates t k A posteriori estimate of the moment error attitude angle vector; Indicates t k The a posteriori estimate of the nominal accelerometer measurement bias at time t; Indicates t k A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k The a posteriori estimate of the error accelerometer measurement bias at time instant; Indicates t k The a posteriori estimate of the nominal gyro measurement bias at time t; Indicates t k A priori estimate of the nominal gyro measurement bias at time instant; Indicates t k The a posteriori estimate of the error in the gyro measurement bias at time instant.

[0248] Finally, Set it to zero vector and enter the next round of state estimation.

[0249] The carrier's posture matrix can be calculated as follows:

[0250]

[0251] in, Indicates t k The carrier posture matrix at the moment; Indicates t k The direction cosine matrix from the navigation system at time to the initial navigation system; Indicates t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time; Indicates t k The a posteriori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t.

[0252] In a possible implementation, a simulation experiment is conducted on the initial alignment method of the dynamic base of the integrated navigation system of the present invention. The initial longitude, latitude and altitude of the carrier are set to 116.329373°, 40.010145° and 31.545000m respectively. The sampling frequency of the IMU is 200Hz. The zero bias of the gyroscope is 20° / h, and the random noise of the gyroscope is The zero bias of the accelerometer is 1 mg, and the random noise of the accelerometer is The sampling frequency of the GNSS receiver is 1 Hz. The velocity output including outliers is generated by the following formula:

[0253]

[0254] The alignment process takes 450 seconds. Figure 2 , showing the motion trajectory of the carrier; see Figure 3 , showing the reference values ​​of the pitch angle and roll angle of the carrier; Figure 4 , showing the reference value of the heading angle of the carrier; see Figure 5 , shows the carrier velocity measurements of the GNSS receiver.

[0255] In addition, in order to verify the effectiveness of the dynamic base initial alignment method of the combined navigation system of the present invention, four schemes were designed for comparison, namely: Scheme 1: The IFA method is used to complete the initial alignment, and the carrier velocity measurement value of the GNSS receiver contains wild values; Scheme 2: The method of the present invention is used to complete the initial alignment, and the carrier velocity measurement value of the GNSS receiver contains wild values; Scheme 3: The IFA method is used to complete the initial alignment, and the carrier velocity measurement value of the GNSS receiver has no wild values; Scheme 4: The method of the present invention is used to complete the initial alignment, and the carrier velocity measurement value of the GNSS receiver has no wild values.

[0256] See also Figure 6 and 7 , respectively, show the zero bias estimation curves of the gyroscope and accelerometer obtained by Scheme 2. Figure 8 , Fig. 9 and Fig.10 , the initial alignment error curves of the four schemes are shown respectively, including the pitch angle alignment error curve, the roll angle alignment error curve and the heading angle alignment error curve.

[0257] from Figure 6 and Figure 7 It can be seen from the figure that even if the carrier velocity measurement value of the GNSS receiver contains wild values, the IMU zero bias value curve estimated by the method of the present invention can converge to the vicinity of the true value.

[0258] See again Figure 8 , Fig. 9 and Fig.10, comparing Scheme 1 and Scheme 2, the traditional IFA method is affected by the wild values ​​in the GNSS output. After about 20 seconds of alignment, it can be seen that the attitude angle error curve has burrs, and the convergence speed is therefore slowed down. The attitude angle error curve of the method proposed in the present invention does not have burrs, reflecting that the method proposed in the present invention can effectively detect and suppress the influence of wild values ​​in the GNSS output, thereby improving the alignment convergence speed. Comparing Scheme 2 and Scheme 4, it can be found that when the GNSS output contains wild values, the attitude angle error curve of the method proposed in the present invention is almost the same as the attitude angle error curve of the method proposed in the present invention when there are no wild values ​​in the GNSS output, which further reflects the robustness of the method proposed in the present invention. Comparing Scheme 2 and Scheme 3, it can be found that the accuracy of the attitude angle error curve of the method proposed in the present invention is significantly better than that of the traditional IFA method. In the 350-450s of the initial alignment process, the roll angle, pitch angle and heading angle errors of the method proposed in the present invention are less than 0.06°, 0.03° and 0.2°, respectively. This is because the method proposed in the present invention estimates and compensates for the IMU zero bias. However, due to the influence of IMU zero bias, the roll angle, pitch angle and heading angle errors of the traditional IFA method are close to 1°, 1.2° and 3° respectively in the 350-450s of the initial alignment process. In summary, the method proposed in the present invention effectively suppresses the influence of wild values ​​and IMU zero bias in the GNSS receiver output, and has a more stable alignment process and higher alignment accuracy compared to the traditional method.

[0259] The following are device embodiments of the present invention, which can be used to implement the method embodiments of the present invention. For details not disclosed in the device embodiments, please refer to the method embodiments of the present invention.

[0260] See also Fig.11 In yet another embodiment of the present invention, a dynamic base initial alignment system for an integrated navigation system is provided, which can be used to implement the dynamic base initial alignment method for the integrated navigation system described above. Specifically, the dynamic base initial alignment system for the integrated navigation system includes a model building module, a data processing module, a state updating module, a solving module, a posture matrix determining module and an alignment control module.

[0261] Among them, the model construction module is used to model the IMU output based on the error-free observation vector model and the reference vector model, and use the reference vector, the direction cosine matrix from the initial carrier system to the carrier system, and the IMU measurement zero bias as state variables to obtain the prediction model and measurement model of the error state, as well as the prediction model of the nominal state; the data processing module is used to receive the IMU output, and obtain the error state, the error state covariance matrix and the prior estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until the GNSS output of a new frame is received; and when the GNSS output of a new frame is received, the direction cosine matrix from the initial navigation system to the navigation system is updated according to the GNSS output, and the observation vector is constructed; the state update module is used to model the GNSS output, and based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector and the observation vector The method comprises the following steps: measuring the measured value, constructing a robust weight function by extending the modulus matching; constructing a robust attitude determination matrix by using the robust weight function, the measured value of the observation vector and the prior estimate of the nominal reference vector, and solving the nominal direction cosine matrix from the initial navigation system to the initial carrier system by an optimization method according to the robust attitude determination matrix; the attitude matrix determination module is used to perform a posteriori estimation of the error state and the error state covariance matrix according to the observation model of the error state through the Kalman filter measurement update formula and the robust weight function, and injecting the a posteriori estimate of the error state into the a priori estimate of the nominal state to complete the a posteriori estimation of the nominal state; and combining the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system and the direction cosine matrix from the initial navigation system to the navigation system to obtain the real-time carrier attitude matrix; the alignment control module is used to control the initial alignment of the moving base according to the real-time carrier attitude matrix.

[0262] All relevant contents of each step involved in the embodiment of the moving base initial alignment method of the aforementioned integrated navigation system can be referred to the functional description of the functional modules corresponding to the moving base initial alignment system of the integrated navigation system in the embodiment of the present invention, and will not be repeated here.

[0263] The division of modules in the embodiments of the present invention is schematic and is only a logical function division. There may be other division methods in actual implementation. In addition, each functional module in each embodiment of the present invention may be integrated into one processor, or may exist physically separately, or two or more modules may be integrated into one module. The above-mentioned integrated modules may be implemented in the form of hardware or in the form of software functional modules.

[0264] In another embodiment of the present invention, a computer device is provided, the computer device comprising a processor and a memory, the memory being used to store a computer program, the computer program comprising program instructions, and the processor being used to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or may be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc., which are the computing core and control core of the terminal, and are suitable for implementing one or more instructions, and are specifically suitable for loading and executing one or more instructions in the computer storage medium to implement the corresponding method flow or corresponding function; the processor described in the embodiment of the present invention can be used for the operation of the initial alignment method of the moving base of the integrated navigation system.

[0265] In another embodiment of the present invention, the present invention also provides a storage medium, specifically a computer-readable storage medium (Memory), which is a memory device in a computer device for storing programs and data. It is understandable that the computer-readable storage medium here can include both the built-in storage medium in the computer device and the extended storage medium supported by the computer device. The computer-readable storage medium provides a storage space, which stores the operating system of the terminal. In addition, one or more instructions suitable for being loaded and executed by the processor are also stored in the storage space, and these instructions can be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory or a non-volatile memory, such as at least one disk memory. The processor can load and execute one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the initial alignment method of the moving base of the combined navigation system in the above embodiment.

[0266] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0267] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0268] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0269] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0270] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A method for initial alignment of a moving base of an integrated navigation system, characterized in that: include: Based on the error-free observation vector model and the reference vector model, the IMU output is modeled and the reference vector, the direction cosine matrix from the initial load system to the load system and the IMU measurement zero bias are used as state variables to establish the state prediction model and state measurement model of the real state. According to the relationship between the real state, the error state and the nominal state, the error state prediction model and measurement model, as well as the nominal state prediction model are obtained. Receive the IMU output, and obtain the error state, error state covariance matrix and a priori estimate of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until the GNSS output of a new frame is received; And when a new frame of GNSS output is received, the direction cosine matrix from the initial navigation system to the navigation system is updated according to the GNSS output, and an observation vector is constructed; Model the GNSS output and construct a robust weight function through extended modulus matching based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector, and the observation vector measurement value; The robust attitude determination matrix is ​​constructed by using the robust weight function, the measured value of the observation vector and the prior estimate of the nominal reference vector, and the nominal direction cosine matrix from the initial navigation system to the initial load system is solved by the optimization method according to the robust attitude determination matrix; According to the observation model of the error state, the error state and the error state covariance matrix are estimated a posteriori through the Kalman filter measurement update formula and the robust weight function, and the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state to complete the a posteriori estimate of the nominal state; and the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system are combined to obtain the real-time carrier attitude matrix; The initial alignment of the moving base is controlled according to the real-time carrier attitude matrix.

2. The method for initial alignment of a moving base of an integrated navigation system according to claim 1, characterized in that: The error-free observation vector model and the reference vector model are derived based on the specific force equation and the chain rule of the direction cosine matrix; according to the specific force equation and the chain rule of the direction cosine matrix, we get: in, represents the differential of the navigation system velocity; Represents the direction cosine matrix from the navigation coordinate system to the initial navigation system; Represents the direction cosine matrix from the initial navigation system to the initial load system; represents the direction cosine matrix from the initial load system to the load system; f b Represents the true value of accelerometer measurement; It represents the projection of the angular velocity of the Earth system relative to the inertial system in the navigation system; represents the projection of the rotational angular velocity of the navigation system relative to the Earth system in the navigation system; × represents the vector cross multiplication operation; v n Indicates the navigation system speed; g n It represents the projection of gravitational acceleration in the navigation system; Multiply both sides of the above equation by Integrating both sides simultaneously, Taking it out from the integral, we get: Among them, β represents the observation vector; α represents the reference vector; represents the direction cosine matrix from the initial navigation system to the navigation system; × represents the vector cross multiplication operation; The IMU output is modeled and the reference vector, the direction cosine matrix from the initial load system to the load system, and the IMU measurement zero bias are used as state variables to establish a state prediction model and a state measurement model of the real state, and according to the relationship between the real state, the error state, and the nominal state, the error state prediction model and the measurement model, as well as the nominal state prediction model include: Model the IMU output and obtain the IMU output model: in, represents the accelerometer measurement value; b a Indicates the accelerometer measurement zero bias; n a represents the accelerometer measuring Gaussian white noise; Represents the gyroscope measurement value; Indicates the true value of gyroscope measurement; b g Indicates the gyroscope measurement bias; n g represents the gyroscope measurement Gaussian white noise; Define the true state as: Wherein, T represents the transpose operation; The nominal state is defined as: in, represents the nominal reference vector; represents the nominal direction cosine matrix from the initial load system to the load system; Indicates the nominal accelerometer measurement bias; Indicates the nominal gyro measurement bias; Define the error state as: Among them, δα represents the error reference vector; δθ represents the attitude error vector; δb a Indicates the error accelerometer measurement bias; δb g Indicates the error gyro measurement bias; The relationship between the true state, nominal state and error state is established as: Where exp(·) represents the exponential function; δθ× represents the antisymmetric matrix of the attitude error vector; Among them, exp(δθ×) can be calculated by the following formula: Where I3 represents the identity matrix of 3 rows and 3 columns; ||δθ|| represents the 2-norm of the attitude error vector; According to the model of IMU measurement output and the IMU zero bias model as a random walk process, the prediction model of the real state can be obtained as follows: in, represents the differential of the reference vector; Represents the differential of the direction cosine matrix from the initial load system to the load system; represents the gyroscope measurement value; (a)× represents the antisymmetric matrix of vector a; represents the differential of the accelerometer measurement bias; Represents the differential of the gyroscope measurement bias; The prediction model for the nominal state is defined as: in, represents the differential of the nominal reference vector; represents the differential of the nominal direction cosine matrix of the initial load system to the load system; represents the gyroscope measurement value; (a)× represents the antisymmetric matrix of vector a; Represents the differential of the nominal accelerometer measurement bias; 0 3×1 represents a 3D zero vector; represents the differential of the nominal gyro measurement bias; According to the differential equations of the true state and the nominal state, and the relationship between the true state, the nominal state and the error state, the prediction model of the error state is obtained: in, represents the differential of the error reference vector; Represents the accelerometer measurement value; represents the differential of the attitude error vector; Represents the gyroscope measurement value; Indicates the nominal gyro measurement bias; represents the differential of the error accelerometer measurement bias; represents the differential of the error gyro measurement bias; define i as the index value of the IMU measurement value, and define Δt s is the IMU measurement period, and the discretized error state prediction model can be obtained as follows: δx i+1 =F i δx i +w i Among them, δx i+1 Indicates t i+1 Error state at time; δx i Indicates t i The error state at the moment; represents the state noise vector; F i Indicates t i The state transfer matrix at time; Define k as the index value of the GNSS measurement value, and define Δt g For the GNSS measurement cycle, the observation model of the error state is constructed through the vector observer: z k =H k δx k +n v,k in, Indicates t k =kΔt g The observed value at time; Indicates t k The observation matrix at time; Indicates t k Error state at time n v,k Indicates t k The measurement noise vector at time; Indicates t k The observation vector constructed from the GNSS measured velocity at the time; Indicates t k The direction cosine matrix from the initial navigation system to the initial load system obtained by the optimization method at each moment; Indicates t k Nominal reference vector at time; 0 3×9 Represents a zero matrix with 3 rows and 9 columns.

3. The method for initial alignment of a moving base of an integrated navigation system according to claim 1, characterized in that: The receiving IMU output and obtaining the error state, the error state covariance matrix and the prior estimation of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state, until the GNSS output of a new frame is received includes: Based on the established state prediction model of the error state, the IMU output is received and the error state and the error state covariance matrix are estimated a priori through the Kalman filter prediction update formula: in, Indicates t i+1 A priori estimate of the error state at the moment; F i Indicates t i The state transfer matrix at time; Indicates t i A priori estimate of the error state at that moment; Indicates t k-1 A priori estimate of the error state at that moment; Indicates t k-1 A posteriori estimate of the error state at that moment; Indicates t i+1 Prior estimate of the error state covariance matrix at the moment; P i - Indicates t i A priori estimate of the error state covariance matrix at the moment; Q i Indicates t i+1 The state noise covariance matrix at the moment; Indicates t k-1 A priori estimate of the error state covariance matrix at each moment; Indicates t k-1 The posterior estimate of the error state covariance matrix at the moment; The a priori estimate of the nominal state is given by: in, Indicates t i+1 A priori estimate of the nominal reference vector at time instant; Indicates t i A priori estimate of the nominal reference vector at time instant; Indicates t i The a priori estimate of the direction cosine matrix from the initial carrier system to the carrier system at time Δt s Represents the IMU sampling interval; I3 represents the identity matrix of 3 rows and 3 columns; Indicates t i Gyroscope measurement value at the moment; Indicates t i A priori estimate of the nominal gyro measurement bias at time; (a)× represents the antisymmetric matrix of vector a; Indicates t i Accelerometer measurement value at the moment; Indicates t i A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A priori estimate of the nominal reference vector at time instant; Indicates t k-1 A posteriori estimate of the nominal reference vector at time instant; Indicates t i+1 The a priori estimate of the direction cosine matrix from the initial load system to the load system at time; exp(·) represents the exponential function; Indicates t k-1 A priori estimate of the direction cosine matrix from the initial load system to the load system at time instant; Indicates t k-1 The a posteriori estimate of the direction cosine matrix from the initial load system to the load system at time instant; Indicates t i+1 A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k-1 A posteriori estimate of the nominal accelerometer measurement bias at time instant; Indicates t i+1 A priori estimate of the nominal gyro measurement bias at that moment; Indicates t k-1 A priori estimate of the nominal gyro measurement bias at that moment; Indicates t k-1 A posteriori estimate of the nominal gyro measurement bias at time instant; The error state, error state covariance matrix and prior estimation of the nominal state are performed with the arrival of the IMU output until a new frame of GNSS output arrives, and the error reference vector, the covariance matrix of the error reference vector and the prior estimation of the nominal reference vector are obtained.

4. The method for initial alignment of a moving base of an integrated navigation system according to claim 1, characterized in that: When a new frame of GNSS output is received, the direction cosine matrix from the initial navigation system to the navigation system is updated according to the GNSS output, and the observation vector is constructed, including: Receive GNSS output and construct observation vector: in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k The nominal reference vector at time instant; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t k GNSS velocity measurement value at the moment; Indicates the GNSS velocity measurement value at the initial moment; Indicates t j The direction cosine matrix from the initial navigation system to the navigation system at time Δt g represents the GNSS sampling interval; I3 represents the identity matrix of 3 rows and 3 columns; Indicates t j The projection of the angular velocity of the navigation system relative to the inertial system at the moment in the navigation system; (a)× represents the antisymmetric matrix of vector a; Indicates t j The projection of the earth's rotation angular velocity at the moment in the navigation system; Indicates t j The projection of the gravitational acceleration at the moment in the navigation system; Indicates t j+1 The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l+1 The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t l The projection of the angular velocity of the navigation system relative to the inertial system at the time in the navigation system; Δt g represents the GNSS sampling interval; exp(·) represents the exponential function.

5. The method for initial alignment of a moving base of an integrated navigation system according to claim 1, characterized in that: The GNSS output is modeled and based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector and the observation vector measurement value, a robust weight function is constructed by extended modulus matching, including: Model the GNSS output and obtain the GNSS output model: in, Indicates t k GNSS measurement speed at the moment; v n Indicates t k The true speed of time; Indicates t k GNSS measurement velocity error value at the moment; Substituting the GNSS measured velocity into the observation vector yields: in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; β k Indicates t k The true value of the observation vector at each moment; δβ k Indicates t k The error observation vector at time; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time; The vector modulus square error is defined as: Among them, ε k Indicates t k The square error of the modulus value at the moment; Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time ||·|| 2 Represents the modulus square operation; Substituting into the error relationship, we can get the normalized modulus square error: in, Indicates t k Normalized modulus square error at time; Var[·] represents variance operation; ||·|| 2 Represents the modulus square operation; Indicates t k The observation vector constructed using the GNSS velocity measurement at each moment; T represents the transpose operation; σ k Indicates t k The standard deviation of the GNSS velocity noise at the moment; Indicates t k A priori estimate of the time error reference vector; Indicates t k A priori estimate of the nominal reference vector at time instant; Represented by the matrix The matrix consisting of the first three rows and first three columns of ; Based on Huber robust filtering theory, a robust weight function is constructed by normalizing the square error of the modulus value: in, represents the robust weight function; γ represents the Gaussian threshold; sgn(·) represents the sign function.

6. The method for initial alignment of a moving base of an integrated navigation system according to claim 1, characterized in that: The method of constructing a robust attitude determination matrix using a robust weight function, an observation vector measurement value, and a priori estimated value of a nominal reference vector, and solving a nominal direction cosine matrix from an initial navigation system to an initial load system by an optimization method according to the robust attitude determination matrix includes: Construct the robust pose determination matrix: Among them, Y k Indicates t k The robust attitude determination matrix at time ; Y k-1 Indicates t k-1 The robust attitude determination matrix at time Δt g Indicates the GNSS sampling interval; Robust weight function representing the normalized modulus square error; Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time instant; in, and The expression is as follows: in, Indicates t k The observation vector constructed using GNSS velocity measurements at all times; Indicates t k A priori estimate of the nominal reference vector at time; T represents the transpose operation; (a)× represents the antisymmetric matrix of vector a; Solving for Y k The eigenvector corresponding to the minimum eigenvalue of is obtained, and the quaternion corresponding to the direction cosine matrix of the optimal initial navigation system to the initial load system is obtained. Then, through the conversion formula of the quaternion and the direction cosine matrix, t is solved. k The nominal direction cosine matrix from the initial navigation system to the initial load system at time 7. The method for initial alignment of a moving base of an integrated navigation system according to claim 1, characterized in that: According to the observation model of the error state, the error state and the error state covariance matrix are estimated a posteriori by using the Kalman filter measurement update formula and the robust weight function, and the a posteriori estimate of the error state is injected into the a priori estimate of the nominal state to complete the a posteriori estimate of the nominal state; and the a posteriori estimate of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system are combined to obtain the real-time carrier attitude matrix, including: The posterior estimation of the error state and the error state covariance matrix is ​​performed by the following formula: Among them, H k Indicates t k The observation matrix at time; Indicates t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time 0; 3×9 represents a zero matrix with 3 rows and 9 columns; z k Indicates t k The observed value at time; Indicates t k The prior estimate of the error state vector at the moment K k Indicates t k The Kalman gain matrix at time t; Indicates t k A priori estimate of the covariance matrix of the error state vector at each moment; R represents the robust weight function of the normalized squared error of the modulus; k Indicates t k The measurement noise matrix at time; Indicates t k A posteriori estimate of the error state vector at the moment; Indicates t k The posterior estimate of the covariance matrix of the error state vector at the moment I 12 Represents the identity matrix with 12 rows and 12 columns; according to get: in, Indicates t k The a posteriori estimate of the time error reference vector; (m:n) represents the operation of taking the m to n elements of the vector; Indicates t k A posteriori estimate of the moment error attitude angle vector; Indicates t k A posteriori estimate of the accelerometer bias at the instant of error; Indicates t k A posteriori estimate of the time error gyro bias; The posterior estimate of the error state is injected into the prior estimate of the nominal state, thus completing the posterior estimate of the nominal state: in, Indicates t k A posteriori estimate of the nominal reference vector at time instant; Indicates t k A priori estimate of the nominal reference vector at time instant; Indicates t k A posteriori estimate of the time error reference vector; Indicates t k The a posteriori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t; Indicates t k A priori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t; Indicates t k A posteriori estimate of the moment error attitude angle vector; Indicates t k The a posteriori estimate of the nominal accelerometer measurement bias at time t; Indicates t k A priori estimate of the nominal accelerometer measurement bias at time instant; Indicates t k The a posteriori estimate of the error accelerometer measurement bias at time instant; Indicates t k The a posteriori estimate of the nominal gyro measurement bias at time t; Indicates t k A priori estimate of the nominal gyro measurement bias at time instant; Indicates t k The a posteriori estimate of the gyro measurement bias error at time Set to zero vector and enter the next round of state estimation; The real-time carrier posture matrix is ​​obtained by the following formula: in, Indicates t k The carrier posture matrix at the moment; Indicates t k The direction cosine matrix from the initial navigation system to the navigation system at time; Indicates t k The nominal direction cosine matrix from the initial navigation system to the initial load system at time; Indicates t k The a posteriori estimate of the nominal direction cosine matrix of the initial load system to the load system at time t.

8. A dynamic base initial alignment system for an integrated navigation system, characterized in that: include: A model building module is used to model the IMU output based on the error-free observation vector model and the reference vector model, and to establish a state prediction model and a state measurement model of the real state by taking the reference vector, the direction cosine matrix from the initial load system to the load system, and the IMU measurement zero bias as state variables, and to obtain the prediction model and measurement model of the error state and the prediction model of the nominal state according to the relationship between the real state, the error state, and the nominal state; The data processing module is used to receive the IMU output and obtain the error state, the error state covariance matrix and the prior estimation of the nominal state through the Kalman filter prediction update formula according to the prediction model of the error state and the nominal state until the GNSS output of a new frame is received; And when a new frame of GNSS output is received, the direction cosine matrix from the initial navigation system to the navigation system is updated according to the GNSS output, and an observation vector is constructed; A state update module is used to model the GNSS output and construct a robust weight function through extended modulus matching based on the error reference vector, the error reference vector covariance matrix, the prior estimate of the nominal reference vector, and the observation vector measurement value; A solution module is used to construct a robust attitude determination matrix using a robust weight function, an observation vector measurement value and a priori estimated value of a nominal reference vector, and solve a nominal direction cosine matrix from an initial navigation system to an initial load system by an optimization method according to the robust attitude determination matrix; The attitude matrix determination module is used to perform a posteriori estimation of the error state and the error state covariance matrix according to the observation model of the error state through the Kalman filter measurement update formula and the robust weight function, and inject the a posteriori estimation of the error state into the a priori estimation of the nominal state to complete the a posteriori estimation of the nominal state; and combine the a posteriori estimation of the direction cosine matrix from the initial carrier system to the carrier system, the nominal direction cosine matrix from the initial navigation system to the initial carrier system, and the direction cosine matrix from the initial navigation system to the navigation system to obtain the real-time carrier attitude matrix; The alignment control module is used to control the initial alignment of the moving base according to the real-time carrier posture matrix.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method for initial alignment of a moving base of an integrated navigation system as claimed in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method for initial alignment of a moving base of an integrated navigation system as claimed in any one of claims 1 to 7 are implemented.