RTK / INS integrated positioning method and system based on lie group model and gauss progressive filtering
By adopting an RTK/INS combined localization method based on Lie group model and Gaussian progressive filtering, the problem of insufficient characterization of attitude error coupling characteristics in traditional methods is solved, achieving high-precision localization in complex environments and improving the stability and adaptability of the system.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- ZHEJIANG UNIV OF TECH
- Filing Date
- 2025-10-21
- Publication Date
- 2026-07-07
AI Technical Summary
Traditional GNSS/INS combined positioning methods have limitations in error models in complex urban environments. They are unable to fully characterize the coupling characteristics of the directionality and magnitude of attitude errors, and their positioning accuracy decreases under signal blockage and multipath effects. Furthermore, the filters are prone to divergence and lack adaptive adjustment mechanisms.
An RTK/INS combined localization method based on Lie group model and Gaussian progressive filtering is adopted. By extending the pose Lie group definition to define left invariant error, an error state propagation model and a loosely coupled measurement model are established. Combined with the Gaussian progressive filtering algorithm, the coupling relationship between attitude and position is fused. Furthermore, a dynamic uncertainty estimation mechanism is introduced to adaptively adjust the noise covariance matrix.
It significantly improves positioning accuracy and robustness, especially in complex environments and high-dynamic motion, reduces the local linearization approximation of attitude error, improves system stability and real-time response capability, and suppresses filter divergence and state estimation inconsistency.
Smart Images
Figure CN121323639B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of high-precision navigation and integrated positioning technology, specifically relating to an RTK (Real-time Dynamic Positioning) / INS (Inertial Navigation System) integrated positioning method and system based on Lie group model and Gaussian progressive filtering. Background Technology
[0002] High-precision positioning and navigation technology is a core foundation for intelligent transportation, unmanned systems, and location services. With the widespread adoption of Global Navigation Satellite Systems (GNSS), GNSS-based positioning technology has been widely applied in fields such as autonomous driving, drones, and mobile robots.
[0003] However, traditional GNSS / INS integrated positioning methods face numerous challenges in complex urban environments, with the core problem stemming from the theoretical limitations of error models built on Euclidean space. Specifically, traditional methods typically employ small-angle linearization approximations to describe attitude errors, which fail to adequately characterize the coupling between directionality and magnitude. This theoretical deficiency is particularly pronounced under large misalignment angles or complex motion scenarios, easily leading to inconsistent or even divergent filter state estimates. Furthermore, signal obstruction and multipath effects in complex urban environments further exacerbate the decline in positioning accuracy and may even cause system failure.
[0004] To address the aforementioned issues, positioning filtering methods based on Lie group theory have offered new solutions in recent years. By directly defining the state of the positioning system (such as attitude, velocity, and position) on a Lie group manifold and utilizing the inherent geometric properties of group operations to reconstruct the error, this method can naturally integrate the coupling characteristics of direction and magnitude, significantly reducing the local linearization approximation error of attitude error in traditional Euclidean space. Nevertheless, existing positioning methods based on Lie group theory still have several key shortcomings. For example, the modeling of observation noise still relies on the Euclidean space assumption and does not fully consider the nonlinear propagation characteristics of observation errors on the Lie group manifold, resulting in a significant decrease in filtering performance when GNSS signals are unstable or contain outliers. Furthermore, the lack of an adaptive adjustment mechanism for system state uncertainties makes the filter prone to estimation bias or slow convergence in scenarios with drastic dynamic changes or large sensor drift. Summary of the Invention
[0005] To address the problems of insufficient modeling accuracy and limited environmental adaptability in existing Lie group-based positioning methods, this invention proposes an RTK / INS combined positioning method and system based on Lie group models and Gaussian progressive filtering. While inheriting the advantages of Lie group theory in nonlinear modeling of attitude and motion states, this invention introduces a dynamic uncertainty estimation mechanism and an efficient numerical solution strategy, which significantly improves the positioning stability and real-time response capability of the system in complex urban environments.
[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:
[0007] Firstly, a combined RTK / INS localization method based on Lie group model and Gaussian progressive filtering is provided, including:
[0008] The system acquires carrier phase and pseudorange measurements from the GNSS module in real time, as well as accelerometer and gyroscope data from the INS module.
[0009] Based on extended pose Lie group Define left-invariant error;
[0010] An error state propagation model for the INS module is established using left-invariant errors, and a loosely coupled measurement model for the GNSS module is also established using left-invariant errors.
[0011] If the real-time acquired carrier phase and pseudorange measurements are valid, the real-time positioning result is output using a Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model; otherwise, the real-time positioning result is estimated based on accelerometer and gyroscope data.
[0012] Several alternative methods are provided below, but they are not intended as additional limitations on the overall solution above. They are merely further additions or optimizations. Provided there are no technical or logical contradictions, each alternative method can be combined individually with respect to the overall solution above, or multiple alternative methods can be combined with each other.
[0013] Preferably, the extended pose Lie group-based Define left-invariant error, including:
[0014] In extended pose Li group Within the framework of the geocentric and geofixed coordinate system, an extended rigid body motion matrix is constructed, and the inverse matrix of the extended rigid body motion matrix is obtained;
[0015] The left-invariant error is defined as the product of the extended rigid body motion matrix and the inverse matrix estimate of the extended rigid body motion matrix, and a Lie algebra is generated.
[0016] Transform the Lie algebra into an antisymmetric matrix, and then perform an exponentiation operation on the antisymmetric matrix to obtain the extended pose Lie group. The Lie group element, i.e., the left-invariant error.
[0017] Preferably, the step of establishing an error state propagation model for the INS module using left-invariant error includes:
[0018] The error state vector includes the attitude error vector, velocity error coupling term, position error coupling term, accelerometer zero bias vector, and gyroscope zero bias vector;
[0019] Establish the differential equations of the extended rigid body motion matrix, and construct the left-invariant inertial navigation error equation based on the state transition matrix and the noise driving matrix;
[0020] The discrete-time error state update equation is obtained by discretizing the left-invariant inertial navigation error equation using the exponential integral method.
[0021] Preferably, the step of establishing a loosely coupled measurement model for the GNSS module using left-invariant errors includes:
[0022] Based on the carrier phase measurement and pseudorange measurement, the absolute position and absolute velocity in the geocentric and geofixed coordinate system are calculated, and the absolute position and absolute velocity are transformed into the navigation coordinate system to obtain the relative position and relative velocity.
[0023] The position error is the difference between the relative position of the GNSS module and the predicted position of the INS module, and the velocity error is the difference between the relative velocity of the GNSS module and the predicted velocity of the INS module.
[0024] Using position error and velocity error as measurements in the Gaussian progressive filtering algorithm, left-hand invariant measurement equations for position and velocity are constructed.
[0025] By combining the left-hand invariant position measurement equation and the left-hand invariant velocity measurement equation, the left-hand invariant loosely coupled measurement equation is obtained.
[0026] Preferably, the Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model outputs real-time positioning results, including:
[0027] Initialization state and error;
[0028] Let the observation likelihood follow a joint Gaussian distribution at the asymptotic pseudo-time.
[0029] The gain matrix of the Gaussian asymptotic Kalman filter algorithm is calculated based on the joint Gaussian distribution;
[0030] Adjust the measurement noise covariance matrix according to the asymptotic pseudo-time;
[0031] The posterior state estimate is updated by referencing the gain matrix and the measurement noise covariance matrix, and combining the measurement values from the GNSS module and the INS module.
[0032] By combining the updated posterior state estimate, the group state is compensated to obtain the updated group state.
[0033] If the amount of observation information injected during the incremental update is less than the threshold, the updated group state is output, and the fused attitude, position, and velocity are obtained as the real-time localization result; otherwise, the gain matrix is recalculated and iteration is performed.
[0034] Preferably, the step of adjusting the measurement covariance according to the asymptotic pseudo-time includes: dividing the original observation noise covariance by the asymptotic pseudo-time to obtain the adjusted measurement covariance.
[0035] The second aspect: Provides an RTK / INS integrated positioning system based on a Lie group model and Gaussian progressive filtering, including an RTK / INS data acquisition module, a progressive filtering fusion module, and a dynamic output module, wherein:
[0036] The RTK / INS data acquisition module is used to collect in real time the carrier phase measurement value and pseudorange measurement value output by the GNSS module, as well as the accelerometer data and gyroscope data output by the INS module.
[0037] The progressive filtering fusion module is used for using extended pose Lie groups. Define the left-invariant error; use the left-invariant error to establish an error state propagation model for the INS module, and use the left-invariant error to establish a loosely coupled measurement model for the GNSS module; if the real-time acquired carrier phase measurement values and pseudorange measurement values are valid, then use a Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model to output the real-time positioning result; otherwise, estimate the real-time positioning result based on accelerometer data and gyroscope data.
[0038] The dynamic output module is used to output the real-time positioning result to the terminal device, and the real-time positioning result is used by the terminal device to achieve real-time positioning.
[0039] The RTK / INS combined localization method and system based on Lie group model and Gaussian progressive filtering provided by this invention has the following advantages compared with the prior art:
[0040] 1. The positioning method provided by this invention starts from the definition of left-invariant error in Lie group space, and establishes the left-invariant inertial navigation error equation and the left-invariant loosely coupled measurement equation. Combined with the extended pose group... The Gaussian progressive filtering method significantly reduces the local linearization approximation error of attitude error in traditional Euclidean space, thus enabling high-precision positioning in various complex environments.
[0041] 2. The Lie group and Gaussian asymptotic Kalman filter algorithm proposed in this invention possesses excellent dynamic environment adaptability. Through a gradually stopping iterative measurement update mechanism, adaptive adjustment of the noise covariance matrix is achieved, effectively improving the state estimation accuracy and convergence speed of the system under large initial deviations and complex dynamic scenarios. This method exhibits significant advantages, especially in highly dynamic motion or signal instability conditions.
[0042] 3. This invention utilizes the definition and method of left-invariant error in Lie group space to effectively solve the large misalignment angle problem existing in traditional RTK / INS combined positioning methods. Compared with traditional methods that struggle to fully characterize the coupling characteristics of directionality and magnitude, this invention defines the navigation state in an extended pose group. This approach naturally integrates the coupling relationship between attitude and position, avoiding the geometric distortion problem in Euclidean space. Experimental results show that, compared with traditional methods, this invention effectively suppresses the problem of filter divergence and inconsistency in state estimation in complex motion scenarios, significantly improving positioning accuracy and robustness. Attached Figure Description
[0043] Figure 1 This is a schematic diagram of RTK / INS signal transmission, processing, and positioning according to the present invention;
[0044] Figure 2 The flowchart shows the RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering according to the present invention.
[0045] Figure 3 This is a schematic diagram of the RTK / INS combined localization system based on Lie group model and Gaussian progressive filtering according to the present invention.
[0046] Figure 4 This is a schematic diagram of the RTK / INS combined positioning device based on Lie group model and Gaussian progressive filtering according to the present invention. Detailed Implementation
[0047] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. 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.
[0048] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to limit the invention.
[0049] Example 1:
[0050] To address the insufficient accuracy of traditional filtering algorithms in scenarios with large misalignment angles, this embodiment proposes an RTK / INS combined localization method based on a Lie group model and Gaussian progressive filtering. This method utilizes an extended pose group... Starting with the definition and properties, a system was constructed. A matrix-form state-space model uniformly describes attitude, velocity, and position parameters; based on an extended pose Lie group. In the Earth-centered Earth-fixed coordinate system (abbreviated as Earth-fixed coordinate system) (system) and navigation coordinate system (abbreviated) The dynamic propagation equation and measurement equation of the left-invariant error were calculated and constructed under the system. Then, the left-invariant Gaussian asymptotic Kalman filter was proposed. The noise covariance was dynamically optimized through an adaptive asymptotic iteration update strategy to achieve high-precision real-time positioning.
[0051] like Figure 1 As shown, this embodiment uses a complex urban canyon environment as an example to analyze RTK / INS data and perform positioning. This method can be executed using a Gaussian progressive filtering algorithm. See the attached diagram for a schematic diagram of RTK / INS signal transmission, processing, and positioning. Figure 1 As shown, via satellites in the same system With mobile station and reference station By performing differential calculations and data fusion on pseudorange and carrier phase measurements, high-precision positioning can be achieved.
[0052] like Figure 2 As shown, this embodiment provides an RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering, specifically including the following steps:
[0053] Step 1: Real-time acquisition of carrier phase and pseudorange measurements output by the GNSS module, as well as accelerometer and gyroscope data output by the INS module.
[0054] The sensor data information acquired in this embodiment includes carrier phase measurements, pseudorange measurements, accelerometer data, and gyroscope data. The carrier phase and pseudorange measurements output by the GNSS module are acquired and calculated. Absolute position under the system and absolute speed Obtain accelerometer data output from the INS module. and gyroscope data It is used for local estimation of attitude, velocity and position at high frequency.
[0055] Step 2: Based on the extended pose Lie group (special Euclidean group) Define left-invariant error.
[0056] Based on Lie groups and The framework of the system models the extended rigid body motion matrix as elements of a Lie group. Represented as:
[0057] (1)
[0058] in Represents the carrier coordinate system (abbreviated as) (system) relative to The attitude matrix of the system; express System relative to The velocity vector of the system; express System relative to The position vector of the system. inverse matrix for:
[0059] (2)
[0060] in express System relative to The attitude matrix of the system; express Vector in Projection under the system; express Vector in The projection under the system.
[0061] For Li Qun left invariant error Defined as the relative transformation relation of Lie group elements, the specific form is as follows:
[0062] (3)
[0063] in Let represent the error defined on the group state, represent the estimated value, and have . = , Represents the inverse matrix The estimated value, pose matrix The estimated value, For projection The estimated value, For projection The estimated value, for The estimated value of the vector. Next, define... This is the attitude error vector; This is a velocity error coupling term; The position error coupling term. Therefore, Lie algebra Represented as:
[0064] (4)
[0065] in Represents the attitude error vector transpose, For velocity error coupling term transpose, Position error coupling term The transpose of . Simultaneously define the exponential mapping operator. This makes the elements of the Lie group Lie algebra generate:
[0066] (5)
[0067] in It is a mapping from a vector to an antisymmetric matrix, also called a cross product matrix transformation. Indicates Lie algebra Convert to an antisymmetric matrix.
[0068] Step 3: Use left-invariant error to establish an error state propagation model for the INS module, and use left-invariant error to establish a loosely coupled measurement model for the GNSS module.
[0069] Step 3.1: Establish an error state propagation model for the measurement input of the INS module.
[0070] The error state vector is selected as follows:
[0071] (6)
[0072] in and These represent the zero bias vectors of the accelerometer and the gyroscope, respectively. and They are and The transpose of . To derive the left-invariant error dynamic model, establish the Lie group elements. Differential equation:
[0073] (7)
[0074] in pose matrix The first derivative, Represents the cross product of vectors. This represents the angular velocity of the carrier relative to the Earth. This indicates the angular velocity measured by the INS module. Represents the Earth's angular velocity of rotation. velocity vector The first derivative, Indicates comparison, Represents gravitational acceleration. Indicates the speed of the carrier relative to the Earth. Represents position vector The first derivative.
[0075] Therefore, the left-invariant inertial navigation error equation can be obtained as follows:
[0076] (8)
[0077] (9)
[0078] (10)
[0079] (11)
[0080] in Error state variable The first derivative, This is the state transition matrix, which describes the dynamic coupling relationship between attitude, velocity, and position errors. This is the noise-driven matrix, representing how the measurement noise of the INS module is mapped to the error state space. This represents the input vector, describing the uncertainty and bias drift in the INS module. Represents angular velocity The estimated value, Indicates comparison The estimated value, and These represent the white noise terms of the accelerometer and gyroscope, respectively. and Indicates driving white noise, From Tie The estimated value of the rotation matrix of the system. Based on the continuous-time model of formulas (10) and (11), the discrete-time error state update equation (i.e., the left-invariant inertial navigation error state update equation) can be derived by the exponential integration method:
[0081] (12)
[0082] in express Error state vector at time step [time]. express The state transfer matrix at time t, express The state transition matrix at time t, express The noise state at any given time, which obeys Distributed process noise vector, Indicates process noise covariance, express The noise driving matrix at time step, This represents the transpose of the noise driving matrix. The covariance matrix representing the input noise, i.e., the power spectral density matrix, has its elements determined by the noise power spectral density of each sensor. This indicates the system's sampling period.
[0083] Step 3.2: Establish a loosely coupled measurement model for GNSS information.
[0084] The position observation equation of the slack combined navigation system (ignoring the lever arm effect) is modeled as follows:
[0085] (13)
[0086] in In order to be in The carrier position vector measured by the GNSS module under the system. Indicates in The estimated carrier location is based on INS. This indicates the positional deviation between the GNSS module and the INS module.
[0087] Absolute position calculated by the GNSS module and absolute speed Transformed to In the system, through the rotation matrix Implement and obtain the relative position of the GNSS module. Predicted location with INS module The difference is the position error. Take the relative velocity of the GNSS module Prediction speed with INS module The difference is the speed error. The formula is expressed as follows:
[0088] (14)
[0089] (15)
[0090] error and The measurements applied to the filter form a unified measurement equation, where the position (observation) left-hand invariant measurement equation is expressed as:
[0091] (16)
[0092] in This represents the positional residual between the GNSS module and the INS module; For defined in Lie algebra The position of the left invariant measures the residual; This is the position measurement matrix; For position observation noise, obey distributed; Represents the measurement noise covariance matrix. pose matrix The estimated value.
[0093] Furthermore, the left-hand invariant equation for velocity (observation) is expressed as:
[0094] (17)
[0095] in This represents the residual between the velocity measured by the GNSS module and the velocity estimated by the INS module; For defined in Lie algebra The velocity left invariant measures the residual; This indicates the speed at which the INS module solves the problem; For velocity measurement matrix; For velocity observation noise, obey distributed; This represents the velocity measurement noise covariance matrix. This indicates the speed deviation between the GNSS module and the INS module.
[0096] Therefore, the left-invariant loosely coupled measurement equation is expressed as:
[0097] (18)
[0098] in for The left-invariant measurement residual at time t is given by a value of t. and / or If the GNSS module outputs both position and velocity, then ; for The measurement matrix at time t, its values are... and / or , for The observation noise at time t, its value is and / or .
[0099] Step 4: If the real-time carrier phase measurement and pseudorange measurement values are valid, the real-time positioning result is output using a Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model; otherwise, the real-time positioning result is estimated based on accelerometer data and gyroscope data.
[0100] In this embodiment, "valid" means that the data is available, such as no data loss, the data is not pure noise, and the data is within the valid range. Furthermore, estimating the real-time positioning result based on the accelerometer and gyroscope data output from the INS module involves the conventional steps of coordinate system transformation combined with integral calculations, which will not be elaborated upon in this embodiment. Additionally, the specific steps of the Gaussian asymptotic filtering algorithm based on the error state propagation model and loosely coupled measurement model in this embodiment are as follows:
[0101] Step 4.1: Initialize the state and error. Initialization , and the initial group state , For prior state estimation, it means based on Information obtained at time State estimation at time; The posterior state after pseudo-time initialization represents the state at pseudo-time. At any time, The update estimate of the state at time step. The pseudo-time for initialization; Let be the prior error covariance, representing Uncertainty in prior estimation of state at time step; The error covariance after pseudo-time initialization represents the pseudo-time. time, The posterior uncertainty of the state is initialized with the prior covariance, and no new information is introduced due to pseudo-time.
[0102] Considering The state equation and measurement equation at time t are as follows:
[0103] (19)
[0104] in express Error state vector at time step [time]. express The left-hand invariant residual at time t, i.e., the observed value of the residual; process noise. and observation noise The noise is uncorrelated, zero-mean noise, process noise. and observation noise The covariances are respectively and .
[0105] Therefore in The predicted state at time t is:
[0106] (20)
[0107] in Indicates based on Information obtained at time The posterior estimated state at time 1; express The state transfer matrix at time step 1. The covariance of the predicted state error at time t is:
[0108] (twenty one)
[0109] in express The posterior estimate of the state error covariance at time t. State transfer matrix The transpose. In asymptotic pseudotime. At time t, the asymptotic posterior probability density function is expressed as:
[0110] (twenty two)
[0111] in For pseudo-time hour, Moment State The posterior probability density, representing the probability density over asymptotic pseudotime. Time, known Time measurement vector and historical measurement vector sequence At that time, state The conditional probability distribution; Indicates 1 to The sequence of measurement vectors at time points; for Moment State The prior probability density represents the sequence of historical measurement vectors known only by prior knowledge. At that time, Moment State The probability distribution; For the product of likelihood functions of multi-step asymptotic measurements; Indicates measurement With state The likelihood is the likelihood function of a single-step measurement, representing the known state. At that time, measurement The probability of occurrence Indicates the first Weighting factor for pseudo-time; This represents the total number of pseudo-time steps.
[0112] Step 4.2, let the asymptotic pseudo-time be... At that time, the observation likelihood follows a joint Gaussian distribution:
[0113] (twenty three)
[0114] in Indicating asymptotic pseudotime At that time, measurement With state The observed likelihood, For the mean block, Indicates pseudo-time hour, Posterior state estimation at time 1; Indicates pseudo-time hour, The predicted value at any given time; For the first Step time; For covariance blocks, Indicates pseudo-time The posterior error covariance of the state at time; Indicates pseudo-time Time-state covariance, Indicates pseudo-time Covariance of observation-state Indicates pseudo-time Time observation - covariance of observations.
[0115] Step 4.3: Calculate the gain matrix of the Gaussian progressive Kalman filter algorithm; , Indicates pseudo-time hour, Gain matrix at time step.
[0116] Step 4.4: Adjust the measurement noise covariance matrix according to the asymptotic pseudo-time. The adjusted measurement covariance is obtained. express The original observation noise covariance at any given time. This represents the measurement noise covariance matrix after pseudo-time adjustment.
[0117] Step 4.5: Update the posterior state estimate by combining the measurements from the GNSS module and the INS module:
[0118] (twenty four)
[0119] (25)
[0120] (26)
[0121] (27)
[0122] In this step, the GNSS module provides the measurement values. This reflects the GNSS module estimation results at the current moment. These are measurements predicted by the IMU module, reflecting the filter's estimation of the GNSS module's measurements, and are expressed through Kalman gain. The differences between the two This feedback is used to update the posterior estimate in the state estimation. Indicates pseudo-time hour, Posterior state estimation at time 1; False time The posterior error covariance of the state at time; Gain matrix transpose; In pseudo-time Measurement matrix at time, Measurement matrix transpose, In pseudo-time The measurement noise covariance matrix at time step.
[0123] Step 4.6: After each incremental update, check the group state. To obtain compensation , Indicates pseudo-time hour, The group state at any given moment; Indicates pseudo-time hour, The group state at any given moment.
[0124] Step 4.7: If the amount of observation information injected during the incremental update is less than the threshold (e.g.) ,in For a given threshold (e.g., a value of 0), output the updated group state. Group state covariance The fused attitude, position, and velocity are obtained as the real-time localization result, and the updated group state is used. It is the best estimate of the data collected by the GNSS module and the IMU module. The uncertainty of the state estimation is described, which can be used to evaluate positioning accuracy; otherwise, return to step 4.3 to recalculate the gain matrix and iterate. Based on this, it can be considered that each asymptotic process... , When the amount of observation information injected in the asymptotic update step is increased, the final group state is obtained. .
[0125] The RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering provided in this embodiment first employs an extended pose Lie group. As a mathematical representation of the navigation state, the state error was remodeled; secondly, the concept of left-invariant error was used to calculate... The error propagation equation under the group is proposed; finally, the adaptive adjustment of the noise covariance matrix is achieved through the gradual stopping iterative measurement update mechanism, thereby effectively improving the state estimation accuracy and convergence performance of the system under large initial deviations and high dynamic environments.
[0126] Example 2:
[0127] like Figure 3 As shown, this embodiment provides an RTK / INS combined localization system based on a Lie group model and Gaussian progressive filtering, including an RTK / INS data acquisition module, a progressive filtering fusion module, and a dynamic output module, wherein:
[0128] The RTK / INS data acquisition module is used to acquire in real time the carrier phase measurement and pseudorange measurement values output by the GNSS module, as well as the accelerometer data and gyroscope data output by the INS module.
[0129] A progressive filtering fusion module for use with extended pose Lie groups Define the left-invariant error; use the left-invariant error to establish an error state propagation model for the INS module, and use the left-invariant error to establish a loosely coupled measurement model for the GNSS module; if the real-time acquired carrier phase measurement value and pseudorange measurement value are valid, then use the Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model to output the real-time positioning result; otherwise, estimate the real-time positioning result based on accelerometer data and gyroscope data.
[0130] In this embodiment, the progressive filtering fusion module refers to the module that integrates the navigation state vector... Modeled as elements of a Lie group, where express System relative to Obtain the attitude matrix; express System relative to The velocity vector of the system; express System relative to The position vector of the system. Performed using group multiplication. To avoid Euclidean space linearization errors, the progressive filtering fusion module adds a pseudo-time parameter of 0-1 compared to a regular Kalman filter. ( ), initial stage Allow for high noise (relaxed correction), later ( The noise level is gradually tightened to the true noise level, and noise adaptation, global optimization and manifold geometry are integrated through pseudo-time parameters.
[0131] The dynamic output module is used to output real-time positioning results to the terminal device, which then uses these results to achieve real-time positioning. The dynamic output module outputs data including three-dimensional coordinates, heading angle, pitch angle, and positioning accuracy indicators, and sends navigation data to the terminal device so that the terminal device can achieve high-precision positioning and navigation based on the navigation data.
[0132] For specific limitations regarding the RTK / INS combined localization system based on Lie group model and Gaussian progressive filtering, please refer to the limitations of the RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering in Example 1. This example will not repeat them.
[0133] Example 3:
[0134] like Figure 4 As shown, this embodiment provides an RTK / INS combined positioning device based on Lie group modeling and Gaussian progressive filtering. The device includes: a data acquisition device, a data processing and analysis device, and a terminal device.
[0135] Data acquisition equipment is used to receive observation data, preprocess the observation data, and calculate the location information including noise from the raw data containing errors.
[0136] The data processing and analysis equipment is used to acquire measurement data sent by the data acquisition equipment, perform pose analysis on the observation data based on the established left-invariant error propagation equation, left-invariant loosely coupled measurement equation and Gaussian asymptotic filtering to obtain the pose estimation result of the current detection system, and output it to the terminal equipment.
[0137] Terminal equipment is used to receive pose estimation results sent by data processing and analysis equipment and to visualize observation data.
[0138] For specific limitations regarding the RTK / INS combined localization device based on Lie group model and Gaussian progressive filtering, please refer to the limitations of the RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering in Example 1. This example will not repeat them.
[0139] It should be noted that in the above embodiment of RTK / INS combined localization based on Lie group modeling and Gaussian progressive filtering, the various modules are divided according to functional logic and are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional unit are only for easy differentiation and are not used to limit the scope of protection of the present invention.
[0140] Example 4:
[0141] This embodiment provides a computer-readable storage medium storing a computer program thereon, characterized in that the computer program, when executed by a processor, implements the steps of the PUF-based IoT OTA upgrade method in Embodiment 1.
[0142] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.
[0143] Example 5:
[0144] To verify the performance of the proposed RTK / INS combined localization method (LIPGF) based on Lie group modeling and Gaussian filtering in complex environments, this experiment uses the UrbanNav public dataset as the benchmark platform. This dataset was collected in a typical urban canyon environment and has high scene representativeness. GNSS measurements were collected at 10Hz using a Trimble NetR9 receiver, and IMU measurements were recorded at 50Hz using a Tamakawaseiki TAG264 device, along with base station data and ground truth data (provided by a POS LV620) to ensure the reliability of the reference trajectory. The reference trajectory and some environmental scenarios cover complex scenarios such as tall building obstruction, overpass obstruction, tree shading, and open, unobstructed areas, reflecting the challenging conditions in practical applications. In the test, the positioning accuracy of the LIPGF method was compared with that of the Extended Kalman Filter (EKF), Left Invariant Extended Kalman Filter (LIEKF), and LIEKF combined with the IGG-3 robust factor (LIEKF-IGG3). Table 1 lists the experimental results of each method, using the mean positioning error and standard deviation as evaluation indicators.
[0145] Table 1 Comparison of Average Positioning Error and Standard Deviation of Different Filtering Methods
[0146]
[0147] Clearly, the LIPGF method of this invention maintains almost unchanged positioning error and standard deviation as the initial misalignment angle increases from 0 degrees to 150 degrees, demonstrating high robustness to the initial misalignment angle. This characteristic gives it a significant advantage in complex urban environments, further validating the superiority and practical application potential of the proposed method.
[0148] The technical features of the above embodiments can be combined in any way. For the sake of brevity, 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.
[0149] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.
Claims
1. A combined RTK / INS localization method based on Lie group model and Gaussian progressive filtering, characterized in that, The RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering includes: The system acquires carrier phase and pseudorange measurements from the GNSS module in real time, as well as accelerometer and gyroscope data from the INS module. Based on extended pose Lie group Define left-invariant error, including: in the extended pose Lie group Within the framework of the geocentric-ground-fixed coordinate system, an extended rigid body motion matrix is constructed, and its inverse is obtained. The left-invariant error is defined as the product of the extended rigid body motion matrix and the estimated value of its inverse, and a Lie algebra is generated. The Lie algebra is transformed into an antisymmetric matrix, and an exponential operation is performed on the antisymmetric matrix to obtain the extended pose Lie group. The elements of the Lie group, i.e., the left-invariant error; An error state propagation model for the INS module is established using left-invariant errors, and a loosely coupled measurement model for the GNSS module is also established using left-invariant errors. If the real-time acquired carrier phase and pseudorange measurements are valid, the real-time positioning result is output using a Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model; otherwise, the real-time positioning result is estimated based on accelerometer and gyroscope data.
2. The RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering according to claim 1, characterized in that, The method of establishing an error state propagation model for the INS module using left-invariant error includes: The error state vector includes the attitude error vector, velocity error coupling term, position error coupling term, accelerometer zero bias vector, and gyroscope zero bias vector; Establish the differential equations of the extended rigid body motion matrix, and construct the left-invariant inertial navigation error equation based on the state transition matrix and the noise driving matrix; The discrete-time error state update equation is obtained by discretizing the left-invariant inertial navigation error equation using the exponential integral method.
3. The RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering according to claim 1, characterized in that, The method of establishing a loosely coupled measurement model for the GNSS module using left-invariant errors includes: Based on the carrier phase measurement and pseudorange measurement, the absolute position and absolute velocity in the geocentric and geofixed coordinate system are calculated, and the absolute position and absolute velocity are transformed into the navigation coordinate system to obtain the relative position and relative velocity. The position error is the difference between the relative position of the GNSS module and the predicted position of the INS module, and the velocity error is the difference between the relative velocity of the GNSS module and the predicted velocity of the INS module. Using position error and velocity error as measurements in the Gaussian progressive filtering algorithm, left-hand invariant measurement equations for position and velocity are constructed. By combining the left-hand invariant position measurement equation and the left-hand invariant velocity measurement equation, the left-hand invariant loosely coupled measurement equation is obtained.
4. The RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering according to claim 1, characterized in that, The Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model outputs real-time positioning results, including: Initialization state and error; Let the observation likelihood follow a joint Gaussian distribution at the asymptotic pseudo-time. The gain matrix of the Gaussian asymptotic Kalman filter algorithm is calculated based on the joint Gaussian distribution; Adjust the measurement noise covariance matrix according to the asymptotic pseudo-time; The posterior state estimate is updated by referencing the gain matrix and the measurement noise covariance matrix, and combining the measurement values from the GNSS module and the INS module. By combining the updated posterior state estimate, the group state is compensated to obtain the updated group state. If the amount of observation information injected during the incremental update is less than the threshold, the updated group state is output, and the fused attitude, position, and velocity are obtained as the real-time localization result; otherwise, the gain matrix is recalculated and iteration is performed.
5. The RTK / INS combined localization method based on Lie group model and Gaussian progressive filtering according to claim 4, characterized in that, The step of adjusting the measurement noise covariance matrix according to the asymptotic pseudo-time includes: dividing the original observation noise covariance matrix by the asymptotic pseudo-time to obtain the adjusted measurement noise covariance matrix.
6. An RTK / INS combined localization system based on a Lie group model and Gaussian progressive filtering, characterized in that, The RTK / INS integrated positioning system based on Lie group model and Gaussian progressive filtering includes an RTK / INS data acquisition module, a progressive filtering fusion module, and a dynamic output module, wherein: The RTK / INS data acquisition module is used to collect in real time the carrier phase measurement value and pseudorange measurement value output by the GNSS module, as well as the accelerometer data and gyroscope data output by the INS module. The progressive filtering fusion module is used for using extended pose Lie groups. Define the left-invariant error; use the left-invariant error to establish an error state propagation model for the INS module, and use the left-invariant error to establish a loosely coupled measurement model for the GNSS module; if the real-time acquired carrier phase measurement values and pseudorange measurement values are valid, then use a Gaussian progressive filtering algorithm based on the error state propagation model and the loosely coupled measurement model to output the real-time positioning result; otherwise, estimate the real-time positioning result based on accelerometer data and gyroscope data. Among them, the Lie group based on extended pose Define left-invariant error, including: In extended pose Li group Within the framework of the geocentric and geofixed coordinate system, an extended rigid body motion matrix is constructed, and the inverse matrix of the extended rigid body motion matrix is obtained; The left-invariant error is defined as the product of the extended rigid body motion matrix and the inverse matrix estimate of the extended rigid body motion matrix, and a Lie algebra is generated. Transform the Lie algebra into an antisymmetric matrix, and then perform an exponentiation operation on the antisymmetric matrix to obtain the extended pose Lie group. The elements of the Lie group, i.e., the left-invariant error; The dynamic output module is used to output the real-time positioning result to the terminal device, and the real-time positioning result is used by the terminal device to achieve real-time positioning.