Vision satellite fusion high-precision positioning method based on tight coupling graph optimization

Abstract

The application provides a visual satellite fusion high-precision positioning method based on tight coupling graph optimization, comprising: collecting visual images, IMU data and RTK-GNSS observation data, generating visual inertial key frames, IMU pre-integration constraints and RTK observation sequences; estimating the time system deviation according to the IMU angular velocity sequence and the RTK position differential sequence, correcting the RTK timestamp and obtaining the time-aligned RTK absolute position observation; solving the Sim(3) similarity transformation by singular value decomposition, aligning the VSLAM local coordinate system to the RTK global coordinate system; taking the key frame pose, velocity, IMU zero offset, three-dimensional position of the map point and the Sim(3) similarity transformation parameter as the state variable, constructing a tight coupling graph optimization model containing the visual re-projection factor, the IMU pre-integration factor and the RTK absolute position factor; correcting the state variable by the nonlinear least square optimization, and dynamically adjusting the RTK absolute position factor according to the RTK-GNSS solution state and the residual consistency.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor fusion positioning technology, and in particular to a high-precision positioning method based on visual satellite fusion optimization using tightly coupled graphs. Background Technology

[0002] With the development of technologies such as mobile robots, autonomous driving, and unmanned inspection, mobile platforms face higher demands for the continuity, accuracy, and global consistency of positioning results during large-scale and long-term operation. Visual inertial odometry (VIO) can fuse camera images and IMU data to output high-frequency, continuous relative pose, making it suitable for short-term positioning in scenarios with insufficient satellite signals. However, it is essentially a relative positioning system, which is susceptible to feature mismatch, IMU bias, and integration errors. Over long periods of operation, it accumulates drift and cannot directly provide stable global absolute coordinates.

[0003] RTK-GNSS can provide centimeter-level absolute position observations in open environments without the long-term drift problem inherent in relative positioning. However, in complex environments such as urban canyons, tree-lined roads, and building obstructions, RTK-GNSS is susceptible to satellite obstruction, multipath reflections, and signal interruptions. This can cause the solution to degrade from a fixed solution to a floating-point solution, a single-point solution, or even an invalid solution, leading to decreased positioning accuracy, position jumps, or positioning interruptions. Consequently, it becomes difficult to support the continuous and reliable operation of a mobile platform independently.

[0004] Existing visual-inertial odometry (VIO) and satellite positioning fusion schemes mostly employ a loosely coupled approach, obtaining positioning results from visual inertial odometry (VIO) and RTK-GNSS separately, and then fusing the two outputs using a filtering algorithm. This type of method typically only corrects the pose at the current moment, making it difficult to directly apply RTK-GNSS absolute position observations to historical keyframes, map points, and coordinate transformation parameters of the VIOS. This results in accumulated drift not being adequately corrected, and global inconsistencies between the trajectory and the map may still exist.

[0005] Therefore, this invention proposes a high-precision positioning method based on visual-satellite fusion optimization using tightly coupled graphs. The information disclosed in the background section is only for enhancing understanding of the background of this disclosure and may therefore contain prior art information that is not common knowledge to those skilled in the art. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings of existing technologies by providing a high-precision positioning method based on tightly coupled graph optimization of visual satellite fusion, thereby solving the technical problems mentioned in the background section.

[0007] To achieve the above objectives, the present invention provides the following technical solution: A high-precision positioning method based on visual-satellite fusion optimization using tightly coupled graphs includes the following steps: S1. Acquire visual images, IMU data, and RTK-GNSS observation data; extract and track features from visual images; pre-integrate IMU data; unify coordinates and identify solution states in RTK-GNSS observation data; and generate visual inertial keyframes, IMU pre-integration constraints, and RTK observation sequences. S2. Construct a motion intensity sequence based on the IMU angular velocity sequence and the RTK position difference sequence. Estimate the time system deviation between VSLAM and RTK-GNSS through cross-correlation calculation, correct the RTK timestamp, and obtain the time-aligned RTK absolute position observations by linear interpolation. S3. Align the RTK absolute position observations with the local position and time of the visual inertial keyframes, and use singular value decomposition to solve the Sim(3) similarity transformation to align the VSLAM local coordinate system to the RTK global coordinate system. S4. Using keyframe pose, velocity, IMU zero bias, map point 3D position and Sim(3) similarity transformation parameters as state variables, construct a tightly coupled graph optimization model containing visual reprojection factor, IMU pre-integration factor and RTK absolute position factor. S5. Establish a nonlinear least squares objective based on the residuals of three types of factors, and jointly correct the state variables through iterative optimization; S6. Monitor the RTK-GNSS solution status and RTK absolute position residual consistency in real time, dynamically add, reduce or remove RTK absolute position factors, and continue to add RTK absolute position factors after RTK signal recovery. S7. Output a drift-free absolute pose sequence and a globally consistent 3D map based on the optimized state variables.

[0008] S1 specifically includes: synchronously acquiring visual images, IMU data, and RTK-GNSS observation data; performing distortion correction, grayscale conversion, feature extraction, feature tracking, and mismatch removal on the visual images to generate visual feature observations; performing zero-bias correction, noise filtering, and pre-integration on the IMU data to generate IMU pre-integration constraints between adjacent visual inertial keyframes; and performing projection coordinate transformation and solution state identification on the RTK-GNSS observation data to form an RTK observation sequence containing timestamps, absolute positions, and solution states.

