Inertia-based integrated navigation method and device for enhancing autonomous characteristics of Lie group

Abstract

The invention belongs to the technical field of inertial navigation, and provides an inertial-based integrated navigation method and device for enhancing the autonomous characteristics of a Lie group in order to solve the technical problems of insufficient autonomous characteristics, reduced navigation precision under the condition of a large misalignment angle and the like of an existing integrated navigation Lie group model. According to the initial navigation state of a moving body and inertial navigation information, a world coordinate system projection-based Kalman filter system equation for enhancing Lie group autonomous characteristics is constructed, dual-reference-system fusion of inertial system velocity representation and earth system attitude / position representation is realized, and the autonomous characteristics of a combined navigation system are enhanced. Compared with a traditional method, the robustness and the positioning precision of the integrated navigation system under the condition of the large misalignment angle can be remarkably improved.

Description

Technical Field

[0001] The invention mainly relates to the technical field of inertial navigation, in particular to an inertial basis combined navigation method and device for enhancing the autonomous characteristics of Lie groups. Background Art

[0002] In scenarios where satellite navigation is available, the inertial / satellite loosely / tightly coupled combined navigation system can achieve high navigation accuracy. However, in satellite-denied environments such as tunnels, underground facilities, and electromagnetic countermeasures, autonomous navigation technology faces severe challenges. Existing land vehicle navigation systems mostly use a solution that combines speed sensors (such as odometers and Doppler speedometers) with inertial navigation to constrain the error accumulation of inertial navigation through speed information. However, this solution has three significant technical bottlenecks: (1) System parameter sensitivity: The installation angle between the inertial navigation system and the speed measuring device, the arm parameters, and the scaling factor must be accurately calibrated in advance. However, in actual applications, changes in vehicle body deformation and road conditions will cause these parameters to shift over time, and traditional fixed parameter models cannot effectively adapt. (2) State estimation reliability problem: When the vehicle is stationary or driving in a straight line for a long time, the state quantities such as gyro bias, accelerometer bias, and installation angle error are weakly observable. The traditional extended Kalman filter (EKF) is prone to estimation divergence under nonlinear strong coupling conditions. (3) Alignment problem with large misalignment angles: Initial alignment on the move under satellite denial conditions needs to deal with large attitude error scenarios. However, the existing matrix Lie group model has the inherent defect of insufficient autonomy, which leads to a significant degradation in the accuracy of navigation parameter estimation under large angle error conditions.

[0003] In particular, the existing matrix Lie group navigation model does not fully consider the influence of the reference frame projection relationship and the initial state of the moving body during the error state conversion process, resulting in insufficient autonomy of the error model and difficulty in maintaining the differential homeomorphism characteristics of the Lie group manifold. This has become a key technical bottleneck restricting the accuracy of combined navigation in high dynamic environments. Summary of the invention

[0004] In view of the autonomy defects and applicability problems of the existing navigation model based on matrix Lie group in large misalignment angle scenes, the present invention proposes an inertial basis combined navigation method and device with enhanced Lie group autonomy characteristics to achieve improved navigation performance.

[0005] To achieve the above purpose, the technical solution adopted by the present invention is as follows: In one aspect, the present invention provides an inertial base integrated navigation method for enhancing the autonomous characteristics of Lie groups, comprising: Receive inertial navigation system navigation information, odometer navigation information, and positioning system navigation information of the moving body; According to the initial navigation state of the moving body and the inertial navigation information, a Kalman filter system equation with enhanced Lie group autonomy characteristics based on the projection of the world coordinate system is constructed; Take the estimated speed of the odometer as the observed quantity and construct the speed observation equation; According to the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection and the velocity observation equation, the inertial basis integrated navigation is performed, and the state and covariance of the inertial basis integrated navigation are obtained as the state and covariance of the Kalman filter observation update; According to the Lie group error state transformation, the attitude, velocity and position are updated.

[0006] In the present invention, the inertial navigation system navigation information includes: navigation timestamp, roll angle, pitch angle, heading angle, longitude, latitude, altitude and three-dimensional speed information under the local navigation system; The odometer navigation information includes: navigation timestamp, carrier speed; The positioning system navigation information is navigation information from a satellite positioning system or an acoustic positioning system, including a navigation timestamp and absolute three-dimensional positioning information.

