Pose, installation angle matrix lie group estimation method and system

Abstract

The application provides a matrix Lie group estimation method and system for pose and installation angle, which is characterized in that: according to specific force and angular velocity output by an IMU module, inertial navigation mechanical arrangement is performed, the attitude, velocity, position and errors of a navigation system are constructed on a Lie group, and an error state vector of the navigation system is constructed; the attitude, velocity and position error equations of the navigation system are constructed, and error state differential equations are obtained accordingly; if there are GNSS or odometry observation values, corresponding measurement equations are constructed; state prediction and measurement update of Kalman filtering are performed, and the navigation state at the current time is corrected; the above steps are repeated to obtain the attitude, position and velocity at each time. The application solves the problem of inconsistent extended Kalman filter variance estimation in a combined navigation system, and improves the robustness and precision of the combined navigation system.

Description

Technical Field

[0001] This invention belongs to the field of navigation methods and applications, and particularly relates to a matrix Lie group scheme for pose estimation and installation angle estimation. Background Technology

[0002] In the field of positioning and navigation, the most commonly used navigation method to obtain high-precision position and attitude information of a vehicle is a combined GNSS (Global Navigation Satellite System) and INS (Inertial Navigation System) navigation system.

[0003] Meanwhile, odometer-assisted navigation has been proven to significantly improve the accuracy of GNSS / INS integrated navigation for vehicles. However, to fully realize the potential of the odometer, the precise installation angle of the IMU (Inertial Measurement Unit) is required. Due to operational factors such as installation location, the IMU coordinate system and the vehicle's coordinate system are not perfectly aligned; the relationship between the two coordinate systems is the installation angle.

[0004] To achieve better navigation results, integrated navigation systems need to fuse observations from GNSS, INS, and odometry devices. Currently, the most widely used data fusion algorithm is the Extended Kalman Filter (EPF), which generally fuses data well and achieves good positioning results. However, EPF also has some limitations. During prediction and update, the EPF relies on the estimated navigation state to construct the Jacobian matrix of the state model. Inaccurate navigation state estimation can lead to incorrect Jacobian matrices. Furthermore, EPF suffers from inconsistent variance estimation; high nonlinearity in the navigation system can cause a decrease in filtering effectiveness or even divergence.

[0005] To address existing problems, this invention proposes a matrix Lie group method for pose estimation and installation angle estimation. By using a matrix Lie group to represent the rotation, translation, and velocity of the system as a whole, the system's differential equations can be considered from the perspective of the Lie group. This approach is natural and more accurate. Compared to representing individual variables, considering the system's differential equations as a whole allows for coupling the relationships between states, making the model more accurate. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies, this invention provides a matrix Lie group scheme for pose estimation and installation angle estimation, which solves problems such as inconsistent variance estimation of extended Kalman filter and high requirements for navigation initial state.

[0007] This invention provides a matrix Lie group estimation method for pose and mounting angle, including acquiring the initial attitude, initial velocity, and initial position of the carrier, and using them as the initial calculation data for the inertial navigation system; the attitude includes the carrier's roll angle, pitch angle, and yaw angle; based on the specific force and angular rate output by the IMU module, inertial navigation mechanical orchestration is performed, updating the attitude, velocity, and position from the previous moment to the current moment; the following steps are performed.

[0008] Based on the specific force and angular rate output by the IMU module, inertial navigation mechanical orchestration is performed, updating the attitude, velocity, and position of the previous moment to the attitude, velocity, and position of the current moment.

[0009] The attitude, velocity, and position of the navigation system are constructed into a matrix Lie group; the errors of the navigation system are defined on the matrix Lie group; the attitude error, velocity error, and position error of the navigation system are obtained based on the defined navigation system errors; the IMU mounting angle error is defined on the matrix Lie group; and the attitude error, velocity error, and position error on the matrix Lie group, as well as the device error of the IMU, the IMU mounting angle error defined on the matrix Lie group, and the odometry scale factor error are constructed into the error state vector of the navigation system.

[0010] Based on the current attitude, velocity, position, and attitude error, velocity error, and position error, construct attitude error equation, velocity error equation, and position error equation to model the device error of the IMU, the IMU mounting angle error, and the odometer scale factor error, and construct the error state differential equation of the Kalman filter accordingly.

