Integrated navigation system design method, device and storage medium based on Lie group modeling

Abstract

The present application proposes a design method, device and storage medium for an integrated navigation system based on Lie group modeling, the method comprising: solving an inertial navigation system in an earth coordinate system to obtain a system state quantity of the inertial navigation system; compiling a state quantity under a Lie group based on the system state quantity, and deriving a right-invariant error state model, and setting a Lie algebra of the right-invariant error state quantity; deriving the error state quantity and state recursion equation of the above model; obtaining a speed measurement equation of a satellite navigation system and / or an odometer system, and updating the position of the integrated navigation system in combination with inertial measurement information in a corresponding sub-filter and outputting it; using a federal framework method to perform decentralized order reduction processing, inputting the output data of each filter into a main filter for information fusion, and performing optimal fusion with an estimate of the main filter to obtain a global estimate; and feeding back an information sharing factor to each sub-filter through the main filter to reset the state covariance matrix.

Description

Technical Field

[0001] The present application relates to the technical field of integrated navigation of multi-source information fusion, and in particular to an integrated navigation system design method, computer equipment and storage medium based on Lie group modeling. Background Art

[0002] With the widespread application of integrated navigation systems in various types of equipment such as automobiles, ships, and aircraft, the requirements for positioning accuracy and stability of navigation systems are becoming increasingly higher. Integrated navigation algorithm design is one of the most important technologies in modern aircraft, vehicles, and ships. However, since these sensors themselves have limitations and each sensor has different characteristics and working principles, the heterogeneity of navigation data leads to the complexity of data analysis and processing, as well as differences in the accuracy of each sensor data, which affects the overall navigation accuracy of the system.

[0003] In order to improve the positioning accuracy and stability of the integrated navigation system in environments where GNSS is easily blocked, such as modern urban roads and tunnels, data fusion is one of the main methods to solve the problem of heterogeneous data in the existing technology. Traditional data fusion methods are based on Kalman Filter or Extended Kalman Filter to achieve data fusion and correction through state estimation and error compensation. However, these methods require linear processing of the system, and there are certain difficulties in modeling and state estimation of nonlinear systems. In addition, due to problems such as error accumulation and noise interference in the data fusion process, the overall performance of the system is reduced.

[0004] In recent years, methods based on Lie group modeling have gradually attracted the attention of researchers and have been widely used in visual navigation. The integrated navigation algorithm can achieve high-precision positioning and attitude estimation by fusing the measurement results of multiple sensors with the motion model. However, the positioning accuracy of the Lie group-based integrated navigation algorithm is restricted by many factors. The coordinate systems between different sensors are not unified, which requires coordinate transformation and state conversion, increasing the complexity and computational overhead of the algorithm. Secondly, in the process of sensor data fusion, since the update of the state quantity depends on the state of the previous moment, the system is less robust to noise and uncertainty factors, resulting in problems such as limited system accuracy. Summary of the invention

[0005] The present application aims to solve one of the technical problems in the related art at least to some extent.

[0006] To this end, the first purpose of this application is to propose a comprehensive navigation system design method based on Lie group modeling, which solves the technical problems of high complexity and limited accuracy of existing methods and can effectively improve.

[0007] The second object of the present application is to provide a computer device.

[0008] A third object of the present application is to provide a non-transitory computer-readable storage medium.

[0009] To achieve the above-mentioned purpose, the first embodiment of the present application proposes a method for designing an integrated navigation system based on Lie group modeling. The integrated navigation system includes an inertial navigation system, a satellite navigation system, an odometer system, a first sub-filter, a second sub-filter and a main filter. The above-mentioned design method includes: solving the inertial navigation system in the earth coordinate system to obtain the system state quantity of the inertial navigation system; compiling the state quantity under the Lie group based on the system state quantity, and deriving the right invariant error state model based on the state quantity under the Lie group, and setting the Lie algebra of the state quantity of the right invariant error state model during the derivation; deriving the error state quantity and state recursion equation of the right invariant error state model; obtaining the speed measurement equation of the satellite navigation system and / or the odometer system, and in the corresponding sub-filter based on the error state quantity The method comprises the following steps: updating the position of the integrated navigation system and outputting the difference state quantity and the state recursion equation in combination with the corresponding speed measurement equation, wherein the first sub-filter corresponds to the speed measurement equation of the satellite navigation system, and the second sub-filter corresponds to the speed measurement equation of the odometer system; adopting the method of the federal framework to perform decentralized order reduction processing, inputting the output data of the first sub-filter and / or the second sub-filter into the main filter for information fusion, performing optimal fusion based on the fused information and combining the estimation of the main filter to obtain a global estimation, wherein the estimation of the main filter is generated based on the error state quantity and the state recursion equation; updating the state based on the global estimation through the main filter, and feeding back the information sharing factor to the first sub-filter and / or the second sub-filter through the main filter to reset the state covariance matrix.