[0007] On the other hand, the inertial base integrated navigation device with enhanced Lie group autonomy characteristics includes: The first module is used to receive the inertial navigation system navigation information, odometer navigation information, and positioning system navigation information of the moving body; The second module is used to construct a Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection according to the initial navigation state of the moving body and the inertial navigation information; The third module is used to construct the speed observation equation by taking the estimated speed of the odometer as the observed quantity; The fourth module is used to perform inertial basis integrated navigation according to the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection and the velocity observation equation, and obtain the state and covariance of the inertial basis integrated navigation as the state and covariance of the Kalman filter observation update; The fifth module is used to update the attitude, velocity and position according to the Lie group error state transformation.

[0008] On the other hand, the present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the steps of the inertial basis combined navigation method with enhanced Lie group autonomy characteristics are implemented.

[0009] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-mentioned inertial basis combined navigation method with enhanced Lie group autonomy characteristics.

[0010] On the other hand, the present invention provides a computer program product, which is stored on a computer-readable storage medium and includes computer instructions, which, when executed by a processor, enable a computer device to implement the steps of the above-mentioned inertial basis combined navigation method with enhanced Lie group autonomy characteristics.

[0011] Compared with the prior art, the present invention has the following beneficial effects: The present invention constructs a Kalman filter system equation with enhanced Lie group autonomy characteristics based on world coordinate system projection according to the initial navigation state of the moving body and the inertial navigation information, realizes the dual reference system fusion of the inertial system velocity representation and the earth system attitude / position representation, and enhances the autonomy characteristics of the combined navigation system.

[0012] Furthermore, the present invention establishes a Lie Group Kalman Filter (LG-EKF-R) algorithm in the form of right multiplication error, which is more advantageous than the model defined by the left error in terms of accuracy and computational efficiency.

[0013] Furthermore, the influence of the initial speed and position of the moving body is deducted in the derivation of the error model of the present invention, thereby simplifying the initial variance setting.

[0014] Compared with traditional methods, the present invention can significantly improve the robustness and positioning accuracy of the integrated navigation system under large misalignment angle conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying creative work.

[0016] Figure 1 A flowchart of an inertial base integrated navigation method with enhanced Lie group autonomy characteristics provided by an embodiment; Figure 2 A schematic diagram of the vehicle-mounted inertial / odometer combined navigation space configuration; Figure 3 This is the trajectory diagram of the vehicle-mounted experiment; Figure 4 The comparison chart of the combined navigation results of the extended Kalman filter (EKF) and the inertial basis combined navigation method (LG-EKF-e) with the projection system as the earth system under the definition of Lie group right error when the initial horizontal attitude error angle is 1° and the heading error angle is 180° respectively; Figure 5The figure is a comparison diagram of the combined navigation results of using the velocity error state transformation Kalman filter (ST-EKF) and the filtering method of the present invention (LG-EKF-w) respectively when the initial horizontal attitude error angle is 1° and the heading error angle is 180°. DETAILED DESCRIPTION

[0017] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0018] The present invention provides an inertial base combined navigation method with enhanced Lie group autonomy characteristics, aiming to replace the conventional extended Kalman filter with a Lie group Kalman filter based on world coordinate system projection, while taking the inertial space as the velocity reference system, the earth system as the attitude and position reference system, and the world coordinate system as the projection system, so as to achieve more accurate navigation and positioning results.

[0019] Reference Figure 1 In one embodiment, an inertial base combined navigation method with enhanced Lie group autonomy is provided, comprising: Receive inertial navigation system navigation information, odometer navigation information, and positioning system navigation information of the moving body; According to the initial navigation state of the moving body and the inertial navigation information, a Kalman filter system equation with enhanced Lie group autonomy characteristics based on the projection of the world coordinate system is constructed; Take the estimated speed of the odometer as the observed quantity and construct the speed observation equation; According to the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection and the velocity observation equation, the inertial basis integrated navigation is performed, and the state and covariance of the inertial basis integrated navigation are obtained as the state and covariance of the Kalman filter observation update; According to the Lie group error state transformation, the attitude, velocity and position are updated.

[0020] The inertial navigation information in the present invention includes: navigation timestamp, roll angle, pitch angle, heading angle, longitude, latitude, altitude and three-dimensional speed information under the local navigation system; Odometer navigation information includes: navigation timestamp, carrier speed; The positioning system navigation information is navigation information from a satellite positioning system or an acoustic positioning system, including a navigation timestamp and absolute three-dimensional positioning information.