[0011] If GNSS or odometer observations exist at the current moment, construct the corresponding measurement equations based on the position and velocity observations provided by GNSS and the velocity observations provided by odometer.

[0012] According to the Kalman filter formula, state prediction is performed using Kalman filtering. If there are GNSS or odometry observations at the current time, measurement updates are performed according to the Kalman filter formula, and the error state quantity obtained from the measurement update is used to correct the attitude, velocity, position, and IMU mounting angle at the current time.

[0013] After entering the next moment, the above process is repeated to obtain the attitude, velocity and position of the carrier at each moment.

[0014] Moreover, the attitude of the carrier is speed is Location is in, This represents the rotation matrix of the carrier coordinate system relative to the world coordinate system. This represents the projection of the velocity vector of the vehicle coordinate system relative to the world coordinate system onto the world coordinate system. represents the projection of the position vector of the carrier coordinate system relative to the world coordinate system into the world coordinate system; e represents the world coordinate system, and b represents the carrier coordinate system.

[0015] Furthermore, the matrix Lie group formed by the attitude, velocity, and position of the navigation system is:

[0016]

[0017] Among them, 0 3×1 This represents a zero vector of size 3×1;

[0018] The error of the navigation system defined on the matrix Lie group is:

[0019]

[0020] Where η is the error of the navigation system defined on the matrix Lie group; χ is the true value of the navigation system state; This is an estimate of the navigation system's state.

[0021] The attitude error of a navigation system defined on a matrix Lie group is:

[0022]

[0023] Where φ is the attitude error of the navigation system defined on the matrix Lie group; Indicates the actual posture of the carrier; Indicates the estimated carrier attitude; I 3×3 φ represents a 3×3 identity matrix, and φ× represents an antisymmetric matrix generated by the three-dimensional vector φ;

[0024] The velocity error of a navigation system defined on a matrix Lie group is:

[0025]

[0026] Among them, Jρ v Let the velocity error of the navigation system be defined on the matrix Lie group; Let represent the attitude rotation matrix from the e-frame to the b-frame. Indicates the actual posture of the carrier; This represents the actual speed of the carrier. Indicates the estimated carrier velocity;

[0027] The position error of a navigation system on a matrix Lie group is defined as:

[0028]

[0029] Among them, Jρ r Let be the position error of the navigation system defined on the matrix Lie group; Indicates the actual posture of the carrier; Indicates the actual location of the carrier; Indicates the estimated carrier location;

[0030] The IMU mounting angle error defined on the matrix Lie group is:

[0031]

[0032] in, The IMU mounting angle error is defined on the matrix Lie group; The rotation matrix representing the actual carrier to the odometer, i.e., the actual odometer mounting angle; This indicates the estimated odometer installation angle;

[0033] The elements of the navigation system's error state vector include the attitude error φ on the Lie group and the velocity error Jρ on the Lie group. v The positional error Jρ on the matrix Lie group r The gyroscope zero bias correction term δb of the IMU g δb, the zero bias correction term of the IMU's accelerometer a IMU mounting angle error defined on a matrix Lie group And the odometer scale factor error δs.

[0034] Furthermore, the attitude error state equation is:

[0035]

[0036] in, δb represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system; g This represents the zero-bias correction term of the IMU gyroscope; w g This represents the white noise of the IMU gyroscope measurement; The derivative of the equivalent rotation vector φ is represented.

[0037] The velocity error state equation is:

[0038]

[0039] in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system. δb represents specific force. aThis represents the zero-bias correction term of the IMU accelerometer; w a This represents the white noise of the IMU accelerometer measurement;

[0040] The position error state equation is:

[0041]

[0042] in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system.

[0043] Moreover, the measurement equations include GNSS position measurement equations, GNSS velocity measurement equations, and odometer velocity measurement equations.

[0044] The GNSS position measurement equation is as follows:

[0045]

[0046] in, This indicates the GNSS phase center position obtained through inertial navigation mechanical orchestration. This indicates the position of the GNSS phase center obtained through GNSS measurement; The lever arm representing the distance from the GNSS phase center to the carrier coordinate system; This represents the GNSS position observation noise vector;

[0047] The GNSS velocity measurement equation is as follows:

[0048]

[0049] in, This represents the GNSS phase center velocity obtained through inertial navigation mechanical orchestration. This represents the GNSS phase center velocity obtained through GNSS measurements; The lever arm representing the distance from the GNSS phase center to the carrier coordinate system; Represents the GNSS velocity observation noise vector;