[0009] S2 specifically includes: constructing a first motion intensity sequence based on IMU angular velocity and a second motion intensity sequence based on RTK position difference; performing standardized cross-correlation operations on the first and second motion intensity sequences, and determining the time offset corresponding to the maximum cross-correlation coefficient as the time system bias; using the time system bias to correct the RTK timestamp, and searching for adjacent RTK observations before and after each visual inertial keyframe timestamp, and obtaining time-aligned RTK absolute position observations through linear interpolation.

[0010] S3 specifically includes: selecting RTK absolute position observations with fixed or floating-point solution states and time alignment completed, and extracting the local positions of the corresponding visual inertial keyframes in the VSLAM local coordinate system; calculating the centroids of the local position sequence and the RTK global position sequence respectively and performing decentralization processing; constructing the covariance matrix based on the decentralization result, solving the rotation matrix, scale factor and translation vector of the Sim(3) similarity transformation through singular value decomposition, and initially aligning the VSLAM local coordinate system to the RTK global coordinate system.

[0011] S4 specifically includes: using the pose, velocity, IMU zero bias, 3D position of map points, and Sim(3) similarity transformation parameters of visual inertial keyframes as state variables; constructing visual reprojection factors based on visual feature observations; constructing IMU pre-integration factors based on IMU pre-integration constraints between adjacent visual inertial keyframes; constructing RTK absolute position factors based on time-aligned RTK absolute position observations; and setting information matrices for RTK absolute position factors according to RTK-GNSS solution states, so that the weight of fixed solutions is greater than the weight of floating-point solutions, and single-point solutions or invalid solutions do not participate in the construction of RTK absolute position factors.

[0012] S5 specifically includes: establishing a nonlinear least squares objective based on the residuals and information matrices of the visual reprojection factor, IMU pre-integration factor, and RTK absolute position factor; using an iterative optimization algorithm to jointly solve the state variables, so that the visual observation error, IMU motion constraint error, and RTK absolute position error converge simultaneously; and using a sliding window mechanism to limit the number of visual inertial keyframes participating in real-time optimization, converting the constraints of historical keyframes that are moved out of the window into prior factors to preserve the influence of historical global constraints on subsequent optimization.

[0013] S6 and S7 specifically include: real-time monitoring of RTK-GNSS solution status and RTK absolute position residual consistency; adding or retaining RTK absolute position factors when RTK-GNSS is a fixed solution or floating-point solution and the residual consistency meets the conditions; reducing or removing RTK absolute position factors when RTK-GNSS is a single-point solution, invalid solution, or residual abnormality, and using robust kernel functions to suppress gross errors; outputting a drift-free absolute pose sequence containing timestamps, global 3D position, attitude information, fusion mode, and positioning reliability based on the optimized state variables, and outputting a 3D map consistent with the RTK global coordinate system.

[0014] The beneficial effects of this invention are as follows: This invention integrates the visual reprojection factor, IMU pre-integration factor, and RTK absolute position factor into a tightly coupled graph optimization model, enabling RTK-GNSS absolute position observations to participate in the joint correction of keyframe pose, map point 3D position, and coordinate transformation parameters. This avoids the problem of traditional loosely coupled methods that only correct the current pose and effectively suppresses the cumulative drift of visual inertial odometry.

[0015] This invention estimates the time system deviation between VSLAM and RTK-GNSS by performing motion intensity cross-correlation calculations on IMU angular velocity sequences and RTK position difference sequences, and obtains time-aligned RTK absolute position observations through linear interpolation, reducing the fusion error caused by inconsistent sampling frequencies and time bases of different sensors. By using singular value decomposition to solve the Sim(3) similarity transformation, the VSLAM local coordinate system is initially aligned to the RTK global coordinate system, enabling visual inertial keyframes, map points, and RTK absolute position observations to participate in optimization under a unified spatial base, thereby improving the global consistency of positioning results and 3D maps.

[0016] This invention dynamically adds, reduces, or removes RTK absolute position factors based on the consistency of RTK-GNSS solution status and RTK absolute position residuals. It also employs a robust kernel function to suppress visual mismatches and RTK multipath gross errors, improving the robustness of localization in complex environments such as urban canyons, tree-lined roads, and building occlusion. By using sliding window optimization and a historical constraint prior retention mechanism, the system limits the scale of real-time optimization variables while preserving the impact of historical global constraints on subsequent localization. This enables the system to continuously output drift-free absolute pose sequences and globally consistent 3D maps, meeting the needs of large-scale operations such as mobile robots, autonomous driving, and precision agriculture. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of the high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to the present invention. Detailed Implementation

[0018] 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.

[0019] In this embodiment, "visual inertial keyframe" refers to an image frame determined by both camera images and IMU data that satisfies the conditions of disparity change or feature tracking quality change; "time-aligned RTK position observation" refers to the absolute RTK position corresponding to a certain visual inertial keyframe timestamp after time deviation correction and linear interpolation; "Sim(3) similarity transformation" refers to a three-dimensional coordinate transformation including scale factor, rotation matrix and translation vector, used to transform the VSLAM local coordinate system to the RTK global coordinate system; "factor graph" refers to an optimization model composed of state variable nodes and constraint factor nodes; "information matrix" refers to the inverse of the observation covariance matrix, used to represent the credible weight of the observation data in the optimization; "sliding window" refers to the set of states that retains the most recent keyframes for joint optimization in real-time optimization.

[0020] In this embodiment, the data sources include: image data output by the camera, angular velocity and acceleration data output by the IMU, timestamps, three-dimensional absolute coordinates and solution status data output by the RTK-GNSS receiver, as well as camera intrinsic parameters, distortion parameters, camera and IMU extrinsic parameters, and boom parameters of the RTK antenna relative to the carrier reference point obtained from calibration before system deployment. All of the above data serve as input data for subsequent time alignment, spatial alignment, factorization, and global optimization.

