SINS large misalignment angle rapid alignment method based on Lie group and factor graph

Abstract

The invention relates to an SINS (Strapdown Inertial Navigation System) large misalignment angle rapid alignment method based on a Lie group and a factor graph, which belongs to the technical field of large misalignment angle alignment and comprises the following steps: S1, defining a system state under a Lie group framework; s2, constructing a factor graph model of SINS / GNSS large misalignment angle rapid alignment; s3, constructing an SINS factor cost function under a Lie group framework; s4, constructing a GNSS factor cost function under the Lie group framework; s5, optimizing the system state under the Lie group framework by using the factor graph model, the SINS factor cost function and the GNSS factor cost function to obtain the optimal estimation of the system state; and S6, according to the optimal estimation of the system state, obtaining an updated Lie group matrix theoretical value containing elements related to the attitude, the speed and the position, thereby completing the SINS large misalignment angle rapid alignment method. The method is high in robustness, good in precision, simple in operation, fast in convergence, low in cost, high in alignment precision and good in practicability.

Description

Technical Field

[0001] The present invention relates to the technical field of large misalignment angle alignment, and particularly relates to a method for rapid alignment of SINS with large misalignment angles based on Lie groups and factor graphs. Background Art

[0002] The strapdown inertial navigation system (SINS) is an autonomous navigation system that is widely used in continuous and highly reliable positioning scenarios. It is usually combined with the global navigation satellite system (GNSS) for integrated navigation to improve navigation performance. In recent years, SINS / GNSS has become an important navigation equipment for ground vehicles, and the accuracy and alignment time of its initial alignment directly affect the subsequent navigation accuracy and the preparation time to enter the working state. Therefore, the initial alignment performance is a key indicator of the navigation system.

[0003] Using the nonlinear error model of SINS for single-stage alignment and skipping the coarse alignment process can perform large misalignment angle alignment. For example, the invention patent with the publication number CN105806363A proposes a method for large misalignment angle alignment of SINS / DVL underwater based on SRQKF, and the invention patent with the publication number CN108225373A discloses a method for large misalignment angle alignment based on an improved fifth-order cubature Kalman filter, and the invention patent with the publication number CN106949906A proposes a method for rapid large misalignment angle alignment based on an integral type extended state observer.

[0004] However, the traditional large misalignment angle alignment method based on nonlinear Kalman filtering is usually not suitable for rapid alignment, and its accuracy is also difficult to reach the same level as that of the traditional two-stage alignment method. Summary of the Invention

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

[0006] To achieve the above purpose, the present invention adopts the following technical solutions: A method for rapid alignment of SINS with large misalignment angles based on Lie groups and factor graphs includes the following steps: S1. Define the system state in 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 defined in the Lie group framework in step S1, construct the SINS factor cost function in the Lie group framework; S4. Based on the system state defined in the Lie group framework in step S1, construct the GNSS factor cost function in the Lie group framework; S5. Optimize the system state in 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 in the Lie group framework defined in step S1 and 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, i.e., complete the rapid alignment of the large misalignment angle of SINS.

[0007] Preferably, in step S1, the specific method for defining the system state in the Lie group framework is as follows: Select MEMS IMU and GNSS RTK to construct an integrated navigation system, and model the group error of SINS as the system state; For navigation initial alignment, define the states related to attitude, velocity, and position on the Lie group matrix, while the IMU biases are defined in the traditional Euclidean space, expressed as: where χ ∈ SE2(3) represents the theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position, represents the theoretical value of the attitude matrix of the b - frame relative to the n - frame, the n - frame is the navigation coordinate system, and the b - frame is the vehicle coordinate system, represents the theoretical value of the velocity vector of the vehicle system relative to the earth coordinate system in the n - frame, represents the theoretical value of the position vector of the vehicle system relative to the earth coordinate system in the n - frame, 0 1×3 represents a 1×3 zero matrix; θ represents the IMU bias vector, and are the bias vectors of the gyroscope and accelerometer respectively; According to the properties of Lie groups: where χ -1 represents the inverse element of χ; represents the theoretical value of the attitude matrix of the n - frame relative to the b - frame; Select the left - invariant error η for processing: where represents the nominal state of χ, represents the inverse element of; represents the actual value of the attitude matrix of the n - frame relative to the b - frame, represents the actual value of the velocity of the vehicle system relative to the earth coordinate system in the b - frame, represents the actual value of the position of the vehicle system relative to the earth coordinate system in the b - frame; In the framework of Lie groups, new attitude errors, velocity errors, and position errors are defined as follows: where φ n is the attitude misalignment angle, the symbol × represents the skew-symmetric matrix generated by a three-dimensional vector, and φ n × represents the skew-symmetric matrix of φ n ; represents the attitude matrix corresponding to the attitude misalignment angle, I represents the identity matrix; J represents the Jacobian matrix of the Rodriguez formula, and represent the velocity error and position error in the form of Lie algebra, respectively, and represent the new velocity error and new position error, respectively; represents the actual velocity value of the carrier system relative to the Earth coordinate system in the n-frame, represents the actual position value of the carrier system relative to the Earth coordinate system in the n-frame; δv n represents the velocity error in the n-frame, that is, the difference between the actual velocity value and the theoretical velocity value, and δp n represents the position error in the n-frame, that is, the difference between the actual position value and the theoretical position value; Due to the characteristics of Lie groups and Lie algebras, the left-invariant error η satisfies: where Λ(ξ) represents the linear isomorphism that maps the vector space to Lie algebra, represents the corresponding error in the form of Lie algebra, and T represents the transpose of the matrix; The expression of the Jacobian matrix J of the Rodriguez formula is: where α is the absolute value of φ n ; Therefore, the system state x in the Lie group framework is defined as:

