A SINS fast alignment method based on Lie group and factor graph under large misalignment angle

Abstract

The application relates to a SINS large misalignment angle fast alignment method based on a Lie group and a factor graph, and belongs to the technical field of large misalignment angle alignment, and comprises the following steps: S1, defining system states under a Lie group framework; S2, constructing a factor graph model of the SINS / GNSS large misalignment angle fast alignment; S3, constructing an SINS factor cost function under the Lie group framework; S4, constructing a GNSS factor cost function under the Lie group framework; S5, optimizing the system states under the Lie group framework by using the factor graph model, the SINS factor cost function and the GNSS factor cost function, and obtaining optimal estimation of the system states; and S6, obtaining a theoretical value of a Lie group matrix containing elements related to an attitude, a speed and a position according to the optimal estimation of the system states, that is, completing the SINS large misalignment angle fast alignment method. The application has the advantages of strong robustness, good precision, simple operation, fast convergence, low cost, high alignment precision and good practicability.

Description

Technical Field

[0001] This invention relates to the field of large misalignment angle alignment technology, and particularly to a fast alignment method for large misalignment angle SINS based on Lie groups and factor diagrams. Background Technology

[0002] Strapdown inertial navigation systems (SINS) are autonomous navigation systems widely used in continuous and high-reliability positioning scenarios. They are typically integrated with Global Navigation Satellite Systems (GNSS) to enhance navigation performance. In recent years, SINS / GNSS has become an important navigation equipment for ground vehicles, and its initial alignment accuracy and alignment time directly affect subsequent navigation accuracy and preparation time for entering operational status. Therefore, initial alignment performance is a key indicator of a navigation system.

[0003] Using the nonlinear error model of SINS for single-stage alignment skips the coarse alignment process, enabling alignment with large misalignment angles. For example, invention patent CN105806363A proposes a SINS / DVL underwater large misalignment angle alignment method based on SRQKF; invention patent CN108225373A discloses a large misalignment angle alignment method based on an improved 5th-order capacitive Kalman filter; and invention patent CN106949906A proposes a fast large misalignment angle alignment method based on an integral extended state observer.

[0004] However, traditional large misalignment angle alignment methods based on nonlinear Kalman filtering are generally not suitable for rapid alignment, and their accuracy is also difficult to achieve the same level as traditional two-stage alignment methods. Summary of the Invention

[0005] In view of the shortcomings of the prior art, the purpose of this invention is to provide a fast alignment method for large misalignment angles of SINS based on Lie groups and factor graphs, so as to solve or improve the defects existing in the prior art.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A fast alignment method for large misalignment angles in SINS based on Lie groups and factor graphs includes the following steps:

[0008] S1. Define the system state under the Lie group framework;

[0009] S2. Construct a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles;

[0010] S3. Based on the system state under the Lie group framework defined in step S1, construct the SINS factor cost function under the Lie group framework.

[0011] S4. Based on the system state under the Lie group framework defined in step S1, construct the GNSS factor cost function under the Lie group framework.

[0012] S5. Optimize the system state under the Lie group framework defined in step S1 using the factor graph model constructed in step S2, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 to obtain the optimal estimate of the system state.

[0013] S6. Based on the system state under the Lie group framework defined in step S1, and according to the optimal estimate of the system state obtained in step S5, obtain the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity and position, thus completing the SINS large misalignment angle fast alignment.

[0014] Preferably, in step S1, the specific method for defining the system state under the Lie group framework is as follows:

[0015] A combined navigation system was constructed using MEMSIMU and GNSS RTK, and the group error of SINS was modeled as the system state.

[0016] For initial alignment during navigation, the attitude, velocity, and position-related states are defined on a Lie group matrix, while the IMU bias null is defined in traditional Euclidean space, represented as:

[0017]

[0018] In the formula, χ∈SE2(3) represents the theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position. This represents the theoretical value of the attitude matrix in the b-frame relative to the n-frame, where n is the navigation coordinate system and b is the vehicle coordinate system. This represents the theoretical value of the velocity vector in the n-frame relative to the Earth coordinate system. This represents the theoretical value of the position vector in the n-system relative to the Earth coordinate system, 0 1×3 θ represents a 1x3 zero matrix; θ represents the IMU zero bias vector. and These are the zero-bias vectors of the gyroscope and the accelerometer, respectively;

[0019] Based on the properties of Lie groups, we have:

[0020]

[0021] In the formula, χ -1 Represent the inverse of χ; This represents the theoretical value of the attitude matrix of the n-system relative to the b-system;

[0022] Left-invariant error η is selected for processing:

[0023]

[0024] In the formula, This represents the nominal state of χ. express The inverse element; This represents the actual value of the attitude matrix of the n-frame relative to the b-frame. This represents the actual velocity of the download system relative to the Earth coordinate system in the b-frame. This represents the actual position of the download system relative to the Earth coordinate system in the b-frame.

[0025] Within the Lie group framework, the new attitude error, velocity error, and position error are defined as follows:

[0026]