[0021] Example: Figure 1 As shown, this embodiment provides a high-precision positioning method based on visual satellite fusion optimization using tightly coupled graphs, including the following steps: S1. Acquire visual images, IMU data, and RTK-GNSS observation data; extract and track features from visual images; pre-integrate IMU data; unify coordinates and identify solution states in RTK-GNSS observation data; and generate visual inertial keyframes, IMU pre-integration constraints, and RTK observation sequences. S2. Construct a motion intensity sequence based on the IMU angular velocity sequence and the RTK position difference sequence. Estimate the time system deviation between VSLAM and RTK-GNSS through cross-correlation calculation, correct the RTK timestamp, and obtain the time-aligned RTK absolute position observations by linear interpolation. S3. Align the RTK absolute position observations with the local position and time of the visual inertial keyframes, and use singular value decomposition to solve the Sim(3) similarity transformation to align the VSLAM local coordinate system to the RTK global coordinate system. S4. Using keyframe pose, velocity, IMU zero bias, map point 3D position and Sim(3) similarity transformation parameters as state variables, construct a tightly coupled graph optimization model containing visual reprojection factor, IMU pre-integration factor and RTK absolute position factor. S5. Establish a nonlinear least squares objective based on the residuals of three types of factors, and jointly correct the state variables through iterative optimization; S6. Monitor the RTK-GNSS solution status and RTK absolute position residual consistency in real time, dynamically add, reduce or remove RTK absolute position factors, and continue to add RTK absolute position factors after RTK signal recovery. S7. Output a drift-free absolute pose sequence and a globally consistent 3D map based on the optimized state variables.

[0022] S1 specifically includes the following: Visual images, raw IMU data, and raw RTK-GNSS observation data are acquired synchronously through a mobile platform. The acquired data is then time-stamped, image preprocessed, feature tracked, IMU pre-integrated, and RTK solution state identified to generate visual inertial keyframes and raw RTK observation sequences with a unified data structure.

[0023] Specifically, the mobile platform is equipped with a monocular or binocular camera, a six-axis IMU, and a dual-frequency RTK-GNSS receiver. The camera is used to acquire environmental images, the IMU is used to acquire the carrier's angular velocity and linear acceleration, and the RTK-GNSS receiver is used to output the absolute position in the global coordinate system. Camera image data comes from the camera's imaging module, raw IMU data comes from the inertial measurement unit, and raw RTK observation data comes from the RTK-GNSS receiver. The camera, IMU, and RTK-GNSS receiver each carry their own timestamps, which are respectively designated as the camera timestamp, IMU timestamp, and RTK timestamp.

[0024] The k-th frame image output by the camera is denoted as Its camera timestamp is ,in, The time is indicated, and the upper right corner 'c' indicates the camera data source. The l-th sampled data output by the IMU is denoted as... ,in, This represents the triaxial angular velocity at the l-th IMU sampling time. This represents the triaxial acceleration at the l-th IMU sampling time. This indicates the IMU timestamp, and the "u" in the upper right corner indicates the source of the IMU data.

[0025] The m-th observation data output by the RTK-GNSS receiver is denoted as ,in, This represents the three-dimensional absolute position of the m-th RTK observation in the UTM coordinate system or other Gaussian projected coordinate system. Indicates eastward coordinates. Indicates northward coordinates. Represents elevation coordinates, Indicates the RTK solution status. This represents the RTK timestamp, and the 'r' in the upper right corner indicates the source of the RTK data.

[0026] Image preprocessing includes distortion correction, grayscale conversion, and image pyramid construction. The camera intrinsic parameter matrix required for distortion correction is denoted as K, and the camera distortion parameters are denoted as D; both are derived from the camera calibration before system deployment. For the original image... Distortion removal processing is performed to obtain a distortion-removed image. This is used to reduce the impact of lens distortion on subsequent feature extraction and reprojection calculations. Grayscale processing is used to reduce the number of image channels, facilitating real-time feature point extraction.

[0027] For distortion-reduced images ORB feature extraction is performed to obtain a set of feature points. ,in, This represents the pixel coordinates of the nth feature point in the k-th frame. Subsequently, the KLT sparse optical flow method is used to track feature points between adjacent image frames, and the RANSAC algorithm is used to remove mismatched feature points.

[0028] RANSAC removal is based on estimating the fundamental or essential matrix from the matched feature points, and identifying and deleting matching points that do not satisfy the epipolar constraint as outliers. If the number of valid feature points in the current frame is less than 50, ORB feature extraction is re-executed; if the average disparity between the current frame and the previous keyframe exceeds 20 pixels, or the number of valid feature points is less than 30, the current frame is selected as the visual inertial keyframe.

[0029] IMU preprocessing includes bias correction, noise filtering, and pre-integration. IMU bias includes gyroscope bias. and accelerometer zero bias Both are obtained through static initialization or backend optimization estimation. For two adjacent visual-inertial keyframes i and i+1, the IMU data is pre-integrated between their timestamps to obtain the relative rotation, relative velocity, and relative displacement constraints: ; ; ; in, This represents the IMU pre-integration relative rotation from keyframe i to keyframe i+1. This represents the change in relative velocity before integration. This represents the change in relative displacement before integration. Indicates the time interval between adjacent IMU samples. This represents the exponential mapping from Lie algebra to rotation matrices.