[0050] The odometer speed measurement equation described in step 5 is as follows:

[0051]

[0052] in, This indicates the odometer speed obtained through inertial navigation mechanical programming. This indicates the odometer speed measured by the odometer. The lever arm representing the odometer's connection to the carrier coordinate system; This represents the speed observation noise vector of the odometer.

[0053] On the other hand, the present invention provides a matrix Lie group estimation system for pose and mounting angle, which is used to implement the matrix Lie group estimation method for pose and mounting angle as described above.

[0054] Furthermore, it includes a processor and a memory, the memory being used to store program instructions, and the processor being used to call the stored instructions in the memory to execute a matrix Lie group estimation method for pose and mounting angle as described above.

[0055] Alternatively, it may include a readable storage medium storing a computer program that, when executed, implements a matrix Lie group estimation method for pose and mounting angle as described above.

[0056] This invention proposes a matrix Lie group scheme for pose estimation and installation angle estimation. The initial attitude, position, and velocity of the carrier are acquired and used as the initial starting data for the inertial navigation system (INS). The INS is mechanically orchestrated based on the IMU module output. The attitude, velocity, position, and their errors of the navigation system are constructed on the Lie group, and the error state vector of the navigation system is also constructed. The attitude, velocity, and position error equations of the navigation system are constructed, and the error state differential equations are derived accordingly. If GNSS or odometry observations exist, the corresponding measurement equations are constructed. Kalman filtering is performed for state prediction and measurement updates, and the navigation state at the current moment is corrected. The above steps are repeated to obtain the attitude, position, and velocity at each moment. This invention solves the problem of inconsistent variance estimation in extended Kalman filtering in integrated navigation systems, improving the robustness and accuracy of the integrated navigation system. Attached Figure Description

[0057] Figure 1 A flowchart of a matrix Lie group method for pose estimation and installation angle estimation provided in an embodiment of the present invention. Detailed Implementation

[0058] To make the objectives and technical solutions of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] See Figure 1 This invention provides a matrix Lie group estimation method for pose and mounting angle, and the specific implementation steps will be described in detail below.

[0060] Step 1: Obtain the initial attitude, initial velocity, and initial position of the vehicle, and use them as the initial calculation data for the inertial navigation system; the attitude includes the vehicle's roll angle, pitch angle, and yaw angle;

[0061] Wherein, the attitude of the carrier is speed is Location is in, This represents the rotation matrix of the carrier coordinate system relative to the world coordinate system. This represents the projection of the velocity vector of the vehicle coordinate system relative to the world coordinate system onto the world coordinate system. represents the projection of the position vector of the carrier coordinate system relative to the world coordinate system into the world coordinate system; e represents the world coordinate system, and b represents the carrier coordinate system.

[0062] Step 2: Based on the specific force and angular rate output by the IMU module, perform inertial navigation mechanical orchestration, and update the attitude, velocity, and position of the previous moment to the attitude, velocity, and position of the current moment;

[0063] The inertial navigation mechanical orchestration steps are as follows: using the attitude, velocity, and position from the previous moment, as well as the specific force and angular velocity information output by the IMU, the attitude, velocity, and position at this moment are calculated according to the navigation differential equations, including attitude update, velocity update, and position update; wherein the formulas for the calculated attitude update, velocity update, and position update are:

[0064]

[0065] Where the subscript k represents the current time, and the subscript k-1 represents the previous time; t k t represents the current time. k-1 Indicates the time of the previous moment; Let represent the quaternion of the current pose, which is related to the current pose. They can be converted to each other; Let represent the pose quaternion of the previous time step, which is related to the pose of the previous time step. They can be converted to each other; This represents the quaternion representing the attitude change of the e-frame due to Earth's rotation within the update interval. ξ k This represents the equivalent rotation vector of the Earth's rotation within the update interval. This represents the Earth's angular velocity of rotation; The quaternion representing the attitude change of the carrier coordinate system within the update time interval. φ k This represents the equivalent rotation vector of the carrier coordinate system at the current moment relative to the previous moment. Δθ k and Δθ k-1 These represent the angle increments at the current and previous moments, respectively. Indicates the speed at the current moment; Indicates the velocity at the previous moment; This represents the projection of the velocity increment caused by the specific force onto the e-frame. This represents the attitude rotation matrix updated in the e-frame. It can be converted Please solve. Δθ k and Δθ k-1 These represent the angle increments at the current and previous moments, respectively. and These represent the velocity increments in the carrier coordinate system at the current and previous moments, respectively. This represents the projection of the velocity increment caused by gravitational acceleration and Coriolis acceleration into the e-frame. g e Represents Earth's gravity. This represents the Earth's angular velocity of rotation; Indicates the current position; Indicates the position at the previous moment.

