TDCP enhanced visual inertial odometer and navigation positioning method thereof

Abstract

The invention relates to a TDCP enhanced visual inertial odometer and a navigation positioning method thereof, the method aims at multi-sensor fusion positioning, uses an extended Kalman filter (EKF) to integrate visual features, IMU data and TDCP observation values, improves local pose estimation precision, and suppresses VIO error accumulation through absolute scale and course constraint provided by TDCP; meanwhile, the local estimation result and the global position of the GNSS are optimized and fused through a global pose map, and high-precision pose estimation which is globally consistent and within a global range is achieved. According to the method, optimization can be completed only through intermittent global positions, the problem of positioning failure of a traditional scheme in a GNSS challenge environment is solved, the problems of cycle slip accumulation and gross error interference when TDCP is independently used are effectively solved, and the method is suitable for automatic driving, unmanned aerial vehicles and other scenes with high real-time performance requirements.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology for intelligent unmanned systems, specifically involving a high-precision positioning method that integrates time differential carrier phase (TDCP), visual and inertial sensors, applicable to scenarios such as autonomous driving, drone navigation, and crowdsourced map building. Background Technology

[0002] Accurate vehicle localization is crucial in fields such as autonomous driving and crowdsourced map building. To achieve highly applicable and accurate localization, methods integrating multiple sensors with complementary characteristics have been extensively studied. Localization methods can be categorized into local and global types based on the sensors and applications.

[0003] Local positioning methods, such as Visual Simultaneous Localization and Mapping (V-SLAM) and Visual Inertial Odometry (VIO), can achieve accurate relative pose estimation and are suitable for local planning and control. However, in Visual Inertial Navigation Systems (VINS), pose estimation error accumulation is unavoidable because global translation and global rotation around the gravity vector are unobservable. The most typical and widely used global positioning method is the Global Navigation Satellite System (GNSS). Its Standard Point Positioning (SPP) can achieve meter-level global positioning, while differential GNSS (such as Real-Time Kinematics, RTK) and Precise Point Positioning (PPP) can achieve decimeter- to centimeter-level accuracy. However, high-precision GNSS positioning technology relies on high-precision correction information, limiting its applicability. To improve GNSS availability and continuity, it is often integrated with Inertial Navigation Systems (INS). However, low-cost IMUs make GNSS / INS vulnerable in complex environments, making it difficult to maintain accurate positioning when GNSS is unavailable or interrupted.

[0004] Furthermore, TDCP can obtain accurate relative observations from GNSS without high-precision correction. It obtains centimeter-level accuracy relative observations by differential processing of continuous GNSS epoch carrier phase measurements, providing high-precision velocity observations and accurate short-term relative positions. However, positioning relying on TDCP suffers from cumulative position errors, and the presence of cycle slips prevents TDCP from being used as an independent positioning method. Nevertheless, TDCP can provide absolute scale and heading information that is difficult to obtain through visual and inertial observations, theoretically significantly improving VIO performance. Summary of the Invention

[0005] The purpose of this invention is:

[0006] 1. Construct an algorithm framework for GNSS time differential carrier phase (TDCP) assisted / enhanced visual inertial odometry and global pose optimization;

[0007] 2. By fusing visual, inertial, and TDCP measurements, low-drift, high-precision local pose estimation is achieved;

[0008] 3. By combining the global pose graph optimization framework, the local estimation results are fused with the GNSS solution results to achieve globally consistent pose estimation.

[0009] The technical solution of the present invention is as follows:

[0010] A TDCP-enhanced visual inertial odometry navigation and positioning method, wherein TDCP is the difference between wave phase observations downloaded from adjacent GNSS epochs, and GNSS is a global navigation satellite system, the method comprising the following steps:

[0011] Step 1: Data preparation and preprocessing. Observational data is collected through the IMU, camera and GNSS receiver on the carrier. The internal and external parameters of the above sensors are pre-calibrated, and the IMU and camera sensors are triggered by the pulse per second (PPS) signal of the GNSS receiver to obtain a unified time reference in order to achieve the unification of the time system. Multi-GNSS navigation ephemeris is downloaded from the IGS data center. The images collected by the camera are extracted and tracked by Fast feature extraction and KLT optical flow tracking technology.

[0012] Step 2, System State Initialization and Prediction: First, the initial position of the vehicle is estimated using navigation ephemeris and observations, and the initial velocity and attitude are estimated using IMU output. Then, the system maintains navigation states such as position, velocity, attitude, and IMU zero bias, as well as historical pose states in the sliding window, and propagates the system state through INS mechanical orchestration based on the IMU output.

[0013] Step 3: Measurement Update. When visual feature points are lost during tracking or the corresponding system state is marginalized, a residual equation is constructed based on the co-view relationship between the historical pose state and the visual measurements to update the navigation state. TDCP Measurement Update. When carrier phase observations of adjacent GNSS epochs are obtained and the IMU pose state at the corresponding time exists, TDCP observations are constructed using the carrier phase observations of two adjacent epochs for measurement update. By combining the carrier phase observations of two adjacent GNSS epochs, the corresponding TDCP observations can be obtained, which are then used to constrain the IMU pose at the corresponding two times.