[0030] RTK-GNSS preprocessing includes coordinate unification and solution state identification. The latitude, longitude, and altitude coordinates output by RTK can first be converted to UTM coordinates or Gaussian projection coordinates to obtain... RTK solution status These include fixed solutions, floating-point solutions, single-point solutions, and invalid solutions. Fixed solutions indicate that the integer ambiguity of the carrier phase is fixed and have the highest confidence. Floating-point solutions indicate that the integer ambiguity is not fixed but still have a certain positioning accuracy. Single-point solutions and invalid solutions are not used as strong absolute position constraints in the optimization.

[0031] The data output in this step includes: a set of visual inertial keyframes. Each keyframe contains an image feature observation set, an IMU pre-integration constraint set, and an RTK original observation sequence.

[0032] For example, when the camera frequency is 30Hz, the IMU frequency is 200Hz, and the RTK-GNSS frequency is 10Hz, the system acquires approximately 30 frames of images, 200 sets of IMU data, and 10 sets of RTK data per second. If the average parallax between the 12th frame and the previous key frame is 24 pixels, then the 12th frame is selected as the visual-inertial key frame, and together with the IMU data before and after it, it forms the motion constraints required for subsequent optimization.

[0033] S2 specifically includes the following: The temporal system bias between visual inertial data and RTK-GNSS data is estimated based on the motion change characteristics of the two data. The estimated temporal system bias is then used to correct the RTK timestamps. Finally, time-aligned RTK absolute position observations are assigned to each visual inertial keyframe through linear interpolation.

[0034] The data sources for this step include: the IMU angular velocity sequence output by S1, the RTK absolute position sequence, the visual inertial keyframe timestamps, and the RTK timestamps. Since VSLAM and RTK-GNSS typically use different internal clocks, there may be a fixed deviation between their timestamps. Without correction, this can lead to a mismatch between the visual inertial keyframes and RTK position observations on the time axis, resulting in systematic errors in spatial alignment and factor optimization. Therefore, this step first estimates the temporal systematic bias.

[0035] To avoid directly using different physical quantities for cross-correlation, this embodiment converts IMU angular velocity data and RTK position difference data into motion intensity sequences respectively. The IMU motion intensity sequence is denoted as... It originates from the magnitude of the IMU's three-axis angular velocity or the vehicle's heading angular velocity component: ; in, Indicates IMU in time The intensity of exercise, This represents the magnitude of the three-axis angular velocity vector.

[0036] RTK motion intensity sequence is denoted as The velocity direction change originates from the RTK position differential. First, the velocity is calculated based on adjacent RTK positions: ; in, This represents the differential velocity corresponding to the m-th RTK observation. The heading angle is then calculated based on the eastward and northward components of the velocity. ; in, This represents the heading angle obtained from the RTK position difference. Indicates the eastward velocity component. This represents the northbound velocity component. Then, the RTK heading angle change rate is calculated: ; in, This indicates angle continuity processing, used to avoid the heading angle from... arrive Errors occur during transitions.

[0037] To eliminate differences in dimensions and amplitude, the two sets of motion intensity sequences were standardized: ; ; in, This represents the standardized IMU motion intensity. This represents the standardized RTK motion intensity. and These represent the mean of the corresponding sequences, and These represent the standard deviations of the corresponding sequences.

[0038] Within the preset search range Internal enumeration of candidate time offsets Calculate the standardized cross-correlation coefficient: ; in, Indicates candidate time offset The corresponding cross-correlation coefficient, M, represents the number of sampling points participating in the cross-correlation calculation. This represents the maximum search time range, preferably 1s to 2s. The candidate offset corresponding to the maximum cross-correlation coefficient is the time system bias. ; in, This represents the time system offset between VSLAM and RTK-GNSS. (The following is a separate, unrelated sentence: "Obtained...") Then, the RTK timestamps are corrected: ; in, This represents the corrected RTK timestamp.

[0039] After completing time correction, for each visual-inertial keyframe Search for two adjacent RTK observations before and after the corrected timestamp. The timestamp of the i-th visual-inertial keyframe is denoted as... If there are two adjacent RTK correction timestamps and ,satisfy: ; Then, the RTK absolute position observations aligned with the keyframe time are calculated using linear interpolation: ; ; in, Indicates the linear interpolation weights. This represents the RTK absolute position observation aligned with the time of the i-th keyframe. If there are no valid RTK observations within 0.1s before or after the keyframe timestamp, or if the time interval between adjacent RTK observations exceeds a preset threshold, then no RTK absolute position factor is assigned to that keyframe, and only visual and IMU constraints are retained.

[0040] For example, if the timestamp of the 20th keyframe is 12.035s, and the two time-corrected RTK observation timestamps are 12.000s and 12.100s respectively, then: ; At this point, the RTK position corresponding to the 20th keyframe is obtained by linearly weighting the two RTK position observations with weights of 0.65 and 0.35. This example clarifies the specific calculation method of "linear interpolation," avoiding a mere description of interpolation without implementation details.

[0041] S3 specifically includes the following: Based on the time-aligned visual inertial keyframe position and RTK absolute position, the singular value decomposition method is used to solve the Sim(3) similarity transformation from the VSLAM local coordinate system to the RTK global coordinate system, and the initial spatial values ​​for subsequent tight coupling optimization are obtained.

[0042] The data sources for this step include: time-aligned RTK absolute position observations output from S2. The keyframe local pose output by the VSLAM front-end, the camera and IMU extrinsic parameters obtained from system calibration, and the RTK antenna boom parameters are all included. The position of the i-th keyframe output by the VSLAM front-end in the local coordinate system is denoted as […]. Where, the upper right corner v represents the VSLAM local coordinate system. The global position corresponding to RTK is denoted as... , where the upper right corner g represents the RTK global coordinate system.