[0066] Step 3: Construct a matrix Lie group to measure the attitude, velocity, and position of the navigation system; define the navigation system error on the matrix Lie group; obtain the attitude error, velocity error, and position error of the navigation system based on the defined navigation system error; define the IMU mounting angle error on the matrix Lie group; and construct the navigation system error state vector from the attitude error, velocity error, position error on the matrix Lie group, the IMU device error, the IMU mounting angle error defined on the matrix Lie group, and the odometry scale factor error.

[0067] The matrix Lie group formed by the attitude, velocity, and position of the navigation system described in step 3 is:

[0068]

[0069] Among them, 0 3×1 This represents a zero vector of size 3×1.

[0070] The error of the navigation system defined on the matrix Lie group in step 3 is:

[0071]

[0072] Where η is the error of the navigation system defined on the matrix Lie group; χ is the true value of the navigation system state; This is an estimate of the navigation system's state.

[0073] The attitude error of the navigation system defined on the matrix Lie group in step 3 is:

[0074]

[0075] Where φ is the attitude error of the navigation system defined on the matrix Lie group; Indicates the actual posture of the carrier; Indicates the estimated carrier attitude; I 3×3 φ represents a 3×3 identity matrix, and φ× represents an antisymmetric matrix generated by the three-dimensional vector φ.

[0076] The velocity error of the navigation system defined on the matrix Lie group in step 3 is:

[0077]

[0078] Among them, Jρ v Let the velocity error of the navigation system be defined on the matrix Lie group; Let represent the attitude rotation matrix from the e-frame to the b-frame. Indicates the actual posture of the carrier; This represents the actual speed of the carrier. Indicates the estimated carrier velocity;

[0079] The position error of the navigation system defined on the matrix Lie group in step 3 is:

[0080]

[0081] Among them, Jρ r Let be the position error of the navigation system defined on the matrix Lie group; Indicates the actual posture of the carrier; Indicates the actual location of the carrier; Indicates the estimated carrier location;

[0082] The IMU mounting angle error defined on the matrix Lie group in step 3 is:

[0083]

[0084] in, The IMU mounting angle error is defined on the matrix Lie group; The rotation matrix representing the actual carrier to the odometer, i.e., the actual odometer mounting angle; This indicates the estimated odometer installation angle;

[0085] Preferably, the error state vector of the navigation system described in step 3 can be represented by a state vector x, as follows:

[0086]

[0087] Where φ is the attitude error on the matrix Lie group, Jρ v Jρ is the velocity error on the matrix Lie group. r Let δb be the position error on the Lie group of the matrix. g For the IMU's gyroscope zero-bias correction term, δb a This is the accelerometer zero-bias correction term for the IMU, and δs is the odometry scale factor error. The IMU mounting angle error is defined on the matrix Lie group.

[0088] Step 4: Based on the attitude, velocity, and position obtained in Step 2 and the attitude error, velocity error, and position error defined in Step 3, construct the attitude error equation, velocity error equation, and position error equation to model the device error of the IMU, the IMU mounting angle error, and the odometer scale factor error, and construct the error state differential equation of the Kalman filter accordingly.

[0089] Preferably, the present invention provides a detailed derivation process for the attitude error equation, velocity error equation, and position error equation on the matrix Lie group in step 4, as follows:

[0090] The derivation of the attitude error state equation is as follows:

[0091]

[0092] and

[0093]

[0094] Therefore there is

[0095]

[0096] in, δb represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system; g This represents the zero-bias correction term of the IMU gyroscope; w g This represents the white noise of the IMU gyroscope measurement; the Earth's rotational angular velocity is neglected in the derivation. The error and the second-order small quantity (φ×)(δb) g + g ); The derivative of the equivalent rotation vector φ is given. Represents the true pose matrix The differential, This represents the differential of the estimated attitude matrix. Represents the time derivative of a variable;