[0027] In the formula, φ n The angle of inaccuracy is φ, where × represents the antisymmetric matrix generated from the three-dimensional vector. n × represents φ n antisymmetric matrix, Let I represent the attitude matrix corresponding to the attitude misalignment angle, and J represent the Jacobian matrix according to the Rodrigues formula. and Let these represent the velocity error and position error in Lie algebraic form, respectively. and These represent the new velocity error and the new position error, respectively. This represents the actual velocity of the download system relative to the Earth coordinate system in the n-frame. δv represents the actual position of the n-system relative to the Earth coordinate system. n δp represents the velocity error in the n-system, i.e., the difference between the actual velocity value and the theoretical velocity value. n This represents the position error in the n-system, that is, the difference between the actual position value and the theoretical position value;

[0028] By the properties of Lie groups and Lie algebras, the left-invariant error η satisfies:

[0029]

[0030] In the formula, Λ(ξ) represents the linear isomorphism that maps the vector space to the Lie algebra. Let represent the error in the corresponding Lie algebraic form, and T represent the transpose of the matrix;

[0031] The expression for the Jacobian matrix J in the Rodriguez formula is:

[0032]

[0033] In the formula, α is φ nThe absolute value;

[0034] Therefore, the system state x under the Lie group framework is defined as:

[0035]

[0036] Preferably, in step S2, the specific method for constructing the factor graph model for rapid alignment of SINS / GNSS with large misalignment angles is as follows:

[0037] By treating GNSS measurement information as unary factor nodes and IMU measurement information as binary factor nodes, a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles is constructed, consisting of prior factor nodes, navigation variable nodes, SINS factor nodes, and GNSS factor nodes.

[0038] Where, the prior factor node is denoted as Navigation variable node is denoted as X i Let i be the time point, i = 1, 2, ..., k; let f be the SINS factor node. sins GNSS factor nodes are denoted as f gnss The velocity information provided by GNSS factor nodes is denoted as i represents time point, i = 1, 2, ..., k;

[0039] The factor graphical model has only one prior factor node. It is directly connected to the navigation variable node X1 at the first moment; navigation variable node X i There are k navigation variable nodes X at adjacent time points. i Between them via SINS factor node f sins Connect, navigation variable node X i With GNSS factor node f gnss One-to-one correspondence connection.

[0040] Preferably, in step S3, the specific method for constructing the SINS factor under the Lie group framework based on the system state defined in step S1 is as follows:

[0041] Left-invariant error state equation of Lie groups for:

[0042]

[0043] In the formula, F is the state transition matrix of the left-invariant error model, G is the noise transition matrix of the left-invariant error model, and ω is the system noise vector. g and ω a These are Gaussian white noise for the gyroscope and accelerometer, respectively.

[0044] The specific formulas for F and G are obtained from the Lie group error differential equation, as follows:

[0045]

[0046] In the formula, This represents the actual value of the angular velocity output by the gyroscope. express antisymmetric matrix, This represents the actual projected value of the Earth's rotational angular velocity in the n-frame. express antisymmetric matrix, This represents the actual value of the attitude matrix of the n-frame relative to the b-frame. This represents the actual value of the attitude matrix of the b-system relative to the n-system. This represents the actual value of the specific force vector measured by the accelerometer. express antisymmetric matrix, The actual value of the velocity vector in the n-frame. express antisymmetric matrix, Let n be the actual value of the position vector of the download system relative to the Earth coordinate system. express antisymmetric matrix, express The antisymmetric matrix, I 3×3 It is a 3x3 identity matrix, 0 3×3 It is a 3x3 zero matrix, where M1, M2, and M3 are meaningless symbols;

[0047] The specific formulas for M1, M2, and M3 are as follows:

[0048]

[0049] In the formula, R represents the theoretical projection of the Earth's rotational angular velocity in the n-frame. N R is the radius of curvature of the zonal circle. M It is the radius of curvature of the meridian. h and v represent latitude and altitude respectively. E Indicates the eastward velocity, v N Indicates northbound speed;

[0050] The left-invariant error state equation of the Lie group Discretization yields:

[0051]

[0052] f LI (x k-1,u k )=x k-1 +Fx k-1 dt;

[0053] In the formula, x k Let x represent the system state at time k. k-1 dt represents the system state at time k-1, and dt represents the time interval. Let f represent the system noise of SINS at time k. LI (x k-1 ,u k ) represents the system state equation of SINS within the Lie group framework, u k This represents the input of SINS at time k;

[0054] Therefore, the SINS factor cost function under the Lie group framework is expressed as:

[0055]

[0056] Preferably, in step S4, the specific method for constructing the GNSS factor under the Lie group framework based on the system state defined in step S1 is as follows:

[0057] Using GNSS velocity information as the initial alignment observation, the theoretical relationship between the velocity vectors of the IMU center and the GNSS antenna phase center satisfies:

[0058]

[0059] In the formula, and These represent the theoretical values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. This represents the theoretical value of the projection of the rotational angular velocity vector of the n-frame relative to the i-frame onto the n-frame, where the i-frame is an inertial coordinate system. express antisymmetric matrix, Let l represent the theoretical value of the attitude matrix of the b-system relative to the n-system. b The lever arm length between the phase center of the IMU and the GNSS antenna, l b × indicates l b antisymmetric matrix, This represents the theoretical value of the angular velocity output by the gyroscope;

[0060] Considering velocity and attitude errors, and taking into account the zero bias of the IMU gyroscope, the expression for the velocity of the GNSS antenna phase center calculated by the IMU is as follows:

[0061]