[0014] Step 4: Global pose optimization. Using the low-drift, high-precision local pose estimation results obtained from the measurement update in Step 3, and the global position provided by GNSS technology (SPP or RTK), the pose graph is fused to obtain a high-precision, globally consistent pose estimation.

[0015] As a further technical solution, in step 2, the system state is defined as the IMU navigation state and the IMU pose state at historical moments in the sliding window. Its error state vector is defined as follows:

[0016]

[0017] in, It indicates the IMU navigation error status, including position error, velocity error, attitude error, accelerometer bias error, and gyroscope bias error; It represents the error state of the IMU pose at a historical moment within the sliding window, and is mainly used to construct multi-state constraints for visual observation.

[0018] The specific definition of navigation error state is as follows:

[0019]

[0020] in, , and These represent the position, velocity, and attitude error states of the carrier in the e-frame (earth-centered earth-fixed (ECEF) coordinate system); and These represent the zero-bias error states of the IMU accelerometer and the gyroscope, respectively.

[0021] The error states of the IMU pose at N historical moments in the sliding window are defined as follows:

[0022]

[0023] in, , These represent the position and attitude error state of the IMU in the e-frame at the Nth historical moment, respectively.

[0024] The IMU output propagates the system state through the INS mechanical orchestration. A simplified linearized dynamic model of the INS can be represented as:

[0025]

[0026] In the above formula, , and These are the derivatives of position, velocity, and attitude, respectively. Represents the rotation matrix from the IMU system (b system) to the ECEF system (e system); and These represent specific force and the Earth's rotational angular velocity, respectively. and These are the errors in the acceleration and angular velocity output by the IMU, respectively. Represents the error vector of gravity; symbol This represents the cross product of vectors. For the zero-bias model of an IMU sensor, the zero-bias vector of the accelerometer... and gyroscope zero bias vector The system state is set as the state for estimation, and its changes are modeled as white noise. and Random walk.

[0027] The navigation state is propagated based on IMU observation data, and the covariance of the navigation state is also propagated accordingly according to the propagation model. Furthermore, the N historical IMU poses within the sliding window are considered constant values ​​without propagation noise. The complete system state propagation model is as follows:

[0028]

[0029] in, .

[0030] Based on the above propagation model, the covariance propagation of all system states can be calculated using the following formula:

[0031]

[0032] In the above formula, Represent the covariance matrix; It is the state transition matrix; Corresponding to the propagation noise matrix. The state in the historical pose window when processing a new image. The dimension will be expanded by adding a clone of the latest IMU pose to the sliding window to facilitate the construction of subsequent visual constraints (multi-state constraints) and TDCP constraints. The dimension expansion process for error states and covariance follows the formula below:

[0033]

[0034] As a further technical solution, step 3 includes the construction of visual multi-state constraints (visual measurement residual equations), and the specific steps are as follows:

[0035] Assume a static feature point exist The observation equation for a device that is constantly observed by a camera can be expressed as follows:

[0036]

[0037] in, Represents the normalized projected coordinates of the feature point in the camera frame; 3D coordinates of the feature point in the camera frame. It is given by the following formula:

[0038]

[0039] in, Representing feature points In the three-dimensional coordinates of the e-frame, the least squares-based multi-view triangulation is calculated in the multi-state constrained Kalman filter framework, and then the left null space mapping is used to decouple it from the camera pose in the sliding window. Indicates the camera position in E-series; It is the direction cosine matrix from the e-frame to the camera frame.

[0040] Whenever a feature point is lost or a historical state is removed, the filtered state is updated using all relevant visual feature observations. By linearizing the observation model based on the current state estimation, the residual matrix of the visual measurements can be approximated as:

[0041]

[0042] in, Residual matrix representing visual measurements; Represents the error state vector; Representing feature points Three-dimensional coordinates in the e-frame; Indicates observation noise; Representing feature points Position error status; and These are the Jacobian values ​​of the visual measurement pair for the state and the feature point location, respectively. By summing all the residuals of this feature, we can obtain:

[0043]

[0044] in , , and The two are respectively by , , and Stacked vectors or matrices. Projecting the residual equation onto the left null space of the feature point coordinates to eliminate variables at the feature point positions yields the improved visual measurement residual equation as follows:

[0045]

[0046] In the above formula, It is the left null space of the observed Jacobian of the feature point location; The observation Jacobian represents the state vector projected onto the left null space of the feature point coordinates; Indicates observation noise; Represents the projection onto the left null space of the feature point coordinates with respect to the feature point. The residual vector; Representing feature points The residual vector.

[0047] For TDCP measurement updates, when carrier phase observations from adjacent GNSS epochs are acquired and the corresponding IMU pose states exist, TDCP observations are constructed using the carrier phase observations from the two adjacent epochs for measurement updates. Receiver position. and carrier position The following relationship exists:

[0048]

[0049] in, This represents the lever arm vector between the GNSS antenna and the center of the IMU.

[0050] By combining the carrier phase observations of two adjacent GNSS epochs, the corresponding TDCP observations can be obtained, which can then be used to constrain the IMU pose at the corresponding two times.