[0097] The derivation of the velocity error state equation is as follows:

[0098]

[0099]

[0100] in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system. δb represents specific force. a This represents the zero-bias correction term of the IMU accelerometer; w a This represents the white noise of the IMU accelerometer measurement; the second-order small quantity (φ×)(δb) is neglected in the derivation. a + a );because The local changes are very small, so they are ignored. and and The values ​​are all very small, therefore This was also considered a small amount and was ignored;

[0101] The derivation of the position error state equation is as follows:

[0102]

[0103] in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system; due to and The values ​​are all very small, therefore It was considered a small amount and was ignored;

[0104] Preferably, the gyroscope zero-bias correction term and the accelerometer zero-bias correction term of the IMU can be modeled as first-order Gaussian Markov processes, with correlation times τ and τ, respectively. g and τ a The first-order Gaussian Markov process driving white noise are respectively and The IMU mounting angle error and odometer scale factor error can be modeled as constants, and their driving white noise are respectively... and η s Thus, the error state differential equation of the Kalman filter can be expressed as:

[0105]

[0106] in, It is the differential of the error state, F is the linear error dynamics matrix, G is the process noise Jacobian matrix, and w is the white noise vector.

[0107]

[0108]

[0109]

[0110] Step 5: If GNSS or odometer observations exist at the current time, construct the corresponding measurement equations based on the position and velocity observations provided by GNSS and the velocity observations provided by odometer.

[0111] Preferably, the present invention provides a detailed derivation process for the GNSS position measurement equation, velocity measurement equation, and odometer velocity measurement equation in step 5, as follows:

[0112] The GNSS position measurement equations are derived as follows:

[0113]

[0114] in, This indicates the GNSS phase center position obtained through inertial navigation mechanical orchestration. This indicates the position of the GNSS phase center obtained through GNSS measurement; The lever arm representing the distance from the GNSS phase center to the carrier coordinate system; This represents the GNSS position observation noise vector;

[0115] The GNSS velocity measurement equation is derived as follows:

[0116]

[0117]

[0118] in, This represents the GNSS phase center velocity obtained through inertial navigation mechanical orchestration. This represents the GNSS phase center velocity obtained through GNSS measurements; The lever arm representing the distance from the GNSS phase center to the carrier coordinate system; This represents the GNSS velocity observation noise vector; second-order terms are ignored in the derivation.

[0119] The derivation of the odometer speed measurement equation is as follows:

[0120]

[0121] in, This indicates the odometer speed obtained through inertial navigation mechanical programming. This indicates the odometer speed measured by the odometer. The lever arm representing the odometer's connection to the carrier coordinate system; The odometer's speed observation noise vector is represented by `diag()`; the diagonal matrix is ​​represented by `diag()`; second-order terms are ignored in the derivation. and

[0122] Preferably, the measurement equation can be expressed as:

[0123] δz=Hx+V

[0124] Where δz is the measurement residual, V is the observation noise vector. H is the measurement Jacobian matrix.

[0125] in, It is the residual of GNSS position observation. It is the residual of GNSS velocity observation. It is the residual of the odometer speed observation. It is the GNSS position observation noise vector. It is the GNSS velocity observation noise vector. It is the speed observation noise vector of the odometer. It is the Jacobian matrix for GNSS position observations. It is the Jacobian matrix for GNSS velocity observation. It is the Jacobian matrix observed by the odometer.

[0126]

[0127]

[0128]

[0129] in If remember and Modeled as Gaussian white noise, its covariance matrices are respectively and It can be proven that if we take an isotropic observation noise covariance matrix... and use and replace and The result of the combined navigation remains unchanged; therefore, state-independent navigation can be used. and The observation matrix is ​​used for Kalman filtering.

[0130] Step 6: Perform state prediction using Kalman filtering according to the Kalman filtering formula; if there are GNSS or odometry observations at the current time, update the measurements according to the Kalman filtering formula, and correct the attitude, velocity, position, and IMU mounting angle at the current time obtained in Step 2 based on the error state quantity obtained from the measurement update.

[0131] The state prediction formula for the Kalman filter described in step 6 is as follows:

[0132]