[0021] Furthermore, a Kalman filter system equation based on the enhanced Lie group autonomy characteristic of the world coordinate system projection is constructed, including: The coordinate system of the inertial navigation system is b The world coordinate system is w Tie; Define the filter state vector ,as follows: ; in represents the attitude error angle of the inertial navigation system, The projection is defined relative to the inertial coordinate system. w The right Lie group velocity error under the system, for relative to w The projection defined by the system is w The right Lie group position error under the system, is the gyroscope constant bias, is the constant zero bias of the accelerometer, is the odometer scale factor error, are the pitch and yaw installation error angles and the lever arm error between the inertial navigation system and the odometer, respectively, and are modeled as follows: ; in is the correlation time of the first-order Markov process, is the odometer noise.

[0022] The combined navigation system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection is as follows: ; The coordinate system of the inertial navigation system is b The world coordinate system is w Tie, represents the filter state vector The derivative of ; ; ; ; in is the correlation time of the first-order Markov process, is the Earth's rotation vector w The projection of the system, For b Tie to w The direction cosine matrix of the system, For projection on w The calculated value of the gravitational acceleration of the system, for w Relative e The location of the system isw The projection of the system, e The Earth coordinate system. Relative to the moving body w The initial position of the system is w The projection of the system, is the relative inertial coordinate system of the moving body (i.e. i The speed of the system is w The calculated value of the projection of the system, Relative to the moving body w The location of the system is w The calculated value of the projection of the system, Indicates converting a vector into its corresponding antisymmetric matrix. Indicates that the parameter is a calculated value. , are the noise vector of the accelerometer and the noise vector of the gyroscope, is the odometer noise.

[0023] Furthermore, the attitude error angle of the inertial navigation system The differential equation is: ; in is the Earth's rotation vector w The projection of the system, For b Tie to w The calculated value of the direction cosine matrix of the system, is the gyro error.

[0024] The right Lie group velocity error constructed in the present invention The differential equation is: ; in, for relative to e The attitude error angle of the inertial navigation system is w The projection of the system, is the accelerometer error; Right Lie Group Position Error The differential equation is: ; in For b Tie to w The calculated value of the direction cosine matrix of the system, is the gyro error.

[0025] The gyroscope constant bias, accelerometer constant bias, lever arm error, pitch and yaw installation error angles between the inertial navigation system and the odometer are considered constants, and the odometer scale factor error is considered a first-order Markov process, and its differential equations are as follows: ; .

[0026] The present invention obtains the estimated speed of the odometer after the speed calculated by the inertial navigation system is corrected by the lever arm. m The following is expressed as: ; in Indicates the odometer calculated by the inertial navigation m The equivalent velocity of the projection under the system, m The system is the odometer coordinate system.

[0027] according to ; Ignoring the second-order small quantity, the velocity observation equation is: ; in Indicates the speed of the odometer in m Projection Indicates that the odometer output is m The velocity under the system, the velocity observation matrix ; is the velocity observation noise, For b Tie to m The attitude transformation matrix of the system, For w Tie to b The attitude transformation matrix of the system, Relative to the moving body w The projection of the angular velocity of the system in the carrier coordinate system, ; ; is the odometer forward speed.

[0028] According to the Lie group error state transformation, the attitude, velocity and position are updated, where the attitude, velocity and position are updated as follows: ; in The velocity of the moving body relative to the inertial coordinate system is w Projection of the system; is the velocity of the moving body relative to the inertial coordinate systemw The calculated value of the projection of the system, Relative to the moving body w The location of the system is w The projection of the system, Relative motion w The location of the system is w The calculated value of the projection of the system.

[0029] The present invention innovatively constructs a composite parameter model for representing the velocity of the relative inertial system and the attitude / position of the relative earth system, and establishes a canonical Lie algebraic mapping of the error state through the projection of the world coordinate system, so that the error state equation more strictly satisfies the autonomous characteristics of the Lie group manifold; The present invention establishes a Lie Group Kalman Filter (LG-EKF-R) algorithm in the form of right-multiplied error. Compared with the traditional left-error model, this architecture has better local linear approximation characteristics, which reduces the computational complexity while ensuring the estimation accuracy. The initial navigation state decoupling technology in the present invention adopts an explicit decoupling method of the initial velocity position parameters of the moving body in the derivation of the error model, and significantly improves the filter convergence speed by introducing a normalized initial variance condition setting.