[0051] The specific construction of TDCP constraints (TDCP observations) includes the following steps:

[0052] The residual equation for loosely coupled TDCP measurement updates is constructed according to the following formula:

[0053]

[0054] in, This represents the residual of loosely coupled TDCP measurement updates; This represents the receiver position difference between adjacent epochs calculated by GNSS, which is estimated from TDCP observations using the least squares method. This represents the receiver position difference between adjacent epochs during IMU pre-integration; The measurement matrix representing loosely coupled epochal differences; This is the error state vector; , The state of two adjacent epochs; The noise represents the position difference obtained by TDCP least squares. , The measurement matrix represents two adjacent epochs of loose coupling; specifically, it is shown in the following formula:

[0055]

[0056] in, , I represents the observations from the IMU system (b system) to the ECEF system (e system) at two adjacent epochs (times p and q), where I is the identity matrix; Represents the lever arm vector between the GNSS antenna and the center of the IMU;

[0057] satellite The residual equation for the tightly coupled TDCP measurement update is:

[0058]

[0059] in, Indicates satellite The residuals of tightly coupled TDCP measurement updates; Represents the carrier phase observations of the epoch difference; This represents the epoch differential carrier phase estimate obtained using pose and clock bias prediction. Indicates satellite The measurement matrix of the epoch difference; Indicates satellite Noise in TDCP measurement updates; , Indicates satellite The measurement matrix of two adjacent epochs is as follows:

[0060]

[0061] in, , This indicates the receiver positions of two adjacent epochs during IMU pre-integration. , This indicates the geometric distance between two adjacent epoch satellites and receivers.

[0062] By stacking TDCP observations from multiple satellites, the complete residual equation can be obtained as follows:

[0063]

[0064] It is worth noting that the receiver clock bias after time differentiation is not included in the system state of this invention. ; This represents the residual from the complete TDCP measurement update; This represents the measurement matrix used in the epoch difference update of TDCP measurements from multiple stacked satellites. This represents noise in TDCP measurement updates involving multiple stacked satellites. Therefore, during TDCP measurement updates, a temporary dimension expansion of the system state is required to estimate the receiver clock error of the time difference, which is then discarded after the update. This invention proposes a method for detecting standardized residuals by setting a maximum threshold. The decision on whether to enable the TDCP update is based on the following: If the standardized residual of the detection is less than the threshold, the observation is considered valid and the TDCP update is enabled; if it exceeds the threshold, it is judged as an abnormal observation and the update is discarded.

[0065] As a further technical solution, in step 4, the global pose graph uses the global pose of the carrier as nodes, the local pose estimation in step 3 provides relative pose constraints and absolute attitude constraints, and the GNSS positioning result provides absolute position constraints. The state vector of the global pose graph optimization problem is defined as follows:

[0066]

[0067] In the above formula, and Indicates the local horizontal coordinate system (northeast sky) The position and orientation of the carrier under the system are represented by three-dimensional vectors and quaternions, respectively.

[0068] Pose graph optimization can be viewed as a maximum likelihood estimation problem:

[0069]

[0070] in, This represents the optimal solution to the pose graph optimization problem. ; and These are the relative pose residual and absolute attitude residual from T-VIO, and the absolute position residual from GNSS technology, respectively. , and That is the corresponding covariance.

[0071] The relative pose derived from the local odometry can be used to construct the local relative pose constraint factor as follows:

[0072]

[0073] in, This represents the relative pose constraint factor derived from the local odometry. and These are provided by T_VIO Time and The local pose of the carrier in the local reference coordinate system at all times is used to construct the relative constraints between global states; Defined as subtraction between poses; and They are Time and The local pose of the carrier in a geographically defined navigation coordinate system (northeast-sky). The covariance of the local factors is determined by the distance of relative motion; the greater the relative motion distance, the greater the covariance and the smaller the weight.

[0074] The aforementioned factor is a typical relative pose constraint factor derived from a local odometry system. This invention also proposes an absolute factor constructed from the local odometry system. In the method proposed in this invention, the fusion of TDCP and VIO achieves accurate absolute attitude estimation; therefore, the local reference frame of the odometry system... It should be consistent with the geographically defined navigation coordinate system n; based on this, a local reference system can be used. The posture of the lower attitude in the n-system By applying constraints, the absolute attitude factor is constructed as follows:

[0075]

[0076] Its uncertainty is assumed to be constant; Local reference frame The lower position, The pose is in the n-system.

[0077] GNSS positioning technology can provide drift-free absolute position estimates, which can be used to construct absolute constraint factors. However, GNSS positioning results often contain gross errors, especially in complex urban scenarios. To avoid the impact of these gross errors on the fusion results, this invention adopts a strict outlier removal strategy. Outliers are removed using satellite count, PDOP value, ratio test, and other internal consistency test indicators. Only a small portion of reliable GNSS positioning results are retained to construct the absolute position constraint factors, which are expressed as follows:

[0078]