[0133] in, P represents the state vector at time k-1. k-1 This represents the system state covariance matrix at time k-1. P represents the predicted state vector. k / k-1 Φ represents the predicted system state covariance matrix. k / k-1 Let Q represent the state transition matrix from time k-1 to time k. k-1 This represents the system noise covariance matrix at time k-1;

[0134] The measurement update formula for the Kalman filter described in step 6 is as follows:

[0135]

[0136] Among them, among them, P represents the state vector at time k. k This represents the system state covariance matrix at time k. P represents the predicted state vector. k / k-1 H represents the predicted system state covariance matrix. k Let R represent the observation matrix. k Z represents the observation noise covariance matrix. k K represents the observation vector. k I represents the Kalman gain matrix, and I represents the identity matrix.

[0137] The correction formulas for the current attitude, velocity, position, and IMU mounting angle described in step 6 are as follows:

[0138]

[0139]

[0140]

[0141]

[0142] in, and Indicates the posture before and after the update. and Indicates the speed before and after the update. and Indicates the position before and after the update. and Indicates the IMU mounting angle before and after the update;

[0143] Step 7: After entering the next moment, repeat steps 2, 3, 4, 5, and 6 to obtain the attitude, velocity, and position of the carrier at each moment.

[0144] In specific implementation, the method proposed in the technical solution of this invention can be automatically executed by those skilled in the art using computer software technology. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of this invention and computer equipment including the computer program running the corresponding computer program, should also be within the protection scope of this invention.

[0145] In some possible embodiments, a matrix Lie group estimation system for pose and mounting angle is provided, including a processor and a memory. The memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute a matrix Lie group estimation method for pose and mounting angle as described above.

[0146] In some possible embodiments, a matrix Lie group estimation system for pose and mounting angle is provided, including a readable storage medium on which a computer program is stored. When the computer program is executed, it implements a matrix Lie group estimation method for pose and mounting angle as described above.

[0147] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended 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. Such 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 various embodiments of the present invention.

Claims

1. A matrix Lie group estimation method for pose and mounting angle, comprising acquiring the initial attitude, initial velocity, and initial position of a carrier, and using them as initial calculation data for an inertial navigation system; the attitude includes the roll angle, pitch angle, and yaw angle of the carrier; performing inertial navigation mechanical orchestration based on the specific force and angular rate output by the IMU module, updating the attitude, velocity, and position of the previous moment to the attitude, velocity, and position of the current moment; characterized in that: Perform the following steps: Based on the specific force and angular rate output by the IMU module, inertial navigation mechanical orchestration is performed, updating the attitude, velocity, and position of the previous moment to the attitude, velocity, and position of the current moment. The attitude, velocity, and position of the navigation system are constructed as a matrix Lie group; the errors of the navigation system are defined on the matrix Lie group; the attitude error, velocity error, and position error of the navigation system are obtained based on the defined errors; the IMU mounting angle error is defined on the matrix Lie group. The attitude error, velocity error, and position error on the matrix Lie group, along with the device error of the IMU, the IMU mounting angle error defined on the matrix Lie group, and the odometry scale factor error, are constructed as the error state vector of the navigation system; the IMU mounting angle error defined on the matrix Lie group is: in, The IMU mounting angle error is defined on the matrix Lie group; = , The rotation matrix representing the actual carrier to the odometer, i.e., the actual odometer mounting angle; This indicates the estimated odometer installation angle; Indicates size is The identity matrix, Represents a three-dimensional vector The generated antisymmetric matrix; Based on the current attitude, velocity, position, and attitude error, velocity error, and position error, construct attitude error equation, velocity error equation, and position error equation to model the device error of the IMU, the IMU mounting angle error, and the odometer scale factor error, and construct the error state differential equation of the Kalman filter accordingly. If GNSS or odometer observations exist at the current moment, construct the corresponding measurement equations based on the position and velocity observations provided by GNSS and the velocity observations provided by odometer. According to the Kalman filter formula, state prediction is performed using Kalman filtering. If there are GNSS or odometry observations at the current time, measurement updates are performed according to the Kalman filter formula, and the error state quantity obtained from the measurement update is used to correct the attitude, velocity, position, and IMU mounting angle at the current time. After entering the next moment, the above process is repeated to obtain the attitude, velocity and position of the carrier at each moment.