[0010] The integrated navigation system design method based on Lie group modeling in the embodiment of the present application first obtains navigation information such as position, velocity and attitude of the inertial navigation system (INS) by solving it in a specified coordinate system. Secondly, by writing the state quantity under the SE2(3) group, the Lie group is used to realize the unified representation of the state. In this way, the rewritten state matrix will not be affected by the state itself at the previous moment. Then, the Lie group state representation is used in the heterogeneous information fusion subfilter to realize the effective fusion of heterogeneous data. At the same time, the feedback adjustment factor is used to realize the adaptive update of the state quantity. This enables the system to better adapt to the influence of noise and uncertainty factors, and improves the robustness and accuracy of the system.

[0011] Optionally, in one embodiment of the present application, the system state quantity is expressed as:

[0012]

[0013] in, , , is the attitude, velocity, and position state relative to the earth coordinate system, , is the angular velocity and acceleration deviation of the inertial navigation device.

[0014] Optionally, in one embodiment of the present application, the state quantity under the Lie group is the state quantity under the SE2(3) group, and the state quantity under the Lie group is expressed as:

[0015]

[0016] in, , , It is the attitude, velocity and position state relative to the earth coordinate system.

[0017] Optionally, in one embodiment of the present application, the right invariant error state model is expressed as:

[0018]

[0019] in, is the state quantity under the Lie group, It is the inverse operation of state quantity estimation;

[0020] Let the Lie algebra be , the derived right invariant error state model is expressed as:

[0021]

[0022] Where I represents the identity matrix, represents the first-order Taylor approximation to the exponential function.

[0023] Optionally, in one embodiment of the present application, the error state quantity is a linear error state quantity, and the error state quantity is expressed as:

[0024]

[0025] in, is the Lie group error state The Lie algebra under the tangent space of It is obtained by the logarithmic mapping under the SE2 (3) group, , Lie group state quantity It is obtained by the exponential mapping under the Lie group, , .

[0026] Optionally, in one embodiment of the present application, the state recursion equation is expressed as:

[0027]

[0028] The linearized state recursion equation is expressed as:

[0029]

[0030]

[0031] in,

[0032]

[0033]

[0034] represents the 3×3 identity matrix, represents the angular velocity of the Earth's rotation in the e system, represents the pose estimation, represents the gravity vector transformation, represents the transformed estimate of velocity, Represents a position estimate.

[0035] Optionally, in one embodiment of the present application, the velocity error measurement of the satellite navigation system is expressed as:

[0036]

[0037] in, represents an estimate of the velocity, It represents the speed of the inertial navigation output. Indicates the error, Represents the attitude relative to the Earth coordinate system;

[0038] The velocity measurement equation of the satellite navigation system is expressed as:

[0039]

[0040] in, represents the 3×3 identity matrix, X represents the system state vector, represents measurement noise, H represents the measurement matrix;

[0041] The speed error measurement of the odometer system is expressed as:

[0042]

[0043] in, represents an estimate of the velocity, represents the attitude relative to the Earth coordinate system, It represents the speed of the inertial navigation output. Indicates the position of the inertial navigation output, represents the angular velocity of the Earth's rotation in the e system, represents the attitude error, represents the speed error, Indicates position error;

[0044] The motion measurement equation of the odometer system is expressed as:

[0045]

[0046] in, Indicates the position of the inertial navigation output, represents the angular velocity of the Earth's rotation in the e system, represents the 3×3 identity matrix, X represents the system state vector, represents measurement noise, and H represents the measurement matrix.

[0047] Optionally, in one embodiment of the present application, the sub-filter and the main filter distribute information evenly.

[0048] To achieve the above-mentioned purpose, the second aspect of the present application proposes a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the above-mentioned comprehensive navigation system design method based on Lie group modeling is implemented.