[0079] in, This represents the absolute position constraint factor constructed from the GNSS positioning results; This indicates the position in the n-frame obtained from GNSS measurements. Indicates the position of the carrier in the n-system.

[0080] Given that the global location information provided by GNSS has already been filtered, it is assigned the same weight. Considering the differences between different GNSS positioning technologies, different outlier removal strategies and weights are set for different GNSS technologies. When using SPP technology, a relatively large number of positioning results are retained and given lower weights; when using RTK technology, a relatively small number of positioning results are retained and given higher weights.

[0081] The present invention also discloses a TDCP (Time Differential Carrier Phase) enhanced visual inertial odometry, including an IMU, a camera, a GNSS receiver, and a data acquisition and processing module. The data acquisition and processing module is used to execute the above-mentioned TDCP enhanced visual inertial odometry navigation and positioning method to achieve high-precision, globally unified pose estimation.

[0082] Compared with the prior art, the beneficial effects of the present invention are:

[0083] The TDCP time differential carrier phase-assisted visual inertial odometry and global pose optimization technology solution provided in this invention has at least the following technical effects or advantages:

[0084] This invention achieves tight coupling and fusion of TDCP and visual inertial odometry. By integrating visual, inertial, and TDCP measurements through an extended Kalman filter, and utilizing the absolute scale and heading constraints provided by TDCP, it fundamentally suppresses the error accumulation problem caused by the unobservable global translation and heading in traditional VIO, achieving low-drift local pose estimation without the need for external correction information. Compared to the error accumulation over time caused by the reliance on relative observations in traditional VIO, this scheme directly constrains the scale and heading through centimeter-level relative observations from TDCP, fundamentally improving the observability of the positioning model.

[0085] Compared to traditional loosely coupled GNSS / INS / Vision schemes that overly rely on GNSS calculation results and suffer performance degradation when signal lock is lost, this invention employs a global pose graph optimization framework. It nonlinearly fuses the high-precision relative pose output by T-VIO, the absolute attitude constrained by TDCP, and the sparse GNSS global position, and eliminates outlier observations through a rigorous outlier detection mechanism, achieving globally consistent pose estimation. This mechanism does not require continuous GNSS signal support; optimization can be completed using only intermittent global positions, thus solving the positioning failure problem of traditional schemes in challenging GNSS environments.

[0086] Furthermore, the dual-mode processing mechanism of tightly coupled and loosely coupled TDCP proposed in this invention, combined with iterative residual detection and sliding window state maintenance, effectively solves the problems of cycle slip accumulation and gross error interference when TDCP is used alone. By using VIO continuous pose estimation to assist TDCP error elimination, compared with the traditional TDCP scheme that relies on double-difference ambiguity recovery, this invention does not rely on baseline quality, thus improving the robustness of sensor fusion in complex environments. At the same time, the real-time filtering framework based on EKF ensures the low latency characteristics of localization solution, making it suitable for scenarios with high real-time requirements such as autonomous driving and drones. Attached Figure Description

[0087] Figure 1This is a flowchart of the global pose optimization algorithm of the present invention. Detailed Implementation

[0088] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are merely 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.

[0089] This invention provides a time-differential carrier phase-assisted visual inertial odometry and a global pose optimization method to solve the problems of error accumulation and poor adaptability to complex environments in traditional visual inertial odometry (VIO), and achieves low-drift, high-precision positioning in large-scale scenarios.

[0090] This embodiment provides a TDCP-enhanced visual inertial odometry navigation and positioning method, the overall idea of ​​which is as follows:

[0091] The method proposed in this invention focuses on improving the accuracy of local pose estimation by tightly fusing time-differential carrier phase (TDCP), visual, and inertial measurements, and achieving globally consistent localization through global pose graph optimization. For multi-sensor fusion localization, an extended Kalman filter (EKF) is used to integrate visual features, IMU data, and TDCP observations. The absolute scale and heading constraints provided by TDCP suppress VIO error accumulation. Simultaneously, the local estimation results are fused with the GNSS global position through nonlinear optimization to achieve high-precision pose estimation globally.

[0092] Figure 1 illustrates the algorithmic framework of the method of this invention. First, the data from the binocular camera, IMU, and GNSS are preprocessed. Visual features are extracted using FAST corner detection and KLT optical flow tracking. Relative observations are obtained by differential processing of the carrier phase of consecutive GNSS epochs using TDCP. Then, tight coupling fusion of TDCP, visual, and inertial data is achieved through EKF to obtain local pose estimation; simultaneously, historical poses within a sliding window are maintained to construct visual constraints. Finally, the local relative pose, absolute attitude, and GNSS global position are input into the pose graph optimization framework. Outliers are eliminated through nonlinear optimization, and global pose consistency constraints are achieved.

[0093] The method of the present invention includes the following steps:

[0094] Step 1: Data preparation and preprocessing. Observational data is collected through the IMU, camera and GNSS receiver on the carrier. The internal and external parameters of the above sensors are pre-calibrated, and the IMU and camera sensors are triggered by the second pulse signal of the GNSS receiver to obtain a unified time reference in order to achieve the unification of the time system. Multi-GNSS navigation ephemeris is downloaded from the IGS data center. The images collected by the camera are extracted and tracked by Fast feature extraction and KLT optical flow tracking technology.