[0043] If the phase center of the RTK antenna does not coincide with the reference point of the VSLAM carrier, a correction is made based on the boom vector obtained before system deployment. The boom vector is denoted as... , representing the offset of the RTK antenna phase center relative to the carrier reference point in the carrier coordinate system, where the upper right corner 'b' represents the carrier coordinate system. After lever arm correction, the RTK global position is obtained consistent with the VSLAM reference point. If the RTK antenna is mounted at the carrier reference point, then .

[0044] The first N time-aligned keyframes with RTK solution states of fixed or floating-point are selected for initial alignment, with the preference being... Furthermore, the keyframe trajectory length should be no less than 1m to avoid instability in similarity transformation solutions caused by static states or short-distance motion. Calculate the centroids of the VSLAM local position sequence and the RTK global position sequence: ; ; in, Indicates the local centroid of VSLAM. This represents the global centroid of the RTK.

[0045] Decentralize the two sets of position sequences: ; ; in, This represents the decentralized result of the i-th VSLAM local location. This represents the decentralized result of the i-th RTK global position.

[0046] Calculate the covariance matrix: ; Here, H represents the covariance matrix consisting of two sets of decentralized position sequences. Singular value decomposition is performed on H: ; Where U and V represent the orthogonal matrices obtained from singular value decomposition, Let represent a singular value diagonal matrix. To avoid reflections in the rotation matrix, a correction matrix is ​​constructed: ; Calculate the rotation matrix from the VSLAM local coordinate system to the RTK global coordinate system: ; in, This represents the rotation matrix from the VSLAM local coordinate system to the RTK global coordinate system. Calculate the scale factor: ; Where s represents the scaling factor from the VSLAM local coordinate scale to the RTK global coordinate scale. Calculate the translation vector: ; in, This represents the translation vector from the VSLAM local coordinate system to the RTK global coordinate system. From this, we obtain the Sim(3) similarity transformation: ; in, Represents any local coordinate point in VSLAM. This represents the converted RTK global coordinate point.

[0047] After the initial transformation is completed, the alignment residual for each participating alignment keyframe is calculated: ; If the alignment residual norm of a keyframe is greater than a preset threshold, such as 0.5m, it is identified as a spatial alignment anomaly, removed from the initial alignment sample, and the Sim(3) similarity transformation is recalculated.

[0048] For example, if 30 keyframes are acquired within the first 10 seconds, and the alignment residual of 28 of these keyframes is less than 0.2m, while the residual of the other 2 keyframes is greater than 1m due to RTK multipathing, then these two outliers should be removed and the solution recalculated. s and This avoids erroneous RTK observations from disrupting the initial coordinate system alignment.

[0049] S4 specifically includes the following: Using keyframe pose, velocity, IMU zero bias, 3D position of map points, and Sim(3) transform parameters as state variables to be optimized, a tightly coupled global optimization model is constructed that simultaneously includes visual reprojection factor, IMU pre-integration factor, and RTK absolute position factor.

[0050] The data sources for this step include: visual feature observations output by S1, IMU pre-integration constraints and initial map point values, time-aligned RTK absolute position observations output by S2, Sim(3) initial transformation output by S3, and camera intrinsic and extrinsic parameter matrices obtained from system calibration.

[0051] Construct the set of state variables to be optimized: ; Where X represents the set of all state variables to be optimized; This represents the pose of the i-th keyframe in the VSLAM local coordinate system, including rotation and translation; Indicates the velocity of the i-th keyframe; This indicates that the gyroscope has zero bias corresponding to the i-th keyframe; This indicates that the accelerometer zero bias corresponds to the i-th keyframe; The j-th map point represents the 3D position of the j-th map point in the VSLAM local coordinate system; s, and Let represent the scale factor, rotation matrix, and translation vector of the Sim(3) similarity transformation, respectively.

[0052] The visual reprojection factor originates from image feature point observations. For the j-th map point observed in the i-th keyframe, the visual observation pixel coordinates are: The visual reprojection residual is defined as: ; in, This represents the visual reprojection residual of the j-th map point in the i-th keyframe; K represents the camera projection function; K represents the camera intrinsic parameter matrix. This represents the extrinsic parameter transformation from the VSLAM carrier coordinate system to the camera coordinate system; This indicates that the map points are transformed to the coordinate system of the i-th keyframe. The physical meaning of the visual reprojection residual is: the difference between the actual detected feature point pixel position and the pixel position obtained by projecting from the current pose and map points.

[0053] The IMU pre-integration factor is derived from the IMU pre-integration results between adjacent keyframes. The IMU pre-integration residual can be expressed as: ; in, This represents the IMU pre-integration residual between the i-th keyframe and the (i+1)-th keyframe. This represents the IMU pre-integration residual function. This residual is used to constrain the relative motion between adjacent keyframes, ensuring that the visual estimation results match the short-term motion changes measured by the IMU.

[0054] The RTK absolute position factor originates from the time-aligned RTK absolute position observations output by S2. The translation component of the i-th keyframe in the VSLAM local coordinate system is denoted as... The predicted position after Sim(3) transformation to the RTK global coordinate system is: ; The RTK absolute position residual is defined as: ; Right now: ; in, This represents the RTK absolute position residual of the i-th keyframe. This represents the RTK absolute position observation time-aligned with the i-th keyframe. The physical meaning of this residual is: the difference between the global position obtained by VSLAM pose transformation and the RTK measured absolute position.