[0062] In the formula, and These represent the actual values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. This represents the actual value of the attitude matrix of the b-system relative to the n-system. This represents the actual angular velocity output by the gyroscope, where I represents the identity matrix, and φ... n For attitude misalignment angle, φ n × represents φ n antisymmetric matrix, This represents the theoretical value of the attitude matrix of the b-system relative to the n-system;

[0063] The actual values ​​of the inertial navigation velocity and the actual values ​​of the gyro angular velocity are defined as follows:

[0064]

[0065] In the formula, δv n This represents the velocity error in the n-system. This represents the theoretical value of the angular velocity output by the gyroscope. This indicates the output error of the gyroscope. ω represents the zero bias vector of the gyroscope. g Gaussian white noise representing a gyroscope;

[0066] Substituting the actual values ​​of the inertial navigation velocity and the gyro angular velocity into the expression for the velocity of the GNSS antenna phase center calculated by the IMU, and omitting the second-order terms related to the error, we obtain:

[0067]

[0068] Actual speed measured by GNSS for:

[0069]

[0070] In the formula, n v This is white noise for GNSS velocity measurement.

[0071] velocity observation vector z sensor This is expressed as the difference between the velocity of the GNSS antenna phase center calculated by the IMU and the velocity actually measured by the GNSS, i.e.:

[0072]

[0073] From the new velocity error formula defined in step S2 within the Lie group framework, the velocity error in the n-system is obtained as follows:

[0074]

[0075] Substituting the velocity error in the n-system into the velocity observation vector formula, we obtain the measurement equation, which includes the group state form:

[0076]

[0077] Discretizing the measurement equations, which contain group state forms, yields:

[0078]

[0079] In the formula, h represents the RTK measurement of the system state at time k. LI (x k ) represents the measurement equation of the system state at time k using RTK. This is represented as Gaussian noise in the RTK measurement equation;

[0080] Therefore, the GNSS factor cost function under the Lie group framework is expressed as:

[0081]

[0082] Preferably, in step S5, the specific method for optimizing the system state under the Lie group framework defined in step S1 using the factor graph model constructed in step S2, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 to obtain the optimal estimate of the system state is as follows:

[0083] The system state under the Lie group framework defined in step S1 is optimized using the factor graph model constructed in step S2:

[0084] In the factor graph model, each factor node represents the probability density function of each sensor measurement. The probability density function of the SINS factor node is expressed as p(x k |x k-1 ,u k The probability density function of GNSS factor nodes is expressed as follows: The optimization problem of the system state using a factorial graphical model can then be expressed as finding the maximum value of the joint probability density function:

[0085]

[0086] In the formula, For the navigation state set to be optimized, X * To optimize the solution state set, p(x0) is a known prior probability;

[0087] Finding the maximum value of the joint probability density function is equivalent to minimizing the cost function of the factor nodes in the factor graph model. By minimizing the cost function, the optimal estimate of the system state x is obtained. * The expression is:

[0088]

[0089] In the formula, The cost function for the SINS factor. The GNSS factor cost function, The prior factor cost function;

[0090] The prior factor cost function is expressed as:

[0091]

[0092] Where x0 is the initial navigation state and u0 is the mean of the initial navigation state;

[0093] Substituting the prior factor cost function, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 into the optimal estimate of the system state x * In the expression, the optimal estimate x of the system state is obtained. * .

[0094] Preferably, in step S6, based on the system state under the Lie group framework defined in step S1, and according to the optimal estimate of the system state obtained in step S5, the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position is obtained. That is, the specific method for completing the SINS large misalignment angle fast alignment is as follows:

[0095] Based on the system state within the Lie group framework defined in step S1, and according to the optimal estimate x of the system state... * The left-invariant error η is updated to obtain the updated left-invariant error η. k ;

[0096] Based on the updated left-invariant error η k The Lie group matrix χ containing elements related to attitude, velocity, and position from step S1 is updated to obtain the updated Lie group matrix χ containing elements related to attitude, velocity, and position. k , represented as:

[0097]

[0098] In the formula, χ k This represents the updated theoretical value of the Lie group matrix, which includes elements related to attitude, velocity, and position. η represents the actual value of the Lie group matrix at time k, containing elements related to attitude, velocity, and position. k This represents the updated left-invariant error;

[0099] Because the updated theoretical value χ of the Lie group matrix includes elements related to attitude, velocity, and position. kIncluding the theoretical values ​​of the attitude matrix of the SINS to be finally solved Complete the rapid alignment of SINS with large misalignment angles.

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

[0101] This invention utilizes the system state within a Lie group framework and dynamically updates the theoretical values ​​of the Lie group matrix containing elements related to attitude, velocity, and position. This improves the alignment convergence speed and effectively reduces the nonlinear characteristics of the navigation system. Furthermore, compared to traditional filter-based alignment methods, it constructs the initial alignment problem as a factor graph model optimization problem and solves for the optimal estimate of the system state through the factor graph model. This results in stronger robustness and higher accuracy than traditional filtering methods. It is also simple to operate, converges quickly, is low-cost, provides high alignment accuracy, and is highly practical. This invention overcomes the problem that traditional nonlinear Kalman filter-based large misalignment angle alignment methods are generally unsuitable for fast alignment, and their accuracy is often difficult to match that of traditional two-stage alignment methods. Attached Figure Description

[0102] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, those skilled in the art can obtain other drawings based on the drawings described below without creative effort.

[0103] Figure 1 This is a flowchart illustrating a fast alignment method for large misalignment angles in SINS based on Lie groups and factor graphs according to the present invention.

[0104] Figure 2 This is a schematic diagram of a factor graph model for a fast alignment method for large misalignment angles in SINS based on Lie groups and factor graphs according to an embodiment of the present invention. Detailed Implementation

[0105] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention. To make the above features and advantages of this invention more apparent and understandable, specific embodiments are provided below with reference to the accompanying drawings for detailed description.

[0106] like Figure 1 and Figure 2 As shown, embodiments of the present invention are provided.

[0107] A fast alignment method for large misalignment angles in SINS based on Lie groups and factor graphs includes the following steps:

[0108] S1. Define the system state under the Lie group framework;

[0109] S2. Construct a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles;

[0110] S3. Based on the system state under the Lie group framework defined in step S1, construct the SINS factor cost function under the Lie group framework.

[0111] S4. Based on the system state under the Lie group framework defined in step S1, construct the GNSS factor cost function under the Lie group framework.

[0112] S5. Optimize the system state under the Lie group framework defined in step S1 using the factor graph model constructed in step S2, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 to obtain the optimal estimate of the system state.

[0113] S6. Based on the system state under the Lie group framework defined in step S1, and according to the optimal estimate of the system state obtained in step S5, obtain the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity and position, thus completing the SINS large misalignment angle fast alignment.

[0114] In this embodiment, the specific method for defining the system state under the Lie group framework in step S1 is as follows:

[0115] A combined navigation system was constructed using two types of sensors: MEMSIMU (Micro-Electro-Mechanical Systems Inertial Measurement Unit) and GNSS RTK (Global Navigation Satellite System Real-Time Dynamic Differential Positioning Satellite Receiver). This system employs an indirect estimation method to model the group error of SINS as the system state.

[0116] For initial alignment during navigation, the attitude, velocity, and position-related states are defined on a Lie group matrix, while the IMU bias null is defined in traditional Euclidean space, represented as:

[0117]

[0118] In the formula, χ∈SE2(3) represents the theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position. This represents the theoretical value of the attitude matrix in the b-frame relative to the n-frame, where n is the navigation coordinate system and b is the vehicle coordinate system. This represents the theoretical value of the velocity vector in the n-frame relative to the Earth coordinate system. This represents the theoretical value of the position vector in the n-system relative to the Earth coordinate system, 0 1×3θ represents a 1x3 zero matrix; θ represents the IMU zero bias vector. and These are the zero-bias vectors of the gyroscope and the accelerometer, respectively;

[0119] Based on the properties of Lie groups, we have:

[0120]

[0121] In the formula, χ -1 Represent the inverse of χ; This represents the theoretical value of the attitude matrix of the n-system relative to the b-system;

[0122] In Lie group spaces, the error is defined as either left-invariant error. And right invariant error For initial navigation alignment, the left-invariant frame exhibits better performance under large misalignment angles; therefore, the left-invariant error η is selected for handling.

[0123]

[0124] In the formula, This represents the nominal state of χ. express The inverse element; This represents the actual value of the attitude matrix of the n-frame relative to the b-frame. This represents the actual velocity of the download system relative to the Earth coordinate system in the b-frame. This represents the actual position of the download system relative to the Earth coordinate system in the b-frame.

[0125] Within the Lie group framework, the new attitude error, velocity error, and position error are defined as follows:

[0126]

[0127] In the formula, φ n The angle of inaccuracy is φ, where × represents the antisymmetric matrix generated from the three-dimensional vector. n × represents φ n antisymmetric matrix, Let I represent the attitude matrix corresponding to the attitude misalignment angle, and J represent the Jacobian matrix according to the Rodrigues formula. and Let these represent the velocity error and position error in Lie algebraic form, respectively. and These represent the new velocity error and the new position error, respectively. This represents the actual velocity of the download system relative to the Earth coordinate system in the n-frame. δv represents the actual position of the n-system relative to the Earth coordinate system. nδp represents the velocity error in the n-system, i.e., the difference between the actual velocity value and the theoretical velocity value. n This represents the position error in the n-system, that is, the difference between the actual position value and the theoretical position value;

[0128] By the properties of Lie groups and Lie algebras, the left-invariant error η satisfies:

[0129]

[0130] In the formula, Λ(ξ) represents the linear isomorphism that maps the vector space to the Lie algebra. Let represent the error in the corresponding Lie algebraic form, and T represent the transpose of the matrix;

[0131] The expression for the Jacobian matrix J in the Rodriguez formula is:

[0132]

[0133] In the formula, α is φ n The absolute value;

[0134] Therefore, the system state x under the Lie group framework is defined as:

[0135]

[0136] In this embodiment, the specific method for constructing the factor graph model for rapid alignment of SINS / GNSS with large misalignment angles in step S2 is as follows:

[0137] Since GNSS measurement information is only related to the current navigation state, it is regarded as a unary factor node; IMU measurement information connects the navigation states of the current time and the next time, so it is regarded as a binary factor node. Combining the system state under the Lie group framework defined in step S1, a factor graph model for fast alignment of SINS / GNSS large misalignment angle is constructed, consisting of prior factor nodes, navigation variable nodes, SINS factor nodes and GNSS factor nodes.

[0138] Where, the prior factor node is denoted as Navigation variable node is denoted as X i Let i be the time point, i = 1, 2, ..., k; let f be the SINS factor node. sins GNSS factor nodes are denoted as f gnss The velocity information provided by GNSS factor nodes is denoted as i represents time point, i = 1, 2, ..., k;

[0139] The factor graphical model has only one prior factor node. Located in the upper left corner of the factor graph, it is directly connected to the navigation variable node X1 at the first time step; navigation variable node Xi There are k navigation variable nodes X at adjacent time points. i Between them via SINS factor node f sins Connect, navigation variable node X i With GNSS factor node f gnss One-to-one correspondence connection.

[0140] In this embodiment, in step S3, the specific method for constructing the SINS factor under the Lie group framework based on the system state defined in step S1 is as follows:

[0141] Left-invariant error state equation of Lie groups for:

[0142]

[0143] In the formula, F is the state transition matrix of the left-invariant error model, G is the noise transition matrix of the left-invariant error model, and ω is the system noise vector. g and ω a These are Gaussian white noise for the gyroscope and accelerometer, respectively.

[0144] The specific formulas for F and G are obtained from the Lie group error differential equation, as follows:

[0145]

[0146] In the formula, This represents the actual value of the angular velocity output by the gyroscope. express antisymmetric matrix, This represents the actual projected value of the Earth's rotational angular velocity in the n-frame. express antisymmetric matrix, This represents the actual value of the attitude matrix of the n-frame relative to the b-frame. This represents the actual value of the attitude matrix of the b-system relative to the n-system. This represents the actual value of the specific force vector measured by the accelerometer. express antisymmetric matrix, The actual value of the velocity vector in the n-frame. express antisymmetric matrix, Let n be the actual value of the position vector of the download system relative to the Earth coordinate system. express antisymmetric matrix, express The antisymmetric matrix, I 3×3 It is a 3x3 identity matrix, 0 3×3It is a 3x3 zero matrix, where M1, M2, and M3 are meaningless symbols;

[0147] The specific formulas for M1, M2, and M3 are as follows:

[0148]

[0149]

[0150] In the formula, R represents the theoretical projection of the Earth's rotational angular velocity in the n-frame. N R is the radius of curvature of the zonal circle. M It is the radius of curvature of the meridian. h and v represent latitude and altitude respectively. E Indicates the eastward velocity, v N Indicates northbound speed;

[0151] The left-invariant error state equation of the Lie group Discretization yields:

[0152]

[0153] f LI (x k-1 ,u k )=x k-1 +Fx k-1 dt;

[0154] In the formula, x k Let x represent the system state at time k. k-1 dt represents the system state at time k-1, and dt represents the time interval. Let f represent the system noise of SINS at time k. LI (x k-1 ,u k ) represents the system state equation of SINS within the Lie group framework, u k This represents the input of SINS at time k;

[0155] Therefore, the SINS factor cost function under the Lie group framework is expressed as:

[0156]

[0157] In this embodiment, in step S4, the specific method for constructing the GNSS factor under the Lie group framework based on the system state defined in step S1 is as follows:

[0158] Using GNSS velocity information as the initial alignment observation can suppress the divergence of inertial navigation calculation results. The theoretical relationship between the velocity vectors of the IMU center and the GNSS antenna phase center satisfies:

[0159]

[0160] In the formula, and These represent the theoretical values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. This represents the theoretical value of the projection of the rotational angular velocity vector of the n-frame relative to the i-frame onto the n-frame, where the i-frame is an inertial coordinate system. express antisymmetric matrix, Let l represent the theoretical value of the attitude matrix of the b-system relative to the n-system. b The lever arm length between the phase center of the IMU and the GNSS antenna, l b × indicates l b antisymmetric matrix, This represents the theoretical value of the angular velocity output by the gyroscope;

[0161] Considering velocity and attitude errors, and taking into account the zero bias of the IMU gyroscope, the expression for the velocity of the GNSS antenna phase center calculated by the IMU is as follows:

[0162]

[0163] In the formula, and These represent the actual values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. This represents the actual value of the attitude matrix of the b-system relative to the n-system. This represents the actual angular velocity output by the gyroscope, where I represents the identity matrix, and φ... n For attitude misalignment angle, φ n × represents φ n antisymmetric matrix, This represents the theoretical value of the attitude matrix of the b-system relative to the n-system;

[0164] The actual values ​​of the inertial navigation velocity and the actual values ​​of the gyro angular velocity are defined as follows:

[0165]

[0166] In the formula, δv n This represents the velocity error in the n-system. This represents the theoretical value of the angular velocity output by the gyroscope. This indicates the output error of the gyroscope. ω represents the zero bias vector of the gyroscope. gGaussian white noise representing a gyroscope;

[0167] Substituting the actual values ​​of the inertial navigation velocity and the gyro angular velocity into the expression for the velocity of the GNSS antenna phase center calculated by the IMU, and omitting the second-order terms related to the error, we obtain:

[0168]

[0169] Actual speed measured by GNSS for:

[0170]

[0171] In the formula, n v This is white noise for GNSS velocity measurement.

[0172] velocity observation vector z sensor This is expressed as the difference between the velocity of the GNSS antenna phase center calculated by the IMU and the velocity actually measured by the GNSS, i.e.:

[0173]

[0174] From the new velocity error formula defined in step S2 within the Lie group framework, the velocity error in the n-system is obtained as follows:

[0175]

[0176] Substituting the velocity error in the n-system into the velocity observation vector formula, we obtain the measurement equation, which includes the group state form:

[0177]

[0178] Discretizing the measurement equations, which contain group state forms, yields:

[0179]

[0180] In the formula, h represents the RTK measurement of the system state at time k. LI (x k ) represents the measurement equation of the system state at time k using RTK. This is represented as Gaussian noise in the RTK measurement equation;

[0181] Therefore, the GNSS factor cost function under the Lie group framework is expressed as:

[0182]

[0183] In this embodiment, in step S5, the system state under the Lie group framework defined in step S1 is optimized using the factor graph model constructed in step S2, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4, to obtain the optimal estimate of the system state. The specific method is as follows:

[0184] The system state under the Lie group framework defined in step S1 is optimized using the factor graph model constructed in step S2:

[0185] In the factor graph model, each factor node represents the probability density function of each sensor measurement. The probability density function of the SINS factor node is expressed as p(x k |x k-1 ,u k The probability density function of GNSS factor nodes is expressed as follows: The optimization problem of the system state using a factorial graphical model can then be expressed as finding the maximum value of the joint probability density function:

[0186]

[0187] In the formula, For the navigation state set to be optimized, X * To optimize the solution state set, p(x0) is a known prior probability;

[0188] Finding the maximum value of the joint probability density function is equivalent to minimizing the cost function of the factor nodes in the factor graph model. By minimizing the cost function, the optimal estimate of the system state x is obtained. * The expression is:

[0189]

[0190] In the formula, The cost function for the SINS factor. The GNSS factor cost function, Let the prior factor cost function be of the form of The formula for squared Mahalanobis distance is ∑, which represents the measurement covariance matrix.

[0191] The prior factor cost function is expressed as:

[0192]

[0193] Where x0 is the initial navigation state and u0 is the mean of the initial navigation state;

[0194] Substituting the prior factor cost function, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 into the optimal estimate of the system state x *In the expression, the optimal estimate x of the system state is obtained. * .

[0195] In this embodiment, in step S6, based on the system state under the Lie group framework defined in step S1, and according to the optimal estimate of the system state obtained in step S5, the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position is obtained. That is, the specific method for completing the SINS large misalignment angle fast alignment is as follows:

[0196] Based on the system state within the Lie group framework defined in step S1, and according to the optimal estimate x of the system state... * The left-invariant error η is updated to obtain the updated left-invariant error η. k ;

[0197] Based on the updated left-invariant error η k The Lie group matrix χ containing elements related to attitude, velocity, and position from step S1 is updated to obtain the updated Lie group matrix χ containing elements related to attitude, velocity, and position. k , represented as:

[0198]

[0199] In the formula, χ k This represents the updated theoretical value of the Lie group matrix, which includes elements related to attitude, velocity, and position. η represents the actual value of the Lie group matrix at time k, containing elements related to attitude, velocity, and position. k This represents the updated left-invariant error;

[0200] Because the updated theoretical value χ of the Lie group matrix includes elements related to attitude, velocity, and position. k Including the theoretical values ​​of the attitude matrix of the SINS to be finally solved Complete the rapid alignment of SINS with large misalignment angles.

[0201] This invention utilizes the system state within a Lie group framework and dynamically updates the theoretical values ​​of the Lie group matrix containing elements related to attitude, velocity, and position. This improves the alignment convergence speed and effectively reduces the nonlinear characteristics of the navigation system. Furthermore, compared to traditional filter-based alignment methods, it constructs the initial alignment problem as a factor graph model optimization problem and solves for the optimal estimate of the system state through the factor graph model. This results in stronger robustness and higher accuracy than traditional filtering methods. It is also simple to operate, converges quickly, is low-cost, provides high alignment accuracy, and is highly practical. This invention overcomes the problem that traditional nonlinear Kalman filter-based large misalignment angle alignment methods are generally unsuitable for fast alignment, and their accuracy is often difficult to match that of traditional two-stage alignment methods.

[0202] The parts of this invention not disclosed in detail are well-known technologies in the field.

[0203] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A fast alignment method for large misalignment angles in SINS based on Lie groups and factor graphs, characterized in that, Includes the following steps: S1. Define the system state under the Lie group framework; S2. Construct a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles; S3. Based on the system state under the Lie group framework defined in step S1, construct the SINS factor cost function under the Lie group framework. S4. Based on the system state under the Lie group framework defined in step S1, construct the GNSS factor cost function under the Lie group framework. S5. Optimize the system state under the Lie group framework defined in step S1 using the factor graph model constructed in step S2, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 to obtain the optimal estimate of the system state. S6. Based on the system state under the Lie group framework defined in step S1, and according to the optimal estimate of the system state obtained in step S5, obtain the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity and position, thus completing the SINS large misalignment angle fast alignment.

2. The SINS large misalignment angle fast alignment method based on Lie group and factor graph as described in claim 1, characterized in that, In step S1, the specific method for defining the system state under the Lie group framework is as follows: A combined navigation system was constructed using MEMSIMU and GNSS RTK, and the group error of SINS was modeled as the system state. For initial alignment during navigation, the attitude, velocity, and position-related states are defined on a Lie group matrix, while the IMU bias null is defined in traditional Euclidean space, represented as: In the formula, χ∈SE2(3) represents the theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position. This represents the theoretical value of the attitude matrix in the b-frame relative to the n-frame, where n is the navigation coordinate system and b is the vehicle coordinate system. This represents the theoretical value of the velocity vector in the n-frame relative to the Earth coordinate system. This represents the theoretical value of the position vector in the n-system relative to the Earth coordinate system, 0 1×3 θ represents a 1x3 zero matrix; θ represents the IMU zero bias vector. and These are the zero-bias vectors of the gyroscope and the accelerometer, respectively; Based on the properties of Lie groups, we have: In the formula, χ -1 Represents the inverse of χ; This represents the theoretical value of the attitude matrix of the n-system relative to the b-system; Left-invariant error η is selected for processing: In the formula, This represents the nominal state of χ. express The inverse element; This represents the actual value of the attitude matrix of the n-frame relative to the b-frame. This represents the actual velocity of the download system relative to the Earth coordinate system in the b-frame. This represents the actual position of the download system relative to the Earth coordinate system in the b-system; Within the Lie group framework, the new attitude error, velocity error, and position error are defined as follows: In the formula, φ n The angle of inaccuracy is φ, where × represents the antisymmetric matrix generated from the three-dimensional vector. n × represents φ n antisymmetric matrix, Let I represent the attitude matrix corresponding to the attitude misalignment angle, and J represent the Jacobian matrix according to the Rodrigues formula. and Let these represent the velocity error and position error in Lie algebraic form, respectively. and These represent the new velocity error and the new position error, respectively. This represents the actual velocity of the download system relative to the Earth coordinate system in the n-frame. δv represents the actual position of the n-system relative to the Earth coordinate system. n δp represents the velocity error in the n-system, i.e., the difference between the actual velocity value and the theoretical velocity value. n This represents the position error in the n-system, that is, the difference between the actual position value and the theoretical position value; By the properties of Lie groups and Lie algebras, the left-invariant error η satisfies: In the formula, Λ(ξ) represents the linear isomorphism that maps the vector space to the Lie algebra. Let represent the error in the corresponding Lie algebraic form, and T represent the transpose of the matrix; The expression for the Jacobian matrix J in the Rodriguez formula is: In the formula, α is φ n The absolute value; Therefore, the system state x under the Lie group framework is defined as:

3. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 2, characterized in that, In step S2, the specific method for constructing the factor graph model for rapid alignment of SINS / GNSS with large misalignment angles is as follows: By treating GNSS measurement information as unary factor nodes and IMU measurement information as binary factor nodes, a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles is constructed, consisting of prior factor nodes, navigation variable nodes, SINS factor nodes, and GNSS factor nodes. Where, the prior factor node is denoted as Navigation variable node is denoted as X i Let i be the time point, i = 1, 2, ..., k; let f be the SINS factor node. sins GNSS factor nodes are denoted as f gnss The velocity information provided by GNSS factor nodes is denoted as i represents time point, i = 1, 2, ..., k; The factor graphical model has only one prior factor node. It is directly connected to the navigation variable node X1 at the first moment; navigation variable node X i There are k navigation variable nodes X at adjacent time points. i Between them via SINS factor node f sins Connect, navigation variable node X i With GNSS factor node f gnss One-to-one correspondence connection.

4. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 3, characterized in that, In step S3, based on the system state under the Lie group framework defined in step S1, the specific method for constructing the SINS factor under the Lie group framework is as follows: Left-invariant error state equation of Lie groups for: In the formula, F is the state transition matrix of the left-invariant error model, G is the noise transition matrix of the left-invariant error model, and ω is the system noise vector. g and ω a These are Gaussian white noise for the gyroscope and accelerometer, respectively. The specific formulas for F and G are obtained from the Lie group error differential equation, as follows: In the formula, This represents the actual value of the angular velocity output by the gyroscope. express antisymmetric matrix, This represents the actual projected value of the Earth's rotational angular velocity in the n-frame. express antisymmetric matrix, This represents the actual value of the attitude matrix of the n-frame relative to the b-frame. This represents the actual value of the attitude matrix of the b-system relative to the n-system. This represents the actual value of the specific force vector measured by the accelerometer. express antisymmetric matrix, The actual value of the velocity vector in the n-frame. express antisymmetric matrix, Let n be the actual value of the position vector of the download system relative to the Earth coordinate system. express antisymmetric matrix, express The antisymmetric matrix, I 3×3 It is a 3x3 identity matrix, 0 3×3 It is a 3x3 zero matrix, where M1, M2, and M3 are meaningless symbols; The specific formulas for M1, M2, and M3 are as follows: In the formula, R represents the theoretical projection of the Earth's rotational angular velocity in the n-frame. N R is the radius of curvature of the zonal circle. M It is the radius of curvature of the meridian. h and v represent latitude and altitude respectively. E Indicates the eastward velocity, v N Indicates northbound speed; The left-invariant error state equation of the Lie group Discretization yields: f LI (x k-1 ,u k )=x k-1 +Fx k-1 dt; In the formula, x k Let x represent the system state at time k. k-1 dt represents the system state at time k-1, and dt represents the time interval. Let f represent the system noise of SINS at time k. LI (x k-1 ,u k ) represents the system state equation of SINS within the Lie group framework, u k This represents the input of SINS at time k; Therefore, the SINS factor cost function under the Lie group framework is expressed as:

5. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 4, characterized in that, In step S4, based on the system state under the Lie group framework defined in step S1, the specific method for constructing the GNSS factor under the Lie group framework is as follows: Using GNSS velocity information as the initial alignment observation, the theoretical relationship between the velocity vectors of the IMU center and the GNSS antenna phase center satisfies: In the formula, and These represent the theoretical values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. This represents the theoretical value of the projection of the rotational angular velocity vector of the n-frame relative to the i-frame onto the n-frame, where the i-frame is an inertial coordinate system. express antisymmetric matrix, Let l represent the theoretical value of the attitude matrix of the b-system relative to the n-system. b The lever arm length between the phase center of the IMU and the GNSS antenna, l b × indicates l b antisymmetric matrix, This represents the theoretical value of the angular velocity output by the gyroscope; Considering velocity and attitude errors, and taking into account the zero bias of the IMU gyroscope, the expression for the velocity of the GNSS antenna phase center calculated by the IMU is as follows: In the formula, and These represent the actual values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. This represents the actual value of the attitude matrix of the b-system relative to the n-system. This represents the actual angular velocity output by the gyroscope, where I represents the identity matrix, and φ... n For attitude misalignment angle, φ n × represents φ n antisymmetric matrix, This represents the theoretical value of the attitude matrix of the b-system relative to the n-system; The actual values ​​of the inertial navigation velocity and the actual values ​​of the gyro angular velocity are defined as follows: In the formula, δv n This represents the velocity error in the n-system. This represents the theoretical value of the angular velocity output by the gyroscope. This indicates the output error of the gyroscope. ω represents the zero bias vector of the gyroscope. g Gaussian white noise representing a gyroscope; Substituting the actual values ​​of the inertial navigation velocity and the gyro angular velocity into the expression for the velocity of the GNSS antenna phase center calculated by the IMU, and omitting the second-order terms related to the error, we obtain: Actual speed measured by GNSS for: In the formula, n v This is white noise for GNSS velocity measurement. velocity observation vector z sensor This is expressed as the difference between the velocity of the GNSS antenna phase center calculated by the IMU and the velocity actually measured by the GNSS, i.e.: From the new velocity error formula defined in step S2 within the Lie group framework, the velocity error in the n-system is obtained as follows: Substituting the velocity error in the n-system into the velocity observation vector formula, we obtain the measurement equation, which includes the group state form: Discretizing the measurement equations, which contain group state forms, yields: In the formula, h represents the RTK measurement of the system state at time k. LI (x k ) represents the measurement equation of the system state at time k using RTK. This is represented as Gaussian noise in the RTK measurement equation; Therefore, the GNSS factor cost function under the Lie group framework is expressed as:

6. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 5, characterized in that, In step S5, the system state under the Lie group framework defined in step S1 is optimized using the factor graph model constructed in step S2, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4. The specific method for obtaining the optimal estimate of the system state is as follows: The system state under the Lie group framework defined in step S1 is optimized using the factor graph model constructed in step S2: In the factor graph model, each factor node represents the probability density function of each sensor measurement. The probability density function of the SINS factor node is expressed as p(x k |x k-1 ,u k The probability density function of GNSS factor nodes is expressed as follows: The optimization problem of the system state using a factorial graphical model can then be expressed as finding the maximum value of the joint probability density function: In the formula, For the navigation state set to be optimized, X * To optimize the solution state set, p(x0) is a known prior probability; Finding the maximum value of the joint probability density function is equivalent to minimizing the cost function of the factor nodes in the factor graph model. By minimizing the cost function, the optimal estimate of the system state x is obtained. * The expression is: In the formula, The cost function for the SINS factor. The GNSS factor cost function, The prior factor cost function; The prior factor cost function is expressed as: Where x0 is the initial navigation state and u0 is the mean of the initial navigation state; Substituting the prior factor cost function, the SINS factor cost function constructed in step S3, and the GNSS factor cost function constructed in step S4 into the optimal estimate of the system state x * In the expression, the optimal estimate x of the system state is obtained. * .

7. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 6, characterized in that, In step S6, based on the system state under the Lie group framework defined in step S1, and according to the optimal estimate of the system state obtained in step S5, the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position is obtained. The specific method for completing the SINS large misalignment angle fast alignment is as follows: Based on the system state within the Lie group framework defined in step S1, and according to the optimal estimate of the system state x... * The left-invariant error η is updated to obtain the updated left-invariant error η. k ; Based on the updated left-invariant error η k The Lie group matrix χ containing elements related to attitude, velocity, and position from step S1 is updated to obtain the updated Lie group matrix χ containing elements related to attitude, velocity, and position. k , is represented as: In the formula, χ k This represents the updated theoretical value of the Lie group matrix, which includes elements related to attitude, velocity, and position. η represents the actual value of the Lie group matrix at time k, containing elements related to attitude, velocity, and position. k This represents the updated left-invariant error; Because the updated theoretical value χ of the Lie group matrix includes elements related to attitude, velocity, and position. k Including the theoretical values ​​of the attitude matrix of the SINS to be finally solved Complete the rapid alignment of SINS with large misalignment angles.

Images