[0049] In order to achieve the above-mentioned objectives, the third aspect embodiment of the present application proposes a non-temporary computer-readable storage medium, which, when the instructions in the storage medium are executed by a processor, can execute the above-mentioned comprehensive navigation system design method based on Lie group modeling.

[0050] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through the practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0052] Figure 1 A flowchart of a comprehensive navigation system design method based on Lie group modeling provided in Example 1 of the present application;

[0053] Figure 2 This is a block diagram of the system hardware structure of an embodiment of the present application;

[0054] Figure 3 A system device connection diagram of an embodiment of the present application;

[0055] Figure 4 This is a flow chart of the Lie group-based comprehensive navigation algorithm according to an embodiment of the present application;

[0056] Figure 5 This is a schematic diagram of the multi-source fusion results of an embodiment of the present application. DETAILED DESCRIPTION

[0057] Embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present application, and should not be construed as limiting the present application.

[0058] The following describes the integrated navigation system design method and device based on Lie group modeling according to an embodiment of the present application with reference to the accompanying drawings.

[0059] Figure 1 A flowchart of a comprehensive navigation system design method based on Lie group modeling provided in Example 1 of the present application.

[0060] like Figure 1 As shown, the integrated navigation system design method based on Lie group modeling includes the following steps:

[0061] Step 101, solving the inertial navigation system in the earth coordinate system to obtain the system state quantity of the inertial navigation system;

[0062] Step 102, compiling a state quantity under a Lie group based on the system state quantity, and deriving a right invariant error state model based on the state quantity under the Lie group, and setting the Lie algebra of the state quantity of the right invariant error state model during the derivation;

[0063] Step 103, deriving the error state quantity and state recursion equation of the right invariant error state model;

[0064] Step 104, obtaining a speed measurement equation of the satellite navigation system and / or the odometer system, and updating the position of the integrated navigation system in the corresponding sub-filter based on the error state quantity and the state recursion equation in combination with the corresponding speed measurement equation and outputting the position, wherein the first sub-filter corresponds to the speed measurement equation of the satellite navigation system, and the second sub-filter corresponds to the speed measurement equation of the odometer system;

[0065] Step 105, using a federated framework method to perform decentralized order reduction processing, inputting the output data of the first sub-filter and / or the second sub-filter into the main filter for information fusion, performing optimal fusion based on the fused information and the estimation of the main filter to obtain a global estimation, wherein the estimation of the main filter is generated based on the error state quantity and the state recursion equation;

[0066] Step 106: Update the state based on the global estimation through the main filter, and feed back the information sharing factor to the first sub-filter and / or the second sub-filter through the main filter to reset the state covariance matrix.

[0067] The integrated navigation system design method based on Lie group modeling in the embodiment of the present application first obtains navigation information such as position, velocity and attitude of the inertial navigation system (INS) by solving it in a specified coordinate system. Secondly, by writing the state quantity under the SE2(3) group, the Lie group is used to realize the unified representation of the state. In this way, the rewritten state matrix will not be affected by the state itself at the previous moment. Then, the Lie group state representation is used in the heterogeneous information fusion subfilter to realize the effective fusion of heterogeneous data. At the same time, the feedback adjustment factor is used to realize the adaptive update of the state quantity. This enables the system to better adapt to the influence of noise and uncertainty factors, and improves the robustness and accuracy of the system.

[0068] Optionally, in one embodiment of the present application, the system state quantity is expressed as:

[0069]

[0070] in, , , is the attitude, velocity, and position state relative to the earth coordinate system, , is the angular velocity and acceleration deviation of the inertial navigation device.

[0071] Optionally, in one embodiment of the present application, the state quantity under the Lie group is the state quantity under the SE2(3) group, and the state quantity under the Lie group is expressed as:

[0072]

[0073] in, , , It is the attitude, velocity and position state relative to the earth coordinate system.

[0074] Optionally, in one embodiment of the present application, the right invariant error state model is expressed as:

[0075]

[0076] in, is the state quantity under the Lie group, It is the inverse operation of state quantity estimation;

[0077] Let the Lie algebra be , the derived right invariant error state model is expressed as:

[0078]

[0079] Where I represents the identity matrix, represents the first-order Taylor approximation to the exponential function.

[0080] Optionally, in one embodiment of the present application, the error state quantity is a linear error state quantity, and the error state quantity is expressed as:

[0081]