2. The matrix Lie group estimation method for pose and mounting angle according to claim 1, characterized in that: The attitude of the carrier is The speed is The location is ;in, This represents the rotation matrix of the carrier coordinate system relative to the world coordinate system. This represents the projection of the velocity vector of the vehicle coordinate system relative to the world coordinate system onto the world coordinate system. represents the projection of the position vector of the carrier coordinate system relative to the world coordinate system into the world coordinate system; e represents the world coordinate system, and b represents the carrier coordinate system.

3. The matrix Lie group estimation method for pose and mounting angle according to claim 2, characterized in that: The matrix Lie group formed by the attitude, velocity, and position of the navigation system is: in, Indicates size is The zero vector; The error of the navigation system defined on the matrix Lie group is: in, Let the error of the navigation system be defined on the matrix Lie group; This represents the actual state of the navigation system. This is an estimate of the navigation system's state. The attitude error of a navigation system defined on a matrix Lie group is: in, Let be the attitude error of the navigation system defined on a matrix Lie group; = , Indicates the actual posture of the carrier; Indicates the estimated carrier attitude; Indicates size is The identity matrix, Represents a three-dimensional vector The generated antisymmetric matrix; The velocity error of a navigation system defined on a matrix Lie group is: in, Let the velocity error of the navigation system be defined on the matrix Lie group; Let represent the attitude rotation matrix from the e-frame to the b-frame. = , Indicates the actual posture of the carrier; This represents the actual speed of the carrier. Indicates the estimated carrier velocity; The position error of a navigation system on a matrix Lie group is defined as: in, Let be the position error of the navigation system defined on the matrix Lie group; = , Indicates the actual posture of the carrier; Indicates the actual location of the carrier; Indicates the estimated carrier location; The elements of the navigation system's error state vector include attitude errors on a matrix Lie group. Velocity error on matrix Lie groups Position error on matrix Lie group IMU gyroscope zero bias correction term IMU accelerometer zero bias correction term IMU mounting angle error defined on a matrix Lie group and odometer scale factor error .

4. The matrix Lie group estimation method for pose and mounting angle according to claim 3, characterized in that: The attitude error equation is as follows: in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system. This represents the zero-bias correction term of the IMU gyroscope; This represents the white noise of the IMU gyroscope measurement; Represents the equivalent rotation vector The differential; The velocity error equation is: in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system. Indicates the ratio of force; This represents the zero-bias correction term of the IMU accelerometer; This represents the white noise of the IMU accelerometer measurement; The position error equation is: in, This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system.

5. The matrix Lie group estimation method for pose and mounting angle according to claim 4, characterized in that: The measurement equations include GNSS position measurement equations, GNSS velocity measurement equations, and odometer velocity measurement equations. The GNSS position measurement equation is as follows: in, , This indicates the GNSS phase center position obtained through inertial navigation mechanical orchestration. This indicates the position of the GNSS phase center obtained through GNSS measurement; The lever arm representing the distance from the GNSS phase center to the carrier coordinate system; This represents the GNSS position observation noise vector; The GNSS velocity measurement equation is as follows: in, , This represents the GNSS phase center velocity obtained through inertial navigation mechanical orchestration. This represents the GNSS phase center velocity obtained through GNSS measurements; The lever arm representing the distance from the GNSS phase center to the carrier coordinate system; Represents the GNSS velocity observation noise vector; The odometer speed measurement equation described in step 5 is as follows: in, , This indicates the odometer speed obtained through inertial navigation mechanical programming. This indicates the odometer speed measured by the odometer. The lever arm representing the odometer's connection to the carrier coordinate system; This represents the speed observation noise vector of the odometer.

6. A matrix Lie group estimation system for pose and mounting angle, characterized in that: This method is used to implement a matrix Lie group estimation method for pose and mounting angle as described in any one of claims 1-5.

7. The matrix Lie group estimation system for pose and mounting angle according to claim 6, characterized in that: It includes a processor and a memory, the memory being used to store program instructions, and the processor being used to call the stored instructions in the memory to execute a matrix Lie group estimation method for pose and mounting angle as described in any one of claims 1-5.

8. The matrix Lie group estimation system for pose and mounting angle according to claim 6, characterized in that: It includes a readable storage medium on which a computer program is stored, and when the computer program is executed, it implements a matrix Lie group estimation method for pose and mounting angle as described in any one of claims 1-5.

Images