[0008] 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: Regarding the measurement information of GNSS as a unary factor node and the measurement information of IMU as a binary factor node, construct a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles, which consists of prior factor nodes, navigation variable nodes, SINS factor nodes, and GNSS factor nodes; Among them, the prior factor node is denoted as The navigation variable node is denoted as X i where i is the time, i = 1, 2,..., k; the SINS factor node is denoted as fsins ; The GNSS factor node is denoted as f gnss ; The velocity information provided by the GNSS factor node is denoted as i is the time, i = 1, 2, …, k; The factor graph model has only one prior factor node which is directly connected to the navigation variable node X1 at the first time; the navigation variable node X i There are a total of k, and the navigation variable nodes X at adjacent times i are connected through the SINS factor node f sins ; the navigation variable node X i is connected to the GNSS factor node f gnss in one-to-one correspondence.

[0009] Preferably, in step S3, based on the system state in the Lie group framework defined in step S1, the specific method for constructing the SINS factor in the Lie group framework is as follows: The left-invariant error state equation of the Lie group is: 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, ω is the system noise vector, ω g and ω a are respectively the Gaussian white noises of the gyroscope and the accelerometer; Among them, the specific formulas of F and G are obtained according to the Lie group error differential equation, as follows: In the formula, represents the actual value of the angular velocity output by the gyroscope, represents the skew-symmetric matrix of represents the actual value of the projection of the earth's angular velocity in the n system, represents the skew-symmetric matrix of represents the actual value of the attitude matrix of the n system relative to the b system, represents the actual value of the attitude matrix of the b system relative to the n system, represents the actual value of the specific force vector measured by the accelerometer, represents the skew-symmetric matrix of is the actual value of the velocity vector in the n system, represents the skew-symmetric matrix of is the actual value of the position vector of the vehicle system relative to the earth coordinate system in the n system, represents the skew-symmetric matrix of denote antisymmetric matrix of, I 3×3 is a 3×3 identity matrix, 0 3×3 is a 3×3 zero matrix, M1, M2, M3 are codes without meaning; wherein, the specific formulas of M1, M2 and M3 are respectively: In the formula, denotes the theoretical value of the projection of the angular velocity of the earth's rotation in the n system, R N is the radius of curvature of the prime vertical, R M is the radius of curvature of the meridian, and h respectively represent latitude and altitude, v E denotes the eastward velocity, v N denotes the northward velocity; Discretize the left-invariant error state equation of the Lie group to obtain: f LI (x k-1 , u k ) = x k-1 + Fx k-1 dt; In the formula, x k denotes the system state at time k, x k-1 denotes the system state at time k - 1, dt represents the time interval, denotes the system noise of the SINS at time k, f LI (x k-1 , u k ) represents the system state equation of the SINS in the Lie group framework, u k denotes the input of the SINS at time k; Therefore, represent the SINS factor cost function in the Lie group framework as:

[0010] Preferably, in step S4, based on the system state defined in step S1 in the Lie group framework, the specific method for constructing the GNSS factor in the Lie group framework is: Use the GNSS velocity information as the observable for initial alignment, and the relationship between the theoretical values of the velocity vectors of the IMU center and the GNSS antenna phase center satisfies: In the formula, and respectively denote the theoretical values of the velocity vectors of the IMU center and the GNSS antenna phase center, Denote the theoretical value of the projection of the rotational angular velocity vector of the n - frame relative to the i - frame in the n - frame. The i - frame is the inertial coordinate system. Denote the skew - symmetric matrix of Denote the theoretical value of the attitude matrix of the b - frame relative to the n - frame. b Denote the lever - arm vector between the IMU and the phase center of the GNSS antenna. b × denote b the skew - symmetric matrix of Denote the theoretical value of the angular velocity output by the gyroscope. Considering the velocity and attitude errors and taking into account the gyro bias of the IMU, the expression for the velocity of the phase center of the GNSS antenna deduced from the IMU is: In the formula, and respectively denote the actual values of the velocity vectors of the IMU center and the phase center of the GNSS antenna. Denote the actual value of the attitude matrix of the b - frame relative to the n - frame. Denote the actual value of the angular velocity output by the gyroscope. I denote the identity matrix, φ n is the attitude misalignment angle, φ n × denote φ n the skew - symmetric matrix of Denote the theoretical value of the attitude matrix of the b - frame relative to the n - frame. The actual values of the inertial navigation velocity and the gyro angular velocity are respectively defined as: In the formula, δv n denotes the velocity error in the n - frame. denotes the theoretical value of the angular velocity output by the gyroscope. denotes the output error of the gyroscope. denotes the zero - bias vector of the gyroscope, ω g denotes the Gaussian white noise of the gyro. After substituting the actual values of the inertial navigation velocity and the gyro angular velocity into the expression for the velocity of the phase center of the GNSS antenna deduced from the IMU, omitting the second - order terms regarding the errors and arranging, we get: The velocity actually measured by GNSS is: In the formula, n v is the white noise of GNSS velocity measurement. The velocity observation vector z sensor is denoted as the difference between the velocity of the phase center of the GNSS antenna deduced from the IMU and the velocity actually measured by GNSS, that is: The new velocity error formula defined in the Lie group framework in step S2 yields the velocity error in the n-frame as follows: Substituting the velocity error in the n-frame into the velocity observation vector formula gives the measurement equation in the form of group state: Discretizing the measurement equation in the form of group state results in: where represents the measured value of the system state at time k by RTK, and h LI (x k ) represents the measurement equation of the system state at time k by RTK, represents the Gaussian noise of the RTK measurement equation; Therefore, the GNSS factor cost function in the Lie group framework is expressed as:

[0011] Preferably, in step S5, the specific method for optimizing the system state in the Lie group framework defined in step S1 by 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: Optimizing the system state in the Lie group framework defined in step S1 by using the factor graph model constructed in step S2: Each factor node in the factor graph model represents the probability density function measured by each sensor. The probability density function of the SINS factor node is expressed as p(x k |x k-1 ,u k ), and the probability density function of the GNSS factor node is expressed as Then the optimization problem of the factor graph model of the system state is expressed as solving the maximum value of the joint probability density function: where is the set of navigation states to be optimized, X * is the set of optimized solution states, and p(x0) is the known prior probability; Equivalently transforming the solution of the maximum value of the joint probability density function into minimizing the cost function of the factor nodes in the factor graph model, and by minimizing the cost function, the expression for the optimal estimate x * of the system state is: In the formula, is the SINS factor cost function, is the GNSS factor cost function, is the prior factor cost function; Among them, the prior factor cost function is expressed as: Among them, x0 is the initial navigation state, and u0 is the mean value of the initial navigation state; Substitute 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 expression of the optimal estimate x of the system state * to obtain the optimal estimate x of the system state * .

[0012] 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, obtain the updated theoretical value of the Lie group matrix including elements related to attitude, velocity, and position, that is, the specific method for completing the rapid alignment of the large misalignment angle of SINS is: Based on the system state under the Lie group framework defined in step S1, according to the optimal estimate x of the system state * , update the left invariant error η to obtain the updated left invariant error η k ; According to the updated left invariant error η k , update the Lie group matrix χ including elements related to attitude, velocity, and position in step S1 to obtain the updated Lie group matrix χ including elements related to attitude, velocity, and position k , which is expressed as: In the formula, χ k represents the updated theoretical value of the Lie group matrix including elements related to attitude, velocity, and position, represents the actual value of the Lie group matrix including elements related to attitude, velocity, and position at time k, and η k represents the updated left invariant error; Since the updated theoretical value χ of the Lie group matrix including elements related to attitude, velocity, and position k includes the theoretical value of the attitude matrix of SINS to be finally solved The rapid alignment of the large misalignment angle of SINS is completed.

[0013] Compared with the prior art, the present invention has the following beneficial effects: The present invention uses the system state and dynamic update in the Lie group framework to include the theoretical values of Lie group matrices related to attitude, velocity, and position, improving the alignment convergence speed and effectively reducing the non-linear characteristics of the navigation system. At the same time, compared with the traditional filter-based alignment method, the initial alignment problem is constructed as a factor graph model optimization problem, and the optimal estimate of the system state is solved through the factor graph model. It has stronger robustness than the traditional filter and good accuracy, with simple operation, fast convergence, low cost, high alignment accuracy, and good practicability. The present invention overcomes the problem that the traditional large misalignment angle alignment method based on non-linear Kalman filter is usually not applicable to fast alignment, and at the same time, its accuracy is also difficult to reach the same level as the traditional two-stage alignment method. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description can also be obtained by those of ordinary skill in the art without creative efforts based on these drawings.

[0015] Figure 1 It is a schematic flowchart of a large misalignment angle fast alignment method for SINS based on Lie group and factor graph of the present invention.