[0082] in, is the Lie group error state The Lie algebra under the tangent space of It is obtained by the logarithmic mapping under the SE2 (3) group, , Lie group state quantity It is obtained by the exponential mapping under the Lie group, , .

[0083] Optionally, in one embodiment of the present application, the state recursion equation is expressed as:

[0084]

[0085] The linearized state recursion equation is expressed as:

[0086]

[0087]

[0088] in,

[0089]

[0090]

[0091] represents the 3×3 identity matrix, represents the angular velocity of the Earth's rotation in the e system, represents the pose estimation, represents the gravity vector transformation, represents the transformed estimate of velocity, Represents a position estimate.

[0092] Optionally, in one embodiment of the present application, the velocity error measurement of the satellite navigation system is expressed as:

[0093]

[0094] in, represents an estimate of the velocity, It represents the speed of the inertial navigation output. Indicates the error, Represents the attitude relative to the Earth coordinate system;

[0095] The velocity measurement equation of the satellite navigation system is expressed as:

[0096]

[0097] in, represents the 3×3 identity matrix, X represents the system state vector, represents measurement noise, H represents the measurement matrix;

[0098] The speed error measurement of the odometer system is expressed as:

[0099]

[0100] in, represents an estimate of the velocity, represents the attitude relative to the Earth coordinate system, It represents the speed of the inertial navigation output. Indicates the position of the inertial navigation output, represents the angular velocity of the Earth's rotation in the e system, represents the attitude error, represents the speed error, Indicates position error;

[0101] The motion measurement equation of the odometer system is expressed as:

[0102]

[0103] in, Indicates the position of the inertial navigation output, represents the angular velocity of the Earth's rotation in the e system, represents the 3×3 identity matrix, X represents the system state vector, represents measurement noise, and H represents the measurement matrix.

[0104] Optionally, in one embodiment of the present application, the sub-filter and the main filter distribute information evenly.

[0105] The embodiment of the present application also proposes a comprehensive navigation system based on Lie group modeling, specifically,

[0106] Figure 2 is a block diagram of the system hardware structure of this embodiment, Figure 3 This is the system equipment connection diagram of this embodiment. Figure 3 The manufacturer of the GNSS is NovAtel, and the model is OEM718D.

[0107] In terms of equipment installation, the platform is designed with placement of each sensor to ensure data accuracy and reliability. The RTK module is located in the middle of the bracket, connected to two positioning antennas, and provides high-precision positioning signals. Three IMUs with different accuracies are installed in the upper, middle, and lower positions of the bracket to measure the vehicle's motion status information. The wheel speed meter is installed on the rear wheel to measure the rotation speed. The single-point GNSS receiver is installed on the right side of the bracket and receives signals through the single-point antenna. The control board is fixed on the left side of the bracket, responsible for processing and controlling the data of each sensor and ensuring unimpeded transmission. The voltage regulator is placed under the bracket to ensure a stable power connection.

[0108] Secondly, in terms of equipment configuration, an appropriate clock source is set to enable all sensor data collection and output to be performed under the same time reference to ensure data consistency. At the same time, the serial port data received by the sensor is stamped with a system timestamp to facilitate subsequent data analysis. The RTK module is configured to ensure correct reception and processing of positioning signals and provide accurate GPS positioning data. The IMU is statically initialized and calibrated to reduce measurement errors. The wheel speed meter is calibrated to ensure the accuracy of the measurement data.

[0109] In terms of data configuration, the GPS receiver receives GPS signals and outputs GPS data synchronized with the system time. The Xsens IMU sensor collects and outputs data. The Xsens IMU sensor is manufactured by Xsens and its model is MIT-300. Other IMU sensors collect data and output IMU data synchronized with the system time. The wheel speed meter outputs speed data synchronized with the system time. Each sensor communicates through the serial port and transmits the collected data to the host computer for subsequent processing.

[0110] The above solution can effectively realize the installation, configuration and data transmission of sensors, laying a solid foundation for subsequent data analysis and processing.

[0111] The Lie group-based integrated navigation algorithm described in this embodiment is specifically: an invariant extended Kalman filter method, which constructs an inertial line linear error model based on Lie algebra.

[0112] Figure 4 This is a flow chart of the Lie group comprehensive navigation algorithm of this embodiment, as shown in Figure 4 As shown, the design and method of the integrated navigation system based on Lie group modeling includes the following steps:

[0113] Construct a system model of an integrated navigation system suitable for Lie group modeling.

[0114] Construct an observation model of an integrated navigation system suitable for Lie group modeling.

[0115] Construct the update equations for the integrated navigation system suitable for Lie group modeling.

[0116] An integrated navigation approach based on Lie group modeling for state estimation.

[0117] Initialization of the inertial navigation system requires initial alignment to bind the initial attitude, velocity and position information of the integrated navigation system, laying the foundation for inertial navigation solution.

[0118] The wheel speed meter initialization requires calibration of the scale coefficient error to lay the foundation for dead reckoning.

[0119] Construct a comprehensive navigation model suitable for Lie group modeling, including:

[0120] Determine the inertial navigation device coordinate system (i system) and the carrier coordinate system (b system) as its body coordinate system, and establish the attitude, velocity and position state quantities relative to the earth coordinate system (e system).

[0121] The calculated output data of the inertial navigation solution will be the three state components attitude ,speed and location , compiled into a state quantity under the SE2 (3) group.

[0122]

[0123] Derivation of the error state model of the right error model based on Lie group theory and state recursion equation.

[0124]

[0125] The right invariant error model is expressed as:

[0126]

[0127]

[0128] in, is the Lie group error state The Lie algebra under the tangent space of can be obtained by the logarithmic mapping under the SE2 (3) group, that is, The corresponding Lie group state quantity It can be obtained by the exponential mapping under the Lie group, that is For the deviation state quantity, linear addition update can be performed directly, that is, .

[0129] The state recursion equation is expressed as:

[0130]

[0131] The speed measurement equation obtained through the satellite navigation system and odometer system is combined with inertial measurement information to update the current system position, thereby achieving precise positioning.

[0132] The corresponding linearization error state and The recursion equation is:

[0133]

[0134]

[0135] in,

[0136]

[0137]

[0138] Furthermore, the measurement equation is expressed as:

[0139] Based on the design of the integrated navigation system, and based on the design of the satellite observation and odometer observation structure, the satellite navigation-assisted speed error measurement is given by the following formula:

[0140]

[0141] The corresponding measurement equation is, To measure noise.

[0142]

[0143] The speed error measurement based on odometer assistance is given by the following formula:

[0144]

[0145] The corresponding measurement equation is, To measure noise.

[0146]

[0147] The federated framework method is used for decentralized order reduction processing, and the output data of each sub-filter is input into the main filter for information fusion, and the estimation of the main filter is carried out. Perform optimal fusion together to obtain a global estimate .

[0148]

[0149]

[0150]

[0151] Where N=2, , , , is the final output variance matrix, is the final output state, represents the information sharing factor, m and f are the main filter and local filter respectively.

[0152] After the main filter performs information fusion, the main filter will feedback the information sharing factor to each sub-filter , to reset the state covariance matrix.

[0153]

[0154] The same filter model is used, and the main filter and the sub-filter adopt information sharing to finally achieve global optimal information fusion. The above design and method are verified through the integrated navigation system experimental platform designed by this application, which is used for the strapdown inertial navigation system, odometer system, satellite navigation system information collection system and computer processing system of integrated navigation.

[0155] In order to verify the inertial vision integrated navigation method based on Lie group state transformation provided in this embodiment, a vehicle-mounted integrated navigation system test was carried out to verify the effectiveness of the integrated navigation method based on Lie group modeling. Figure 5 The red line in the middle is the integrated navigation, and the black line is the reference value, which further verifies that the reliability of the system is improved under GNSS denial conditions.

[0156] In order to implement the above embodiments, the present application also proposes a non-temporary computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the method of the above embodiments is implemented.

[0157] In order to implement the above embodiment, the present application also proposes a computer program product. When the instruction processor in the computer program product is executed, the above integrated navigation system design method based on Lie group modeling is executed.

[0158] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example" or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

[0159] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0160] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present application belong.

[0161] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purposes of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in combination with these instruction execution systems, devices or apparatuses. More specific examples (non-exhaustive list) of computer-readable media include the following: an electrical connection with one or more wires (electronic device), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways if necessary, and then stored in a computer memory.