[0095] Step 2, System State Initialization and Prediction: First, the initial position of the vehicle is estimated using navigation ephemeris and observations, and the initial velocity and attitude are estimated using IMU output. Then, the system maintains navigation states such as position, velocity, attitude, and IMU zero bias, as well as historical pose states in the sliding window, and propagates the system state through INS mechanical orchestration based on the IMU output.

[0096] Specifically, in step 2, the system state is defined as the IMU navigation state and the IMU pose state at historical moments in the sliding window. Its error state vector is defined as follows:

[0097]

[0098] in, This is the error state vector; It indicates the IMU navigation error status, including position error, velocity error, attitude error, accelerometer bias error, and gyroscope bias error status; It represents the error state of the IMU pose at a historical moment within the sliding window, and is mainly used to construct multi-state constraints for visual observation.

[0099] The specific definition of navigation error state is as follows:

[0100]

[0101] in, , and These represent the position, velocity, and attitude error states of the carrier in the e-frame (earth-centered earth-fixed (ECEF) coordinate system);

[0102] The error states of the IMU pose at N historical moments in the sliding window are defined as follows:

[0103]

[0104] The IMU output propagates the system state through the INS mechanical orchestration. A simplified linearized dynamic model of the INS can be represented as:

[0105]

[0106] In the above formula, , and These are the derivatives of position, velocity, and attitude, respectively. Represents the rotation matrix from the IMU system (b system) to the ECEF system (e system); and These represent specific force and the Earth's rotational angular velocity, respectively. and These are the errors in the acceleration and angular velocity output by the IMU, respectively. Represents the error vector of gravity; symbol This represents the cross product of vectors. For the zero-bias model of an IMU sensor, the zero-bias vector of the accelerometer... and gyroscope zero bias vector The system state is set as the state for estimation, and its changes are modeled as white noise. and Random walk.

[0107] The navigation state is propagated based on IMU observation data, and the covariance of the navigation state is also propagated accordingly according to the propagation model. Furthermore, the historical IMU poses within the sliding window are treated as constant values ​​without propagation noise. The complete system state propagation model is as follows:

[0108]

[0109] in, It is a zero matrix with 15 rows and 6N columns. The meaning of other zero matrices can be deduced similarly. It is an identity matrix with 6 rows and 6 columns; the meanings of other identity matrices can be deduced similarly.

[0110]

[0111] Based on the above propagation model, the covariance propagation of all system states can be calculated using the following formula:

[0112]

[0113] In the above formula, k represents time. Represent the covariance matrix; It is the state transition matrix; Corresponding propagation noise matrix.

[0114] The state in the history pose window when processing new images. The dimension will be expanded by adding a clone of the latest IMU pose to the sliding window to facilitate the construction of subsequent visual constraints (multi-state constraints) and TDCP constraints. The dimension expansion process for error states and covariance follows the formula below:

[0115]

[0116] in, , These represent the position and attitude error state of the carrier at time k in the e-frame within the sliding window, respectively.

[0117] Step 3: Measurement Update, achieving local pose estimation and obtaining the GNSS global position. Visual measurement update: When visual feature points are lost during tracking or the corresponding system state is marginalized, a residual equation is constructed based on the co-view relationship of visual measurements from the historical pose state to update the navigation state. TDCP measurement update: When carrier phase observations of adjacent GNSS epochs are obtained and the corresponding IMU pose state exists, TDCP observations are constructed using the carrier phase observations of two adjacent epochs for measurement update. By combining the carrier phase observations of two adjacent GNSS epochs, the corresponding TDCP observations can be obtained, which are then used to constrain the IMU pose at the corresponding two times.

[0118] Specifically, step 3 includes the construction of visual multi-state constraints (visual measurement residual equations), and the specific steps are as follows:

[0119] Assume a static feature point exist The observation equation for a device that is constantly observed by a camera can be expressed as follows:

[0120]

[0121] in, Indicates the feature point in camera system C k Normalized projected coordinates; feature points Three-dimensional coordinates of the camera system at time k It is given by the following formula:

[0122]

[0123] in, Representing feature points In the three-dimensional coordinates of the e-frame, the least squares-based multi-view triangulation is calculated in the multi-state constrained Kalman filter framework, and then the left null space mapping is used to decouple it from the camera pose in the sliding window. Indicates the camera position in E-series; It is the direction cosine matrix from the e-frame to the camera frame.

[0124] Whenever a feature point is lost or a historical state is removed, the filtered state is updated using all relevant visual feature observations. By linearizing the observation model based on the current state estimation, the residual matrix of the visual measurements can be approximated as:

[0125]

[0126] in, The residual matrix for visual measurements; and These are the Jacobians of the visual measurement to the system state and the feature point position, respectively, where j represents the feature point number and i represents the time. This represents observation noise. By extracting all the residuals that stack this feature, we can obtain:

[0127]