[0055] The information matrix of the RTK factor is set according to the RTK solution state. The information matrix of the i-th RTK factor is denoted as... Its definition is: ; in, This represents the weight matrix of the i-th RTK factor. This represents the zero matrix. When... When this happens, the RTK factor is not added to the optimization model. The above settings mean that fixed solutions have higher confidence and therefore correspond to larger weights; floating-point solutions have lower confidence and therefore correspond to smaller weights; single-point solutions and invalid solutions are not used as absolute position constraints in the optimization.

[0056] For example, when a keyframe corresponds to a fixed RTK solution, its eastward and northward standard deviations are set to 0.02m, and its elevation standard deviation is set to 0.05m; when it corresponds to a floating-point RTK solution, its eastward and northward standard deviations are set to 0.10m, and its elevation standard deviation is set to 0.20m. This clarifies the numerical impact of different RTK qualities on optimization, avoiding insufficient disclosure due to simply writing "high weight" and "medium weight".

[0057] S5 specifically includes the following: The tightly coupled factor graph constructed by S4 is transformed into a nonlinear least squares problem with an information matrix and a robust kernel function, and the Levenberg-Marquardt algorithm is used for iterative solution to obtain the keyframe pose, 3D position of map points and Sim(3) transformation parameters after global correction.

[0058] The data sources for this step include: the set of state variables X constructed by S4, visual reprojection residuals, IMU pre-integration residuals, RTK absolute position residuals, and information matrices corresponding to each type of residual. The optimization objective is to adjust the state variables to simultaneously minimize the errors of visual observation, IMU motion constraints, and RTK absolute position constraints within a unified model.

[0059] Establish a unified optimization objective function: ; in, Represents the set of all valid visual observation constraints. Represents the set of all valid IMU pre-integration constraints. Represents the set of all valid RTK absolute position constraints; Represents the visual factor information matrix. Represents the IMU factor information matrix. This represents the information matrix of the i-th RTK factor; , and These represent the robust kernel functions corresponding to the visual, IMU, and RTK residuals, respectively.

[0060] The weighted residual norm is defined as: ; in, Represents any residual vector. This represents the information matrix corresponding to the residual. The formula indicates that: the higher the observation confidence level, the larger the information matrix, and the greater the impact of the residual of the same size on the objective function; conversely, the lower the observation confidence level, the smaller the information matrix, and the weaker its impact on the objective function.

[0061] The Levenberg-Marquardt algorithm is used for iterative solution. In each iteration, the current state variable is linearized to form the incremental equation: ; in, This represents the Jacobian matrix of the residuals with respect to the state variables. Represents the comprehensive information matrix. Indicates the damping coefficient. Represents the identity matrix. Indicates the increment of the state variable. This represents the current residual vector. (The result is obtained.) Then, update the state variables: ; in, This indicates an incremental update operation applicable to pose, rotation, and Euclidean variables. For Euclidean variables such as 3D position, velocity, and bias, This represents ordinary addition; for rotation and pose variables, This represents an incremental update on a Lie group.

[0062] To meet real-time requirements, this step employs a sliding window optimization strategy. The sliding window length is denoted as L, representing the number of most recent keyframes participating in each optimization iteration, preferably between 10 and 30. When a new keyframe is added, if the number of keyframes within the window exceeds L, the oldest keyframe with the weakest common-view constraint is removed from the current window, and its visual constraints, IMU constraints, and RTK constraints are converted into prior factors through edge detection. ; in, This represents the prior residuals resulting from marginalization. This indicates that the state variables are still retained within the window. This represents the prior residual function. In this way, although historical keyframes are no longer repeatedly optimized as independent variables, their constraint information is still retained in subsequent optimizations, thus balancing global consistency and real-time performance.

[0063] For example, when the sliding window length L=20, the system simultaneously optimizes the pose, velocity, IMU bias, and related map points of the most recent 20 keyframes. When the 21st keyframe is added to the window, the system marginalizes the 1st keyframe as a priori factor and continues to optimize the 2nd to 21st keyframes. This avoids the number of optimization variables growing indefinitely while not completely discarding the influence of early RTK absolute position constraints on subsequent trajectories.

[0064] After optimization, the keyframe pose, map point position, and Sim(3) parameters after global correction are obtained. Since the RTK absolute position factor directly constrains the global position of the keyframe and propagates to the historical keyframe and map point through visual co-view relationship and IMU adjacent frame constraint, a single high-quality RTK observation can not only correct the current pose, but also reverse the historical state with which it has co-view or motion constraint relationship, thereby suppressing the long-term cumulative drift of VSLAM.

[0065] S6 specifically includes the following: Real-time monitoring of RTK-GNSS solution status and residual consistency; dynamic addition, reduction or removal of RTK absolute position factors based on RTK signal quality; and the use of Huber robust kernel function to suppress gross errors caused by visual mismatches, RTK multipath and IMU short-term anomalies.

[0066] The data sources for this step include: the solution status output from the RTK-GNSS receiver. RTK absolute position residuals constructed by S4 Visual reprojection residuals IMU pre-integration residuals And the current state estimation result obtained by S5 optimization.

[0067] First, factor selection is performed based on the RTK solution status. When the RTK solution status is a fixed solution, a high-weight RTK absolute position factor is added; when the RTK solution status is a floating-point solution, a medium-weight RTK absolute position factor is added; when the RTK solution status is a single-point solution or an invalid solution, no RTK absolute position factor is added, and the system continues to operate relying on the visual reprojection factor and the IMU pre-integration factor.