[0162] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0163] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0164] In addition, each functional unit in each embodiment of the present application may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0165] The storage medium mentioned above may be a read-only memory, a magnetic disk or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limiting the present application. A person of ordinary skill in the art may change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A method for designing an integrated navigation system based on Lie group modeling, characterized in that: The integrated navigation system includes an inertial navigation system, a satellite navigation system, an odometer system, a first sub-filter, a second sub-filter and a main filter, and the method includes: Solving the inertial navigation system in an earth coordinate system to obtain a system state quantity of the inertial navigation system; Based on the system state quantity, the state quantity under the Lie group is compiled, and the right invariant error state model is derived based on the state quantity under the Lie group. When deriving, the Lie algebra of the state quantity of the right invariant error state model is set, wherein the state quantity under the Lie group is the state quantity under the SE2 (3) group, and the state quantity under the Lie group is , the right invariant error state model is expressed as: in, is the state quantity under the Lie group, It is the inverse operation of state quantity estimation; Let the Lie algebra be , the Lie group error state quantity is expressed as: Where I represents the identity matrix, represents the first-order Taylor approximation of the exponential function; The error state quantity and state recursion equation of the right invariant error state model are derived, wherein the error state quantity is a linear error state quantity, and the error state quantity is expressed as: in, is the Lie group error state The Lie algebra under the tangent space of It is obtained by the logarithmic mapping under the SE2 (3) group, , Lie group state quantity It is obtained by the exponential mapping under the Lie group, , ; Obtain the speed measurement equation of the satellite navigation system and / or the odometer system, and update the position of the integrated navigation system in the corresponding sub-filter based on the error state quantity and the state recursion equation in combination with the corresponding speed measurement equation and output it, wherein the first sub-filter corresponds to the speed measurement equation of the satellite navigation system, the second sub-filter corresponds to the speed measurement equation of the odometer system, and the speed error measurement of the satellite navigation system is expressed as: in, represents an estimate of the velocity, It represents the speed of the inertial navigation output. Indicates the error, Represents the attitude relative to the Earth coordinate system; The velocity measurement equation of the satellite navigation system is expressed as: in, represents the 3×3 identity matrix, X represents the system state vector, represents measurement noise, H represents the measurement matrix; The speed error measurement of the odometer system is expressed as: in, represents an estimate of the velocity, represents the attitude relative to the Earth coordinate system, It represents the speed of the inertial navigation output. Indicates the position of the inertial navigation output, represents the angular velocity of the Earth's rotation in the e system, represents the attitude error, represents the speed error, Indicates position error; The kinematic measurement equation of the odometer system is expressed as: in, Indicates the position of the inertial navigation output, represents the angular velocity of the Earth's rotation in the e system, represents the 3×3 identity matrix, X represents the system state vector, represents measurement noise, H represents the measurement matrix; A decentralized order reduction process is performed by using a federated framework method, output data of the first sub-filter and / or the second sub-filter is input into the main filter for information fusion, and optimal fusion is performed based on the fused information and combined with an estimate of the main filter to obtain a global estimate, wherein the estimate of the main filter is generated based on the error state quantity and the state recursion equation; The state is updated based on the global estimation through the main filter, and the information sharing factor is fed back to the first sub-filter and / or the second sub-filter through the main filter to reset the state covariance matrix.

2. The integrated navigation system design method based on Lie group modeling as claimed in claim 1, characterized in that: The system state quantity is expressed as: in, , , is the attitude, velocity, and position state relative to the earth coordinate system, , is the angular velocity and acceleration deviation of the inertial navigation device.

3. The integrated navigation system design method based on Lie group modeling as claimed in claim 1, characterized in that: The state quantity under the Lie group is expressed as: in, , , It is the attitude, velocity and position state relative to the earth coordinate system.

4. The integrated navigation system design method based on Lie group modeling as claimed in claim 1, characterized in that: The state recursion equation is expressed as: The linearized state recursion equation is expressed as: in, represents the 3×3 identity matrix, represents the angular velocity of the Earth's rotation in the e system, represents the pose estimation, represents the gravity vector transformation, represents the transformed estimate of velocity, Represents a position estimate.

5. The integrated navigation system design method based on Lie group modeling as claimed in claim 1, characterized in that: The sub-filters share information equally with the main filter.

6. A computer device, characterized in that: The invention comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the integrated navigation system design method based on Lie group modeling as described in any one of claims 1 to 5 is implemented.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the integrated navigation system design method based on Lie group modeling as described in any one of claims 1 to 5 is implemented.

Images