[0128] in , , and For respectively by , , and A vector or matrix formed by stacking; Representing feature points The positional error state is determined. The residual equation is projected onto the left null space of the feature point coordinates to eliminate variables related to the feature point positions, resulting in the improved visual measurement residual equation as follows:

[0129]

[0130] In the above formula, It is the left null space of the observed Jacobian of the feature point location; The observation Jacobian of the state vector projected onto the left null space of the feature point coordinates; This represents the observation noise after projection onto the left null space of the feature point coordinates. Represents the projection onto the left null space of the feature point coordinates with respect to the feature point. The residual vector; Representing feature points The residual vector.

[0131] For TDCP measurement updates, when carrier phase observations from adjacent GNSS epochs are acquired and the corresponding IMU pose states exist, TDCP observations are constructed using the carrier phase observations from the two adjacent epochs for measurement updates. Receiver position. and carrier position The following relationship exists:

[0132]

[0133] in, This represents the lever arm vector between the GNSS antenna and the center of the IMU. This represents the transition from the IMU system (b system) at time k to the ECEF system (e system).

[0134] By combining the carrier phase observations of two adjacent GNSS epochs, the corresponding TDCP observations can be obtained, which can then be used to constrain the IMU pose at the corresponding two times.

[0135] The construction of TDCP constraints includes the following steps:

[0136] The residual equation for loosely coupled TDCP measurement updates is constructed according to the following formula:

[0137]

[0138] in, This represents the residual of loosely coupled TDCP measurement updates; This represents the receiver position difference between adjacent epochs calculated by GNSS, which is estimated from TDCP observations using the least squares method. This represents the receiver position difference between adjacent epochs during IMU pre-integration; The measurement matrix representing loosely coupled epochal differences; This is the error state vector; , The state of two adjacent epochs; This represents the noise in the position difference obtained by TDCP least squares.

[0139]

[0140] in, , I represents the observations from the IMU system (b system) to the ECEF system (e system) at two adjacent epochs (times p and q), where I is the identity matrix; This represents the lever arm vector between the GNSS antenna and the center of the IMU.

[0141] satellite The residual equation for the tightly coupled TDCP measurement update is:

[0142]

[0143] in, Indicates satellite The residuals of tightly coupled TDCP measurement updates; Represents the carrier phase observations of the epoch difference; This represents the epoch differential carrier phase estimate obtained using pose and clock bias prediction. Indicates satellite Noise in TDCP measurement updates.

[0144]

[0145] in, , This indicates the receiver positions of two adjacent epochs during IMU pre-integration. , This indicates the geometric distance between two adjacent epoch satellites and receivers.

[0146] By stacking TDCP observations from multiple satellites, the complete residual equation can be obtained as follows:

[0147]

[0148] It is worth noting that the receiver clock bias after time differentiation is not included in the system state of this invention. ; This represents the measurement matrix used in the epoch difference update of TDCP measurements from multiple stacked satellites. This represents noise in TDCP measurement updates involving tightly coupled multiple satellites. Therefore, during TDCP measurement updates, a temporary dimension expansion of the system state is required to estimate the receiver clock error of the time difference, which is then discarded after the update. This invention proposes a method for detecting standardized residuals by setting a maximum threshold. The decision on whether to enable the TDCP update is based on the following: If the standardized residual of the detection is less than the threshold, the observation is considered valid and the TDCP update is enabled; if it exceeds the threshold, it is judged as an abnormal observation and the update is discarded.

[0149] Step 4: Global pose optimization. Using the low-drift, high-precision local pose estimation results obtained from Step 3, and the global position provided by GNSS technology (SPP or RTK), the pose graph is fused to obtain a high-precision, globally consistent pose estimation.

[0150] Specifically, in step 4, the global pose graph uses the global pose of the carrier as nodes. The local pose estimation in step 3 provides relative pose constraints and absolute attitude constraints, while the GNSS positioning result provides absolute position constraints. The state vector of the global pose graph optimization problem is defined as follows:

[0151]

[0152] In the above formula, and Indicates the local horizontal coordinate system (northeast sky) The position and attitude of the carrier under the system. M is the total number of poses at all times contained in the global pose graph;

[0153] Pose graph optimization can be viewed as a maximum likelihood estimation problem (an expression for the global pose graph optimization problem):

[0154]

[0155] in, This represents the optimal solution to the pose graph optimization problem. , and These are the relative pose residual and absolute attitude residual from T-VIO, and the absolute position residual from GNSS technology, respectively. , and That is the corresponding covariance.

[0156] The relative pose from the local odometry can be used to construct local factors as follows:

[0157]

[0158] in, This represents the relative pose constraint factor derived from the local odometry. and These are provided by T_VIO Time and The local pose of the carrier in the local reference coordinate system at all times is used to construct the relative constraints between global states; Defined as subtraction between poses; and They are Time and The local pose of the carrier in a geographically defined navigation coordinate system (northeast-sky). The covariance of the local factors is determined by the distance of relative motion; the greater the relative motion distance, the greater the covariance and the smaller the weight.