[0016] Figure 2 It is a schematic diagram of the factor graph model of a large misalignment angle fast alignment method for SINS based on Lie group and factor graph in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0017] To make the objectives, technical solutions, and advantages of the present invention clearer, the following will clearly and completely describe the technical solutions in the present invention with reference to the drawings in the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art without creative efforts based on the embodiments in the present invention belong to the scope of protection of the present invention. To make the above features and advantages of the present invention more obvious and understandable, specific embodiments are hereby given and described in detail in conjunction with the drawings as follows.

[0018] As Figure 1 and Figure 2 shown, the embodiments of the present invention provide. A large misalignment angle fast alignment method for SINS based on Lie group and factor graph, comprising the following steps: S1. Define the system state in the Lie group framework; S2. Construct a factor graph model for fast alignment of SINS / GNSS with large misalignment angles; S3. Based on the system state in the Lie group framework defined in step S1, construct the SINS factor cost function in the Lie group framework; S4. Based on the system state in the Lie group framework defined in step S1, construct the GNSS factor cost function in the Lie group framework; S5. Use 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 optimize the system state in the Lie group framework defined in step S1, and obtain the optimal estimate of the system state; S6. Based on the system state in the Lie group framework defined in step S1, 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, that is, complete the rapid alignment of the large misalignment angle of SINS.

[0019] In this embodiment, in step S1, the specific method for defining the system state in the Lie group framework is as follows: Select two sensors, MEMS IMU (Micro-Electro-Mechanical System Inertial Measurement Unit) and GNSS RTK (Global Navigation Satellite System Real-Time Kinematic Differential Positioning Satellite Receiver), to construct a combined navigation system. This system uses the indirect estimation method to model the group error of SINS as the system state; For the initial alignment of navigation, the states related to attitude, velocity, and position are defined on the Lie group matrix, while the IMU biases are defined in the traditional Euclidean space, expressed as: In the formula, χ ∈ SE2(3) represents the theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position, represents the theoretical value of the attitude matrix of the b system relative to the n system. The n system is the navigation coordinate system, and the b system is the carrier coordinate system, represents the theoretical value of the velocity vector of the carrier system relative to the earth coordinate system in the n system, represents the theoretical value of the position vector of the carrier system relative to the earth coordinate system in the n system, 0 1×3 represents a zero matrix of 1 row and 3 columns; θ represents the IMU bias vector, and are the bias vectors of the gyroscope and accelerometer respectively; According to the properties of the Lie group, there are: In the formula, χ -1 represents the inverse element of χ; represents the theoretical value of the attitude matrix of the n system relative to the b system; The error definition in the Lie group space is divided into left-invariant error and right-invariant error For the initial alignment of navigation, under the condition of large misalignment angle, the left-invariant frame shows better performance. Therefore, the left-invariant error η is selected for processing: wherein, represents the nominal state of χ, represents the inverse element of; represents the actual value of the attitude matrix of the n-system relative to the b-system, represents the actual value of the velocity of the download system relative to the earth coordinate system in the b-system, represents the actual value of the position of the download system relative to the earth coordinate system in the b-system; In the Lie group framework, the new attitude error, velocity error, and position error are defined respectively as: wherein, φ n is the attitude misalignment angle, and the symbol × represents the skew-symmetric matrix generated by the three-dimensional vector. φ n × represents the skew-symmetric matrix of φ n , represents the attitude matrix corresponding to the attitude misalignment angle, I represents the identity matrix; J represents the Jacobian matrix of the Rodriguez formula, and represent the velocity error and position error in the form of Lie algebra respectively, and represent the new velocity error and new position error respectively; represents the actual value of the velocity of the download system relative to the earth coordinate system in the n-system, represents the actual value of the position of the download system relative to the earth coordinate system in the n-system; δv n represents the velocity error in the n-system, that is, the difference between the actual velocity value and the theoretical velocity value. δp n represents the position error in the n-system, that is, the difference between the actual position value and the theoretical position value; Due to the characteristics of Lie group and Lie algebra, the left-invariant error η satisfies: wherein, Λ(ξ) represents the linear isomorphism that maps the vector space to Lie algebra, represents the corresponding error in the form of Lie algebra, and T represents the transpose of the matrix; The expression of the Jacobian matrix J of the Rodriguez formula is: wherein, α is the absolute value of φ n ; Therefore, the system state x in the Lie group framework is defined as:

[0020] In this embodiment, 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: Since the measurement information of GNSS is only related to the current navigation state, the measurement information of GNSS is regarded as a unary factor node; the measurement information of the IMU connects the navigation states of two consecutive moments, the current moment and the next moment, and the measurement information of the IMU is regarded as a binary factor node. Combining the system state in the Lie group framework defined in step S1, a factor graph model for rapid alignment of SINS / GNSS with large misalignment angles is constructed, which consists of a prior factor node, navigation variable nodes, SINS factor nodes, and GNSS factor nodes; Among them, the prior factor node is denoted as The navigation variable node is denoted as X i , where i represents the moment, i = 1, 2,..., k; the SINS factor node is denoted as f sins ; the GNSS factor node is denoted as f gnss ; the velocity information provided by the GNSS factor node is denoted as where i represents the moment, i = 1, 2,..., k; The factor graph model has only one prior factor node which is located in the upper left corner of the factor graph and is directly connected to the navigation variable node X1 at the first moment; there are a total of k navigation variable nodes X i , and the adjacent navigation variable nodes X i are connected by the SINS factor node f sins , and the navigation variable node X i is connected to the GNSS factor node f gnss in a one-to-one correspondence.

[0021] In this embodiment, in step S3, based on the system state in the Lie group framework defined in step S1, the specific method for constructing the SINS factor in the Lie group framework is as follows: The left-invariant error state equation of the Lie group is: 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, ω is the system noise vector, ω g and ω a are the Gaussian white noises of the gyroscope and accelerometer respectively; Among them, the specific formulas of F and G are obtained from the Lie group error differential equation, as follows: In the formula, represents the actual value of the angular velocity output by the gyroscope, represents The skew-symmetric matrix represents the actual value of the projection of the angular velocity of the Earth's rotation in the n system. represents The skew-symmetric matrix represents the actual value of the attitude matrix of the n system relative to the b system. represents the actual value of the attitude matrix of the b system relative to the n system. represents the actual value of the specific force vector measured by the accelerometer. represents The skew-symmetric matrix is the actual value of the velocity vector in the n system. represents The skew-symmetric matrix is the actual value of the position vector of the carrier system relative to the Earth coordinate system in the n system. represents The skew-symmetric matrix represents The skew-symmetric matrix, I 3×3 is a 3x3 identity matrix, 0 3×3 is a 3x3 zero matrix, and M1, M2, and M3 are meaningless codes. Among them, the specific formulas for M1, M2, and M3 are respectively: In the formula, represents the theoretical value of the projection of the angular velocity of the Earth's rotation in the n system, R N is the radius of curvature of the prime vertical, R M is the radius of curvature of the meridian, and h respectively represent latitude and altitude, v E represents the eastward velocity, v N represents the northward velocity; Discretize the left-invariant error state equation of the Lie group to obtain: f LI (x k-1 , u k ) = x k-1 + Fx k-1 dt; In the formula, x k represents the system state at time k, x k-1 represents the system state at time k-1, dt represents the time interval, represents the system noise of the SINS at time k, f LI (x k-1 , uk ) represents the system state equation of SINS in the Lie group framework, u k represents the input of SINS at time k; Therefore, the factor cost function of SINS in the Lie group framework is expressed as:

[0022] In this embodiment, in step S4, based on the system state in the Lie group framework defined in step S1, the specific method for constructing the GNSS factor in the Lie group framework is as follows: Using GNSS velocity information as the observable for initial alignment can suppress the divergence of the inertial navigation calculation results. The theoretical value relationship of the velocity vectors between the IMU center and the GNSS antenna phase center satisfies: In the formula, and respectively represent the theoretical values of the velocity vectors of the IMU center and the GNSS antenna phase center, represents the projection theoretical value of the rotation angular velocity vector of the n - system relative to the i - system in the n - system. The i - system is the inertial coordinate system, represents the skew - symmetric matrix of represents the theoretical value of the attitude matrix of the b - system relative to the n - system, l b represents the lever - arm quantity between the IMU and the GNSS antenna phase center, l b × represents the skew - symmetric matrix of l b ; represents the theoretical value of the angular velocity output by the gyroscope; Considering velocity and attitude errors and taking into account the gyro bias of the IMU, the expression for the velocity of the GNSS antenna phase center deduced from the IMU is: In the formula, and respectively represent the actual values of the velocity vectors of the IMU center and the GNSS antenna phase center, represents the actual value of the attitude matrix of the b - system relative to the n - system, represents the actual value of the angular velocity of the gyroscope, I represents the identity matrix, φ n is the attitude misalignment angle, φ n × represents the skew - symmetric matrix of φ n ; 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 value of the gyro angular velocity are respectively defined as: In the formula, δv nIt represents the velocity error in the n - frame. It represents the theoretical value of the angular velocity output by the gyroscope. It represents the output error of the gyroscope. It represents the bias vector of the gyroscope, ω g It represents the Gaussian white noise of the gyro. After substituting the actual inertial navigation velocity and the actual gyro angular velocity into the expression of the velocity of the GNSS antenna phase center deduced by the IMU, omitting the second - order terms about the error and organizing, we get: The velocity actually measured by GNSS is: In the formula, n v is the white noise of GNSS velocity measurement; The velocity observation vector z sensor is expressed as the difference between the velocity of the GNSS antenna phase center deduced by the IMU and the velocity actually measured by GNSS, that is: From the new velocity error formula defined in the Lie group framework in step S2, the velocity error in the n - frame is: Substituting the velocity error in the n - frame into the velocity observation vector formula, we get the measurement equation in the form of group state: Discretizing the measurement equation in the form of group state, we get: In the formula, It represents the measured value of the system state at time k by RTK, h LI (x k ) represents the measurement equation of the system state at time k by RTK, It represents the Gaussian noise of the RTK measurement equation; Therefore, the GNSS factor cost function in the Lie group framework is expressed as:

[0023] In this embodiment, in step S5, the specific method for optimizing the system state in the Lie group framework defined in step S1 by 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: Using the factor graph model constructed in step S2 to optimize the system state in the Lie group framework defined in step S1: Each factor node in the factor graph model represents the probability density function measured by each sensor. The probability density function of the SINS factor node is expressed as p(x k |x k-1 ,u k ), and the probability density function of the GNSS factor node is expressed as Then, the optimization problem of the factor graph model of the system state is expressed as solving the maximum value of the joint probability density function: In the formula, is the set of navigation states to be optimized, X * is the set of optimized solution states, and p(x0) is the known prior probability; Equivalently transforming the solution of the maximum value of the joint probability density function into minimizing the cost function of the factor nodes in the factor graph model. By minimizing the cost function, the optimal estimate x * of the system state is obtained, and the expression is: In the formula, is the SINS factor cost function, is the GNSS factor cost function, is the prior factor cost function, and the formula in the form of represents the squared Mahalanobis distance, and ∑ represents the measurement covariance matrix; Among them, the prior factor cost function is expressed as: Among them, x0 is the initial navigation state, and u0 is the mean value of the initial navigation state; Substitute 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 expression of the optimal estimate x * of the system state to obtain the optimal estimate x * of the system state.

[0024] In this embodiment, in step S6, based on the system state under the Lie group framework defined in step S1 and the optimal estimate of the system state obtained in step S5, the updated theoretical value of the Lie group matrix including elements related to attitude, velocity, and position is obtained. That is, the specific method for completing the rapid alignment of the large misalignment angle of SINS is: Based on the system state under the Lie group framework defined in step S1, according to the optimal estimate x * of the system state, update the left-invariant error η to obtain the updated left-invariant error η k ; According to the updated left-invariant error η k, the Lie group matrix χ including elements related to attitude, velocity, and position in step S1 is updated to obtain an updated Lie group matrix χ including elements related to attitude, velocity, and position. k , which is expressed as: In the formula, χ k represents the theoretical value of the Lie group matrix including elements related to attitude, velocity, and position after update. represents the actual value of the Lie group matrix including elements related to attitude, velocity, and position at time k, and η k represents the updated left-invariant error; Since the theoretical value χ of the Lie group matrix including elements related to attitude, velocity, and position after update k includes the theoretical value of the attitude matrix of the SINS to be finally solved. Complete the rapid alignment of the large misalignment angle of the SINS.

[0025] The present invention uses the system state and dynamic update in the Lie group framework to obtain the theoretical value of the Lie group matrix including elements related to attitude, velocity, and position, which improves the alignment convergence speed and effectively reduces the non-linear characteristics of the navigation system. At the same time, compared with the traditional filter-based alignment method, the initial alignment problem is constructed as a factor graph model optimization problem, and the optimal estimate of the system state is solved through the factor graph model. It has stronger robustness than the traditional filter and better accuracy, is simple to operate, has fast convergence, low cost, high alignment accuracy, and good practicability. The present invention overcomes the problem that the traditional large misalignment angle alignment method based on non-linear Kalman filter is usually not applicable to rapid alignment, and at the same time, its accuracy is difficult to reach the same level as the traditional two-stage alignment method.

[0026] The parts not detailedly disclosed in the present invention belong to the well-known technologies in the art.

[0027] 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 them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A fast alignment method for SINS with large misalignment angle based on Lie group and factor graph, characterized in that: The steps include: S1. Define the system state under the Lie group framework; S2, construct a factor graph model for SINS / GNSS fast alignment with large misalignment angles; S3, based on the system state under the Lie group framework defined in step S1, construct a SINS factor cost function under the Lie group framework; S4, constructing a GNSS factor cost function under the Lie group framework based on the system state under the Lie group framework defined in step S1; S5, optimizing the system state under the Lie group framework defined in step S1 by 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 an optimal estimate of the system state; S6. Based on the system state under the Lie group framework defined in step S1 and the optimal estimate of the system state obtained in step S5, the updated theoretical value of the Lie group matrix containing elements related to the attitude, velocity and position is obtained, thus completing the SINS large misalignment angle rapid alignment.

2. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 1 is characterized in that: In step S1, the specific method of defining the system state under the Lie group framework is: MEMSIMU and GNSS RTK are used to build an integrated navigation system, and the group error of SINS is modeled as the system state; For the initial alignment of navigation, the states related to attitude, velocity and position are defined on the Lie group matrix, while the IMU zero bias is defined in the traditional Euclidean space, expressed as: Where χ∈SE2(3) represents the theoretical value of the Lie group matrix containing elements related to posture, velocity and position, It represents the theoretical value of the attitude matrix of the b system relative to the n system. The n system is the navigation coordinate system and the b system is the carrier coordinate system. It represents the theoretical value of the velocity vector of the download system relative to the earth coordinate system in the n-frame. represents the theoretical value of the position vector of the download system relative to the earth coordinate system in the n system, 0 1×3 represents a zero matrix of 1 row and 3 columns; θ represents the IMU zero bias vector, and are the zero bias vectors of the gyroscope and accelerometer respectively; According to the properties of Lie groups: In the formula, χ -1 represents the inverse element of χ; It represents the theoretical value of the attitude matrix of system n relative to system b; Select the left invariant error η for processing: In the formula, represents the nominal state of χ, express The inverse element of Indicates the actual value of the attitude matrix of the n system relative to the b system, It represents the actual value of the velocity of the download system relative to the earth coordinate system in the b system. It indicates the actual value of the position of the download system relative to the earth coordinate system in the b system; In the Lie group framework, the new attitude error, velocity error and position error are defined as: In the formula, φ n is the attitude misalignment angle, the symbol × represents the antisymmetric matrix generated by the three-dimensional vector, φ n × represents φ n The antisymmetric matrix of represents the attitude matrix corresponding to the attitude misalignment angle, I represents the identity matrix; J represents the Jacobian matrix of the Rodriguez formula, and They represent the velocity error and position error in Lie algebraic form, and Represent the new velocity error and new position error respectively; It represents the actual value of the speed of the download system relative to the earth coordinate system in the n system, Indicates the actual value of the position of the download system relative to the earth coordinate system in the n system; δv n It represents the speed error in the n system, that is, the difference between the actual speed value and the theoretical speed value, δp n It represents the position error in the n system, that is, the difference between the actual position value and the theoretical position value; According to the characteristics of Lie groups and Lie algebras, the left invariant error η satisfies: Where Λ(ξ) represents the linear isomorphism that maps the vector space to the Lie algebra, represents the corresponding Lie algebraic form of error, T represents the transpose of the matrix; The expression of the Jacobian matrix J of the Rodriguez formula is: In the formula, α is φ n The absolute value of Therefore, the system state x in 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 is characterized in that: In step S2, the specific method of constructing the factor graph model for SINS / GNSS large misalignment angle rapid alignment is: The measurement information of GNSS is regarded as a univariate factor node, and the measurement information of IMU is regarded as a binary factor node. A factor graph model for SINS / GNSS large misalignment angle fast alignment is constructed, which consists of a priori factor nodes, navigation variable nodes, SINS factor nodes and GNSS factor nodes. Among them, the prior factor node is recorded as The navigation variable node is denoted by X i , i is the time, i=1,2,…,k; SINS factor node is denoted as f sins ; GNSS factor node is denoted as f gnss ; The speed information provided by the GNSS factor node is recorded as i is the time, i=1,2,…,k; The factor graph model has only one prior factor node It is directly connected to the navigation variable node X1 at the first moment; the navigation variable node X i There are k navigation variable nodes X at adjacent moments. i Between SINS factor nodes f sins Connect, Navigate variable node X i With GNSS factor node f gnss One-to-one connection.

4. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 3 is characterized in that: In step S3, based on the system state under the Lie group framework defined in step S1, the specific method of constructing the SINS factor under the Lie group framework is: Left Invariant Error State Equation of Lie Group for: Where F is the state transfer matrix of the left invariant error model, G is the noise transfer matrix of the left invariant error model, ω is the system noise vector, ω g and ω a are the Gaussian white noise of the gyroscope and accelerometer, respectively; Among them, the specific formulas of F and G are obtained according to the Lie group error differential equation, as follows: In the formula, Indicates the actual value of the angular velocity output by the gyroscope. express The antisymmetric matrix of It represents the actual value of the projection of the earth's rotation angular velocity in the n system. express The antisymmetric matrix of Indicates the actual value of the attitude matrix of the n system relative to the b system, Indicates the actual value of the attitude matrix of the b system relative to the n system, Represents the actual value of the specific force vector measured by the accelerometer, express The antisymmetric matrix of is the actual value of the velocity vector in the n system, express The antisymmetric matrix of is the actual value of the position vector of the download system relative to the earth coordinate system in the n system, express The antisymmetric matrix of express The antisymmetric matrix, I 3×3 is a 3-row, 3-column identity matrix, 0 3×3 is a zero matrix of 3 rows and 3 columns, M1, M2, and M3 are meaningless codes; Among them, the specific formulas of M1, M2 and M3 are: In the formula, R represents the projection theoretical value of the earth's rotation angular velocity in the n system. N is the radius of curvature of the y-axis, R M is the radius of curvature of the meridian, and h represent latitude and altitude respectively, v E represents the eastward velocity, v N represents the north speed; The left invariant error state equation of the Lie group Discretize and get: f LI (x k-1 ,u k )=x k-1 +Fx k-1 dt; In the formula, x k represents the system state at time k, x k-1 represents the system state at time k-1, dt represents the time interval, represents the system noise of SINS at time k, f LI (x k-1 ,u k ) represents the system state equation of SINS in the Lie group framework, u k 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 is characterized in that: In step S4, based on the system state in the Lie group framework defined in step S1, the specific method of constructing the GNSS factor in the Lie group framework is: Using GNSS velocity information as the observation quantity for initial alignment, the theoretical value relationship between the velocity vector of the IMU center and the GNSS antenna phase center satisfies: In the formula, and Represent the theoretical values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. It represents the theoretical value of the projection of the rotation angular velocity vector of the n system relative to the i system in the n system, and the i system is an inertial coordinate system. express The antisymmetric matrix of represents the theoretical value of the attitude matrix of system b relative to system n, l b It represents the arm length between the IMU and GNSS antenna phase center, l b × indicates l b The antisymmetric matrix of Indicates the theoretical value of the angular velocity output by the gyroscope; Taking into account the velocity and attitude errors, and taking into account the IMU gyro bias, the velocity of the GNSS antenna phase center calculated by the IMU is expressed as: In the formula, and Represent the actual values ​​of the velocity vectors at the IMU center and the GNSS antenna phase center, respectively. Indicates the actual value of the attitude matrix of the b system relative to the n system, represents the actual value of the angular velocity output by the gyroscope, I represents the unit matrix, φ n is the attitude misalignment angle, φ n × represents φ n The antisymmetric matrix of It represents the theoretical value of the attitude matrix of the b system relative to the n system; The actual value of the inertial navigation velocity and the actual value of the gyro angular velocity are defined as: In the formula, δv n represents the velocity error in the n system, Represents the theoretical value of the angular velocity output by the gyroscope, represents the output error of the gyroscope, represents the zero bias vector of the gyroscope, ω g represents the Gaussian white noise of the gyroscope; After substituting the actual values ​​of the inertial navigation velocity and the gyro angular velocity into the velocity expression of the GNSS antenna phase center calculated by the IMU, omitting the second-order term of the error, we get: The actual speed measured by GNSS for: Where n v It is the GNSS speed measurement white noise; Velocity observation vector z sensor It is expressed as the difference between the velocity of the GNSS antenna phase center estimated by the IMU and the velocity actually measured by the GNSS, that is: According to the new velocity error formula defined in step S2 under the Lie group framework, the velocity error in the n system is obtained as: Substituting the velocity error in the n-frame into the velocity observation vector formula, we get the measurement equation containing the group state form: Discretize the measurement equation containing the group state form and obtain: In the formula, represents the RTK measurement of the system status at time k, h LI (x k ) represents the RTK measurement equation for the system state at time k, Represented as Gaussian noise of 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 is 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: The factor graph model constructed in step S2 is used to optimize the system state under the Lie group framework defined in step S1: Each factor node in the factor graph model 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 the GNSS factor node is expressed as Then the factor graph model optimization problem of the system state is expressed as solving the maximum value of the joint probability density function: In the formula, is the navigation state set to be optimized, X * To optimize the solution state set, p(x0) is the known prior probability; The solution of the maximum value of the joint probability density function is equivalent to minimizing the cost function of the factor node 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, is the SINS factor cost function, is the GNSS factor cost function, is the prior factor cost function; Among them, the prior factor cost function is expressed as: Among them, x0 is the initial state of navigation, and u0 is the mean of the initial state of navigation; Substitute 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 x of the system state * In the expression of, we can obtain the optimal estimate x of the system state * .

7. The SINS large misalignment angle fast alignment method based on Lie group and factor graph according to claim 6 is 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 the attitude, velocity and position is obtained, that is, the specific method for completing the fast alignment of the SINS with a large misalignment angle is: Based on the system state under the Lie group framework defined in step S1, according to the optimal estimate x of the system state * , update the left invariant error η, and get the updated left invariant error η k ; According to the updated left invariant error η k , the Lie group matrix x containing elements related to posture, speed and position in step S1 is updated to obtain an updated Lie group matrix x containing elements related to posture, speed and position k , expressed as: In the formula, χ k represents the updated theoretical value of the Lie group matrix containing elements related to attitude, velocity, and position, represents the actual value of the Lie group matrix containing elements related to posture, velocity and position at time k, η k represents the updated left invariant error; Since the updated Lie group matrix containing elements related to attitude, velocity, and position has the theoretical value χ k Contains the theoretical value of the SINS attitude matrix to be solved in the end Complete SINS large misalignment angle rapid alignment.

Images