[0068] Secondly, a secondary judgment is made based on the consistency of the residuals. The Mahalanobis distance is calculated for the i-th RTK residual: ; in, This represents the consistency test value of the i-th RTK observation. If... If the value exceeds a preset chi-square threshold (e.g., a threshold of 11.34 for a 3D location observation at 99% confidence), then the RTK observation is considered a suspected gross error. For suspected gross errors, the system can reduce their information matrix weight or directly remove the RTK factor. ; in, This represents the weight decay coefficient, with a preferred value range of 0.1 to 0.5. If RTK gross errors occur in multiple consecutive keyframes, RTK factor addition should be paused during this period to prevent incorrect absolute positions caused by multipathing or occlusion from disrupting global optimization.

[0069] The Huber robust kernel function is applied to the visual reprojection factor and the RTK absolute position factor. The residual norm is denoted as r, and the robust kernel threshold is denoted as... The Huber robust kernel function is defined as follows: ; in, This represents the residual cost after processing with the Huber robust kernel. This represents the robust kernel threshold, preferably set to 1.0. The meaning of this formula is: when the residual is small, it is treated as ordinary squared error to maintain high-precision convergence; when the residual is large, the squared growth is converted to linear growth to reduce the damage of gross errors to the optimization results.

[0070] When the RTK signal completely fails, the system no longer adds new RTK absolute position factors, but retains the keyframe poses, map points, and prior constraints already optimized using historical RTK factors. This causes the system to degenerate into a VSLAM mode subject to historical global constraints, rather than completely degenerating into a pure VSLAM mode without global anchors. When the RTK signal recovers to a fixed or floating-point solution, the system immediately re-adds RTK absolute position factors to subsequent keyframes and propagates the new absolute position constraints to the current trajectory and local map through sliding window optimization and prior factors.

[0071] For example, in an urban canyon scenario, when a vehicle enters an area obstructed by tall buildings, the RTK state may change from a fixed solution to an invalid solution. At this point, the system stops adding RTK factors and maintains continuous localization using only vision and IMU. Since the keyframes before entering the obstructed area have already been corrected by global RTK constraints, the trajectory will not immediately drift significantly. When the vehicle leaves the obstructed area, the RTK state returns to a fixed solution, the system re-adds RTK factors, and adds new absolute position constraints to the optimization window, bringing the current pose back to the global coordinate system. This example clarifies the specific handling methods for "degenerate operation" and "restoration correction" in discontinuous RTK scenarios.

[0072] S7 specifically includes the following: The system outputs a globally optimized drift-free absolute pose sequence, a globally consistent 3D map, and real-time positioning data. The output results are then used for mobile robot path planning, autonomous driving trajectory tracking, automatic return to charging station, precision agriculture operations, and large-scale inspection tasks.

[0073] The data sources for this step include: keyframe poses obtained from S5 optimization, 3D positions of map points, and Sim(3) transform parameters, as well as the effective localization status after robust processing in S6. The system outputs real-time poses at a fixed frequency, preferably 30Hz. Each frame output data includes a timestamp, 3D position in the UTM coordinate system, pose quaternion or pose angle, RTK solution status, current fusion mode, and localization reliability.

[0074] The i-th output pose data can be represented as: ; in, This represents the i-th output location result; This indicates that the timestamp will be output. Indicates the three-dimensional position in the UTM coordinate system; Represents attitude quaternions; Indicates attitude angle; Indicates the RTK solution status; This indicates the current fusion mode, which includes the tightly coupled RTK-VSLAM mode and the VSLAM mode constrained by historical global constraints. This indicates the reliability of the location.

[0075] For a map point, if the position of the j-th map point in the VSLAM local coordinate system is... Then its global coordinates are obtained through Sim(3) transformation: ; in, This represents the 3D coordinates of the j-th map point in the UTM coordinate system. Through this transformation, the 3D environment map output by the system is consistent with the real-world coordinate system and can be directly used for path planning and map updating.

[0076] Location credibility The reliability can be determined comprehensively based on the RTK solution status, residual size, and current fusion mode. For example, when the RTK is a fixed solution and the RTK residual consistency check passes, the localization reliability is high; when the RTK is a floating-point solution and the visual reprojection residual is within the normal range, the localization reliability is medium; when the RTK is invalid and the system relies on the VSLAM mode under historical global constraints, the localization reliability gradually decreases as the RTK interruption time increases.

[0077] Under RTK fixed solution conditions, the system output pose accuracy is preferably ±5cm; within 1 minute of RTK signal interruption, relying on optimized historical states, IMU pre-integration, and visual co-view constraints, the positioning error is preferably controlled within 0.5m. It should be noted that this accuracy data comes from system test results or implementation verification results, and is not intended as the sole limitation under different hardware configurations, different scenarios, and different sensor accuracy conditions.

[0078] For example, in a scenario where a mobile robot automatically returns to its charging station, the robot operates in a tightly coupled RTK-VSLAM mode when running in an open outdoor area, outputting UTM coordinates and attitude angles for global path planning. When the robot passes through areas shaded by trees or buildings, RTK becomes temporarily ineffective, and the system switches to VSLAM mode, which is subject to historical global constraints, to continue outputting continuous poses. When the robot re-enters the open area, the fixed RTK solution is restored, the system re-incorporates RTK factors and corrects the pose, enabling the robot to accurately return to the charging station after operating over a large area. This example illustrates how the output data can be directly used by upper-layer applications and demonstrates that this method can maintain positioning continuity and global consistency even when the RTK signal is intermittent.