[0030] In order to verify the effectiveness of the inertial base combined navigation method with enhanced Lie group autonomy provided by the present invention, an on-board experiment is used for experimental verification. The on-board experimental equipment includes a high-precision fiber optic gyro inertial navigation system, a wheel odometer, a satellite receiver, etc. Figure 2 The schematic diagram of the vehicle-mounted inertial / odometer combined navigation space configuration. The raw data frequency of the inertial navigation system's gyro with a table is 200Hz, and the zero bias is 0.003° / h and 10 , random walk are 0.0003 and , the resolution of the odometer is , the satellite single-point positioning accuracy is better than 1 meter. In order to increase the credibility and adequacy of the experiment, a set of open-loop trajectory experiments were conducted. The experimental trajectory is as follows Figure 3 As shown. Four filtering schemes, namely, EKF (extended Kalman filter), ST-EKF (speed error state transformation Kalman filter), LG-EKF-e (inertial base combined navigation method with the projection system as the earth system under the definition of Lie group right error), and the inertial base combined navigation method (LG-EKF-w) with enhanced Lie group autonomy characteristics provided by the present invention, are used for combined navigation positioning. Figure 4 The comparison chart of the combined navigation results of the extended Kalman filter (EKF) and the inertial basis combined navigation method (LG-EKF-e) with the projection system as the earth system under the definition of Lie group right error when the initial horizontal attitude error angle is 1° and the heading error angle is 180° respectively; Figure 5The comparison diagram of the combined navigation results of the velocity error state transformation Kalman filter (ST-EKF) and the filtering method of the present invention (LG-EKF-w) is shown when the initial horizontal attitude error angle is 1° and the heading error angle is 180°. It can be seen that the positioning accuracy of the EKF algorithm is the worst, and the LG-EKF-w algorithm has the optimal positioning result under the condition of large initial misalignment angle.

[0031] One embodiment provides an inertial base integrated navigation device with enhanced Lie group autonomy, including: The first module is used to receive the inertial navigation system navigation information, odometer navigation information, and positioning system navigation information of the moving body; The second module is used to construct a Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection according to the initial navigation state of the moving body and the inertial navigation information; The third module is used to construct the speed observation equation by taking the estimated speed of the odometer as the observed quantity; The fourth module is used to perform inertial basis integrated navigation according to the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection and the velocity observation equation, and obtain the state and covariance of the inertial basis integrated navigation as the state and covariance of the Kalman filter observation update; The fifth module is used to update the attitude, velocity and position according to the Lie group error state transformation.

[0032] The implementation methods of the above modules and the construction of the model can all adopt the methods described in any of the above embodiments, which will not be repeated here.

[0033] On the other hand, the present invention provides a computer device, including a memory and a processor, the memory storing a computer program, and the processor implementing the steps of the inertial base combined navigation method with enhanced Lie group autonomy provided in any of the above embodiments when executing the computer program. The computer device may be a server. The computer device includes a processor, a memory, a network interface and a database connected via a system bus. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store sample data. The network interface of the computer device is used to communicate with an external terminal via a network connection.

[0034] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the inertial base combined navigation method with enhanced Lie group autonomy characteristics provided in any of the above embodiments.

[0035] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0036] Matters not covered by the present invention are known technologies.

[0037] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0038] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention. It should be pointed out that, for a person of ordinary skill in the art, several modifications and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

[0039] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An inertial base combined navigation method with enhanced Lie group autonomy, characterized in that: include: Receive inertial navigation system navigation information, odometer navigation information, and positioning system navigation information of the moving body; According to the initial navigation state of the moving body and the inertial navigation information, the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection is constructed as follows: ； The coordinate system of the inertial navigation system is b The world coordinate system is w Tie, represents the filter state vector The derivative of ； ； ； ； in is the correlation time of the first-order Markov process, is the Earth's rotation vector w The projection of the system, For b Tie to w The direction cosine matrix of the system, For projection on w The calculated value of the gravitational acceleration of the system, for w Relative e The location of the system is w The projection of the system, e The Earth coordinate system. Relative to the moving body w The initial position of the system is w The projection of the system, is the velocity of the moving body relative to the inertial coordinate system w The calculated value of the projection of the system, Relative to the moving body w The location of the system is w The calculated value of the projection of the system, Indicates converting a vector into its corresponding antisymmetric matrix. , are the noise vector of the accelerometer and the noise vector of the gyroscope, is the odometer noise; Take the estimated speed of the odometer as the observed quantity and construct the speed observation equation; According to the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection and the velocity observation equation, the inertial basis integrated navigation is performed, and the state and covariance of the inertial basis integrated navigation are obtained as the state and covariance of the Kalman filter observation update; According to the Lie group error state transformation, the attitude, velocity and position are updated.

2. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 1, characterized in that: The inertial navigation system navigation information includes: navigation timestamp, roll angle, pitch angle, heading angle, longitude, latitude, altitude and three-dimensional speed information under the local navigation system; The odometer navigation information includes: navigation timestamp, carrier speed; The positioning system navigation information is navigation information from a satellite positioning system or an acoustic positioning system, including a navigation timestamp and absolute three-dimensional positioning information.

3. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 2 is characterized in that: Define the filter state vector ,as follows: in represents the attitude error angle of the inertial navigation system, The projection is defined relative to the inertial coordinate system. w The right Lie group velocity error under the system, for relative to w The projection defined by the system is w The right Lie group position error under the system, is the gyroscope constant bias, is the constant zero bias of the accelerometer, is the odometer scale factor error, are the pitch and yaw installation error angles and the lever arm error between the inertial navigation system and the odometer, respectively, and are modeled as follows: in is the correlation time of the first-order Markov process, is the odometer noise.

4. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 3 is characterized in that: Attitude error angle of inertial navigation system The differential equation is: in is the Earth's rotation vector w The projection of the system, For b Tie to w The calculated value of the direction cosine matrix of the system, is the gyro error.

5. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 3 is characterized in that: Right Lie group velocity error The differential equation is: in, for relative to e The attitude error angle of the inertial navigation system is w The projection of the system, is the accelerometer error.

6. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 3 is characterized in that: Right Lie Group Position Error The differential equation is: in For b Tie to w The calculated value of the direction cosine matrix of the system, is the gyro error.

7. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 3, characterized in that: The gyroscope constant bias, accelerometer constant bias, lever arm error, pitch and yaw installation error angles between the inertial navigation system and the odometer are considered constants, and the odometer scale factor error is considered a first-order Markov process, and its differential equations are as follows: 。 8. The inertial base integrated navigation method with enhanced Lie group autonomy according to any one of claims 4 to 7, characterized in that: The speed calculated by the inertial navigation system is corrected by the lever arm to obtain the estimated speed of the odometer; Construct the velocity observation equation as follows: in Indicates the odometer calculated by the inertial navigation m The equivalent velocity of the projection under the system, m The system is the odometer coordinate system. Indicates that the odometer output is m Velocity under the system; velocity observation matrix ； is the velocity observation noise, For b Tie to m The attitude transformation matrix of the system, For w Tie to b The attitude transformation matrix of the system, Relative to the moving body w The projection of the angular velocity of the system in the carrier coordinate system, ， is the odometer forward speed.

9. The inertial base integrated navigation method with enhanced Lie group autonomy according to claim 8, characterized in that: According to the Lie group error state transformation, the attitude, velocity and position are updated, where the attitude, velocity and position are updated as follows: in The velocity of the moving body relative to the inertial coordinate system is w Projection of the system; is the velocity of the moving body relative to the inertial coordinate system w The calculated value of the projection of the system, Relative to the moving body w The location of the system is w The projection of the system, Relative motion w The location of the system is w The calculated value of the projection of the system.

10. An inertial base integrated navigation device with enhanced Lie group autonomy, characterized in that: include: The first module is used to receive the inertial navigation system navigation information, odometer navigation information, and positioning system navigation information of the moving body; The second module is used to construct the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection according to the initial navigation state of the moving body and the inertial navigation information, as follows: ； The coordinate system of the inertial navigation system is b The world coordinate system is w Tie, represents the filter state vector The derivative of ； ； ； ； in is the correlation time of the first-order Markov process, is the Earth's rotation vector w The projection of the system, For b Tie to w The direction cosine matrix of the system, For projection on w The calculated value of the gravitational acceleration of the system, for w Relative e The location of the system is w The projection of the system, e The Earth coordinate system. Relative to the moving body w The initial position of the system is w The projection of the system, is the velocity of the moving body relative to the inertial coordinate system w The calculated value of the projection of the system, Relative to the moving body w The location of the system is w The calculated value of the projection of the system, Indicates converting a vector into its corresponding antisymmetric matrix. , are the noise vector of the accelerometer and the noise vector of the gyroscope, is the odometer noise; The third module is used to construct the speed observation equation by taking the estimated speed of the odometer as the observed quantity; The fourth module is used to perform inertial basis integrated navigation according to the Kalman filter system equation based on the enhanced Lie group autonomy characteristics of the world coordinate system projection and the velocity observation equation, and obtain the state and covariance of the inertial basis integrated navigation as the state and covariance of the Kalman filter observation update; The fifth module is used to update the attitude, velocity and position according to the Lie group error state transformation.

Images