[0159] The aforementioned factor is a typical relative pose constraint factor derived from a local odometry system. This invention also proposes an absolute factor constructed from the local odometry system. In the method proposed in this invention, the fusion of TDCP and VIO achieves accurate absolute attitude estimation; therefore, the local reference frame of the odometry system... It should be consistent with the geographically defined navigation coordinate system n; based on this, a local reference system can be used. Lower position pose in the n-system By applying constraints, the absolute attitude factor is constructed as follows:

[0160]

[0161] This represents the absolute attitude constraint factor constructed by the local odometer, with its uncertainty set to a constant.

[0162] GNSS positioning technology can provide drift-free absolute position estimates, which can be used to construct absolute constraint factors. However, GNSS positioning results often contain gross errors, especially in complex urban scenarios. To avoid the impact of these gross errors on the fusion results, this invention adopts a strict outlier removal strategy. Outliers are removed using satellite count, PDOP value, ratio test, and other internal consistency test indicators. Only a small portion of reliable GNSS positioning results are retained to construct the absolute position constraint factors, which are expressed as follows:

[0163]

[0164] in, This indicates that the GNSS positioning results are used to construct the absolute position constraint factor; This indicates the position in the n-frame obtained from GNSS measurements. Indicates the position of the carrier in the n-system.

[0165] Given that the global location information provided by GNSS has already been filtered, it is assigned the same weight. Considering the differences between different GNSS positioning technologies, different outlier removal strategies and weights are set for different GNSS technologies. When using SPP technology, a relatively large number of positioning results are retained and given lower weights; when using RTK technology, a relatively small number of positioning results are retained and given higher weights.

[0166] This invention also provides a TDCP-enhanced visual inertial odometry, including an IMU, a camera, a GNSS receiver, and a data acquisition and processing module. The data acquisition and processing module is used to execute the above-mentioned TDCP-enhanced visual inertial odometry navigation and positioning method to achieve high-precision, globally unified pose estimation.

[0167] Experimental Example

[0168] To better verify the effectiveness of the invention, the inventors specifically evaluated the performance of the odometer in campus and urban scenarios, and also evaluated its global pose estimation accuracy.

[0169] The campus scene features low-rise buildings and trees along the roadsides, with approximately four sections exhibiting significant occlusion, resulting in an overall semi-occluded scene. Experimental results in the campus scene show that the T-VIO (TC) method of this invention improves absolute positioning error by approximately 79.73% and 84.98% compared to Open VINS (Open Visual-Inertial Navigation System) and VINS Fusion (Visual Inertial Navigation System Fusion), respectively; and by approximately 86.7% and 86.8% in the heading direction, demonstrating low drift and high-precision pose estimation performance. The improvement in RTE is also significant, with high RTE retention accuracy, indicating that the relative pose estimation accuracy is not affected by gross errors. This invention also investigated different global pose estimation methods fused with SPP / RTK positioning results. The T-VIO-GO method of this invention improves position estimation accuracy by over 65% compared to IC-GVINS (INSCentric GNSS Visual Inertial Navigation System) and VINS Fusion, and attitude estimation accuracy by over 70%.

[0170] Urban scenes are more complex than campus scenes, with tall buildings and trees along the roads, and typical urban canyon scenes, resulting in longer experimental durations. Urban scene experimental results: The results show that different global pose estimation methods are fused with SPP / RTK localization results in urban scenes. The method of this invention achieves robust and high-precision pose estimation in complex urban scenes, with localization accuracy improved by more than 80% compared to IC-GVINS and VINS Fusion.

[0171] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A TDCP-enhanced visual inertial odometry navigation and positioning method, characterized in that, include: Step 1: Data preparation and preprocessing, acquiring observation data and navigation ephemeris data through the IMU, camera and GNSS receiver on the carrier; Extraction and tracking of visual feature points in images captured by the camera; Step 2: System state initialization and state prediction. The initial position of the vehicle is estimated by using navigation ephemeris data and observation data, and the initial velocity and attitude of the vehicle are estimated by using IMU output. System maintenance includes position, velocity, attitude, navigation state with IMU zero bias, and historical pose state in the sliding window. The system state is propagated through INS mechanical orchestration based on the IMU output. Step 3: Measurement update. When visual feature points are lost during tracking or the corresponding system state is marginalized, a residual equation is constructed based on the co-view relationship between the historical pose state and the visual measurements to update the navigation state. When the carrier phase observations of adjacent GNSS epochs are obtained and the IMU pose state at the corresponding time exists, the TDCP observations are constructed using the carrier phase observations of the two adjacent epochs for measurement updates. Step 4: Global pose optimization. The local pose estimation results obtained from the measurement update in Step 3 are fused with the GNSS global position through pose graph optimization to obtain a globally consistent pose estimation.

2. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 1, characterized in that, In step 1, the IMU, camera, and GNSS receiver are pre-calibrated for intrinsic and extrinsic parameters. The navigation ephemeris of Multi-GNSS is downloaded from the IGS data center. The second pulse signal of the GNSS receiver is used to trigger the IMU and camera sensors to obtain a unified time reference. The images acquired by the camera are extracted and tracked for visual feature points through Fast feature extraction and KLT optical flow tracking technology.

3. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 1, characterized in that, In step 3, the construction of the visual measurement residual equation includes: Whenever a visual feature point is lost or a historical state is removed, the filtered state is updated using all relevant visual feature observations; by linearizing the observation model through the current state estimation, the residual matrix of the visual measurements is represented as: ； in, Residual matrix representing visual measurements; Represents the error state vector; and These are the Jacobians of the visual measurements relative to the system state and the feature point positions, respectively. Representing feature points Three-dimensional coordinates in the e-frame; Representing observation noise; by taking all the residuals stacked on this feature, we obtain the following equation: ； in , , and For respectively by , , and A vector or matrix formed by stacking; Representing feature points The positional error state is determined by projecting the residual equation onto the left null space of the feature point coordinates to eliminate variables in the feature point position, resulting in the improved visual measurement residual equation as follows: ； In the above formula, It is the left null space of the observed Jacobian of the feature point location; The observation Jacobian of the state vector, projected onto the left null space of the feature point coordinates; This represents the observation noise after projection onto the left null space of the feature point coordinates; Represents the projection onto the left null space of the feature point coordinates with respect to the feature point. The residual vector; Representing feature points The residual vector.

4. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 1, characterized in that, In step 3, the construction of TDCP observations includes: The residual equation for loosely coupled TDCP measurement updates is constructed according to the following formula: ； in, This represents the residual of loosely coupled TDCP measurement updates; This represents the receiver position difference between adjacent epochs calculated by GNSS, which is estimated from TDCP observations using the least squares method. This represents the receiver position difference between adjacent epochs during IMU pre-integration; The measurement matrix representing loosely coupled epochal differences; This is the error state vector; , The state of two adjacent epochs; The noise represents the position difference obtained by TDCP least squares. ； in, , I represents the observations from the b-system to the e-system in two adjacent epochs, where I is the identity matrix; Represents the lever arm vector between the GNSS antenna and the center of the IMU; satellite The residual equation for the tightly coupled TDCP measurement update is: ； in, Indicates satellite The residuals of tightly coupled TDCP measurement updates; Represents the carrier phase observations of the epoch difference; This represents the epoch differential carrier phase estimate obtained using pose and clock bias prediction. Indicates satellite The measurement matrix of the epoch difference; Indicates satellite Noise in TDCP measurement updates; ； in, , This indicates the receiver positions of two adjacent epochs during IMU pre-integration. , This indicates the geometric distance between two adjacent epoch satellites and receivers; By stacking TDCP observations from multiple satellites, the residual equation for the complete TDCP measurement update is obtained as follows: ； in, This represents the residual from the complete TDCP measurement update; This represents the measurement matrix used in the epoch difference update of TDCP measurements from multiple stacked satellites. For receiver clock bias; This indicates noise in the TDCP measurement updates from multiple stacked satellites.

5. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 4, characterized in that, In step 3, the TDCP measurement update also includes temporarily expanding the system state to estimate the receiver clock error of the time difference, and discarding it after the update is completed. The method of setting a maximum threshold to detect standardized residuals determines whether to enable the TDCP update.

6. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 1, characterized in that, In step 4, the global pose of the carrier is used as a node, the local pose estimation provides relative pose constraints and absolute attitude constraints, and the GNSS positioning result provides absolute position constraints, so as to optimize the global pose graph.

7. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 6, characterized in that, In step 4, the expression for the global pose graph optimization problem is: ； in, This represents the optimal solution to the pose graph optimization problem. , and These are the relative pose residual and absolute attitude residual from T-VIO, and the absolute position residual from GNSS technology, respectively. , and That is the corresponding covariance; The relative pose constraint factors from the local odometry are as follows: ； in, This represents the relative pose constraint factor derived from the local odometry. and These are provided by T_VIO Time and The local pose of the carrier at each moment is used to construct the relative constraints between global states; Defined as subtraction between poses; and They are Time and The local position of the carrier in the geographical sense of Northeast China at all times; The absolute attitude constraint factors constructed from the local odometry are as follows: ； This represents the absolute attitude constraint factor constructed by the local odometry, with its uncertainty set to a constant. Local reference frame The lower position, The pose in the n-system; An outlier elimination strategy is adopted, retaining only a small portion of reliable GNSS positioning results to construct an absolute position constraint factor, the expression of which is as follows: ； in, This represents the absolute position constraint factor constructed from the GNSS positioning results; This indicates the position in the n-frame obtained from GNSS measurements. This indicates the position of the carrier in the n-system.

8. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 7, characterized in that, The outlier removal strategy includes using satellite count, PDOP value, ratio test, and other internal consistency test indicators to remove outliers.

9. The TDCP-enhanced visual inertial odometry navigation and positioning method according to claim 7, characterized in that, For global locations provided by GNSS that have already been filtered, assign them the same weight; set different outlier removal strategies and weights for different GNSS technologies.

10. A TDCP-enhanced visual inertial odometry, characterized in that, It includes an IMU, a camera, a GNSS receiver, and a data acquisition and processing module, wherein the data acquisition and processing module is used to execute the TDCP-enhanced visual inertial odometry navigation and positioning method according to any one of claims 1-9.

Images