[0079] All the above formulas are performed using dimensionless numerical calculations; the relevant formulas are based on empirical models that approximate the real situation, obtained through extensive data collection and software simulation fitting. The preset parameters and thresholds involved in the formulas can be conventionally set and adjusted by those skilled in the art according to the physical constraints of the actual application scenario.

[0080] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0081] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0082] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A high-precision positioning method based on visual-satellite fusion optimization using tightly coupled graphs, characterized in that, Includes the following steps: S1. Acquire visual images, IMU data, and RTK-GNSS observation data; extract and track features from visual images; pre-integrate IMU data; unify coordinates and identify solution states in RTK-GNSS observation data; and generate visual inertial keyframes, IMU pre-integration constraints, and RTK observation sequences. S2. Construct a motion intensity sequence based on the IMU angular velocity sequence and the RTK position difference sequence. Estimate the time system deviation between VSLAM and RTK-GNSS through cross-correlation calculation, correct the RTK timestamp, and obtain the time-aligned RTK absolute position observations by linear interpolation. S3. Align the RTK absolute position observations with the local position and time of the visual inertial keyframes, and use singular value decomposition to solve the Sim(3) similarity transformation to align the VSLAM local coordinate system to the RTK global coordinate system. S4. Using keyframe pose, velocity, IMU zero bias, map point 3D position and Sim(3) similarity transformation parameters as state variables, construct a tightly coupled graph optimization model containing visual reprojection factor, IMU pre-integration factor and RTK absolute position factor. S5. Establish a nonlinear least squares objective based on the residuals of three types of factors, and jointly correct the state variables through iterative optimization.

2. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 1, characterized in that, Also includes: S6. Monitor the RTK-GNSS solution status and RTK absolute position residual consistency in real time, dynamically add, reduce or remove RTK absolute position factors, and continue to add RTK absolute position factors after RTK signal recovery.

3. The high-precision positioning method based on visual satellite fusion optimization according to claim 2, characterized in that, Also includes: S7. Output a drift-free absolute pose sequence and a globally consistent 3D map based on the optimized state variables.

4. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 1, characterized in that, S1 specifically includes: Simultaneously acquire visual images, IMU data, and RTK-GNSS observation data; Visual images are subjected to distortion correction, grayscale conversion, feature extraction, feature tracking, and mismatch removal to generate visual feature observations; Zero bias correction, noise filtering, and pre-integration are performed on the IMU data to generate IMU pre-integration constraints between adjacent visual-inertial keyframes; The RTK-GNSS observation data is transformed by projection coordinates and the solution status is identified to form an RTK observation sequence containing timestamps, absolute positions and solution status.

5. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 1, characterized in that, S2 specifically includes: A first motion intensity sequence is constructed based on the IMU angular velocity, and a second motion intensity sequence is constructed based on the RTK position difference. Standardized cross-correlation calculations are performed on the first motion intensity sequence and the second motion intensity sequence, and the time offset corresponding to the maximum cross-correlation coefficient is determined as the time system bias. The RTK timestamps are corrected using time system bias, and adjacent RTK observations are searched before and after each visual inertial keyframe timestamp. Time-aligned RTK absolute position observations are obtained through linear interpolation.

6. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 1, characterized in that, S3 specifically includes: Select RTK absolute position observations with fixed or floating-point solution status and time alignment completed, and extract the local position of the corresponding visual inertial keyframes in the VSLAM local coordinate system; The centroids of the local position sequence and the RTK global position sequence are calculated and then decentered. Based on the decentralized results, a covariance matrix is ​​constructed. The rotation matrix, scale factor and translation vector of the Sim(3) similarity transformation are solved by singular value decomposition. The VSLAM local coordinate system is initially aligned to the RTK global coordinate system.

7. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 1, characterized in that, S4 specifically includes: Visual inertial keyframe pose, velocity, IMU zero bias, 3D position of map points, and Sim(3) similarity transformation parameters are used as state variables. Visual reprojection factors are constructed based on visual feature observations, IMU pre-integration factors are constructed based on IMU pre-integration constraints between adjacent visual inertial keyframes, and RTK absolute position factors are constructed based on time-aligned RTK absolute position observations. The information matrix is ​​set for the RTK absolute position factor according to the RTK-GNSS solution status, so that the weight of the fixed solution is greater than the weight of the floating-point solution, and the single-point solution or invalid solution does not participate in the construction of the RTK absolute position factor.

8. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 1, characterized in that, S5 specifically includes: A nonlinear least squares objective is established based on the residuals and information matrices of the visual reprojection factor, IMU pre-integration factor, and RTK absolute position factor. An iterative optimization algorithm is used to jointly solve the state variables, so that the visual observation error, IMU motion constraint error and RTK absolute position error converge simultaneously. A sliding window mechanism is used to limit the number of visual inertial keyframes participating in real-time optimization, and the constraints of historical keyframes that are removed from the window are converted into prior factors to preserve the influence of historical global constraints on subsequent optimization.

9. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 3, characterized in that, S6 and S7 specifically include: Real-time monitoring of RTK-GNSS solution status and RTK absolute position residual consistency; adding or retaining RTK absolute position factors when RTK-GNSS is a fixed solution or floating-point solution and residual consistency meets the conditions; reducing weight or removing RTK absolute position factors when RTK-GNSS is a single-point solution, invalid solution or residual abnormality; and using robust kernel functions to suppress gross errors.

10. The high-precision positioning method for visual satellite fusion based on tightly coupled graph optimization according to claim 9, characterized in that, Also includes: Based on the optimized state variables, output a drift-free absolute pose sequence containing timestamps, global 3D position, pose information, fusion mode, and localization reliability, and output a 3D map consistent with the RTK global coordinate system.

Images