Multi-source fusion persistent positioning method combined with prior feature map

Abstract

The application discloses a multi-source fusion continuous positioning method combined with prior feature maps, and aims at the problem that existing visual-inertial odometry is prone to cumulative error in GNSS denial environment. The application constructs a prior feature map containing key frame poses, feature point descriptors and 3D coordinates based on an open source algorithm. A trajectory displacement difference alignment strategy is adopted to complete the joint initialization of a visual-inertial system and GNSS, and ESIKF is used to replace EKF to improve fusion accuracy. A single-frame geometric constraint cumulative-batch manifold alignment strategy is adopted to realize robust alignment of a local coordinate system and a map coordinate system. Based on reverse PnP, relative poses are solved by using current 3D point clouds and historical 2D observation constraints, the map matching result is taken as observation and is fused with GNSS or replaces GNSS observation, and ESIKF is used for loose coupling to eliminate cumulative error, so that continuous high-precision positioning in a GNSS failure scene is realized.

Description

Technical Field

[0001] This application relates to the field of fusion positioning, and more particularly to a multi-source fusion continuous positioning method, apparatus and device that combines prior feature maps. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology for intelligent autonomous robots to perform tasks. In recent years, this technology has been widely researched and applied, and has been successfully applied in many fields such as micro-drones, autonomous driving, virtual reality (VR), and augmented reality (AR).

[0003] With the advancements in research and open-source development in recent years, SLAM technology is also evolving towards multi-sensor fusion. Among these, the visual-inertial odometry (VIO) fusion method has become one of the most mainstream positioning solutions due to its advantages of light weight, low cost, and low power consumption. However, pure visual-inertial systems are essentially a dead reckoning method, which can only use surrounding environmental information to calculate relative pose. As the operating time and distance increase, accumulated errors (drift) inevitably occur, leading to a significant decrease in positioning accuracy after long-term operation.

[0004] To suppress accumulated errors, fusing Global Navigation Satellite System (GNSS / RTK) signals with a visual-inertial system has become an effective approach. This method utilizes absolute position information to correct VIO drift. However, such tightly or loosely coupled schemes are highly dependent on the quality of satellite signals. In environments where satellite signals are blocked or GNSS denial is severe due to multipath effects, such as indoors, tunnels, or urban canyons, the system will degenerate back to pure VIO mode, thus facing the problem of accumulated error divergence again, leading to positioning discontinuities or drastic jumps.

[0005] To address the challenge of long-term, high-precision positioning in environments without satellite signals, utilizing pre-constructed high-precision environmental maps (prior maps) for assisted positioning has become an important trend. However, existing map-based positioning methods still face numerous challenges in practical applications: for example, how to efficiently construct lightweight feature maps containing rich information; how to quickly and robustly initialize and align the local coordinate system of the current visual-inertial system with the global coordinate system of the prior map during system startup or relocalization to avoid optimization getting trapped in local minima; and how to smoothly integrate the absolute pose obtained from map matching into the state estimator (such as ESIKF) as an observation constraint to eliminate drift without causing problems. Summary of the Invention

[0006] The main objective of this application is to provide a multi-source fusion continuous localization method, apparatus, and device that combines prior feature maps. It aims to achieve continuous localization without cumulative error in GNSS unstable or failed scenarios by using a visual-inertial algorithm through sequential map construction, robust initial alignment based on gravity manifold, and a tightly coupled fusion strategy based on reverse PnP.

[0007] To achieve the above objectives, this application provides a multi-source fusion persistent localization method combining prior feature maps, comprising:

[0008] The target area is scanned based on a global localization algorithm, and a serialized prior feature map containing key frame basic information and feature point information is constructed.

[0009] The visual-inertial system positioning trajectory and the GNSS system positioning trajectory are jointly initialized by a displacement difference alignment strategy to obtain the first transformation matrix. The state is estimated by the VIO algorithm, and the local pose and GNSS observation are fused based on the ESIKF algorithm to obtain the pose state of the visual-inertial odometry positioning result in the global coordinate system ENU after correction. The VIO algorithm is a nonlinear optimization algorithm.

[0010] A single-frame geometric constraint accumulation-batch processing manifold alignment strategy and VIO algorithm are adopted to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system. When a co-view relationship is detected between the current frame and the map keyframe, constraints are established by using the current local 3D spatial point cloud in the feature point information of the paired co-view frames and the historical 2D observations of the map. The current pose state of the current frame relative to the map reference camera coordinate system is solved by the inverse PnP algorithm.

[0011] Based on the current pose state, the corresponding latitude and longitude at the current moment are calculated. The latitude and longitude are then filtered and fused with the GNSS system positioning trajectory at the current moment to obtain a new GNSS system positioning trajectory. Alternatively, when the GNSS system fails, it can be used as a virtual GNSS observation. The ESIKF algorithm combined with the VIO algorithm is used to fuse the positioning results to achieve continuous positioning.

[0012] Optionally, the displacement difference alignment strategy includes calculating the displacement difference between adjacent time points based on a given time-aligned sequence of trajectory point pairs to construct an observation vector, thereby obtaining the displacement difference between the aligned visual-inertial system positioning trajectory and the GNSS system positioning trajectory at their respective adjacent time points.

[0013] The joint initialization of the visual-inertial system positioning trajectory and the GNSS system positioning trajectory through the displacement difference alignment strategy, to obtain the first transformation matrix, includes:

[0014] The rigid body rotation constraint expression is determined based on the displacement difference between adjacent time points.

[0015] Constructing a nonlinear least squares problem based on rigid body rotation constraint expressions;

[0016] The Levenberg-Marquardt method is used to solve the least squares problem, obtain the optimal rotation matrix, and recover the translation vector using the latest reliable GNSS observations;

[0017] The first transformation matrix from the local coordinate system of the vision-inertial system to the global coordinate system ENU is obtained based on the recovered translation vector.

[0018] Optionally, the step of employing a single-frame geometric constraint accumulation-batch processing manifold alignment strategy and the VIO algorithm to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system includes:

[0019] A single-frame geometric constraint accumulation-batch processing manifold alignment strategy is adopted to obtain the relative pose estimate of the current camera in the map reference camera coordinate system;

[0020] Obtain the first three-dimensional spatial position, where the first three-dimensional spatial position is the three-dimensional spatial position in the pose state after correction under the global coordinate system ENU;

[0021] The second three-dimensional spatial position is determined based on the relative pose estimation of the current camera in the map reference camera coordinate system, where the second three-dimensional spatial position is the position of the current frame in the map coordinate system;

[0022] The first three-dimensional spatial position and the second three-dimensional position are processed by a displacement difference alignment strategy to obtain the second transformation matrix;

[0023] The state transition is performed using a second transformation matrix to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system.

[0024] Optionally, the step of employing a single-frame geometric constraint accumulation-batch processing manifold alignment strategy to obtain the relative pose estimate of the current camera in the map reference camera coordinate system includes:

[0025] Based on current GNSS observations, spatially adjacent keyframes are selected as candidate keyframes from the prior feature map.

[0026] Construct the bag-of-words vector of the current frame, and obtain the optimal candidate frame by performing similarity matching with the bag-of-words vectors of candidate keyframes. The optimal candidate frame and the current frame are co-viewed frame pairs.

[0027] Based on Euclidean distance brute-force matching of co-view frame pairs, and using the 2D-2D epipolar geometry method, the rotation matrix and normalized translation vector of the camera coordinate system relative to the map reference camera coordinate system are obtained.

[0028] The metric scale is recovered by using the geographic distance between the current frame GNSS observation value and the map keyframe GNSS record value, thus obtaining the geographic distance between the current frame GNSS observation value and the map keyframe GNSS record value;

[0029] By collecting trajectory point pairs from the initial sampling frame images, the displacement difference alignment strategy is used to calculate the transformation matrix from the prior map coordinate system to the global coordinate system ENU, thereby obtaining the relative pose estimate of the current camera in the map reference camera coordinate system.

[0030] Optionally, the step of solving the current pose state of the current frame relative to the map reference camera coordinate system using the inverse PnP algorithm includes:

[0031] Define the set of 3D feature points in the current frame in the current camera coordinate system, and construct matching point pairs using the local 3D feature point set of the current frame and the normalized 2D feature point set stored in the map keyframe;

[0032] Based on the pinhole camera model, a reprojection error function for matching point pairs is defined, and the Huber robust kernel function is used in the reprojection error function to suppress the influence of outliers.

[0033] The optimization problem of the reprojection error function is solved using the RANSAC framework to obtain the rotation matrix and translation vector of the current frame relative to the map keyframe, which represents the current pose state of the current frame relative to the map reference camera coordinate system.

[0034] Optionally, the step of calculating the latitude and longitude of the current moment based on the current pose state includes:

[0035] Obtain the global pose and extrinsic parameters of the map keyframe image in the global coordinate system ENU;

[0036] Based on the global pose and extrinsic parameters, as well as the rotation matrix and translation vector, the global absolute position of the current machine in the global coordinate system ENU is obtained through coordinate system chain transformation. Using the GNSS coordinates of the map keyframe image as anchor points, and combined with the local Cartesian coordinate increments calculated based on the global absolute position, the latitude and longitude of the current moment are calculated inversely.

[0037] Optionally, the keyframe basic information includes a timestamp, keyframe index, keyframe pose in the global coordinate system, and corresponding latitude and longitude information, and the feature point information includes feature point index, descriptor, normalized planar coordinates, pixel coordinates, and spatial 3D coordinates of the feature point in the map coordinate system.

[0038] Furthermore, to achieve the above objectives, this application also provides a multi-source fusion persistent localization apparatus that combines prior feature maps, including:

[0039] The prior map construction module is used to scan the target area based on the global positioning algorithm and construct a serialized prior feature map containing key frame basic information and feature point information.

[0040] The pose correction module is used to jointly initialize the visual-inertial system positioning trajectory and the GNSS system positioning trajectory through a displacement difference alignment strategy to obtain the first transformation matrix, and to perform state estimation through the VIO algorithm. Based on the ESIKF algorithm, the local pose and GNSS observation are fused to obtain the pose state of the visual-inertial odometry (VIO) positioning result in the global coordinate system ENU after correction. The VIO algorithm is a nonlinear optimization algorithm.

[0041] The pose solving module is used to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system using a single-frame geometric constraint accumulation-batch processing manifold alignment strategy and VIO algorithm. When a co-view relationship is detected between the current frame and the map keyframe, constraints are established using the current local 3D spatial point cloud in the feature point information of the paired co-view frames and the historical 2D observations of the map. The current pose state of the current frame relative to the map reference camera coordinate system is solved by the inverse PnP algorithm.

[0042] The fusion positioning module is used to calculate the latitude and longitude of the current time based on the current pose state, filter and fuse the latitude and longitude with the GNSS system positioning trajectory at the current time to obtain a new GNSS system positioning trajectory, or when the GNSS system fails, it is used as a virtual GNSS observation. The ESIKF algorithm is combined with the VIO algorithm to fuse the positioning results to achieve continuous positioning.

[0043] To achieve the above objectives, this application also provides an electronic device, the electronic device comprising: at least one processor, a memory, and an input / output unit; wherein the memory is used to store a computer program, and the processor is used to call the computer program stored in the memory to execute the multi-source fusion continuous localization method combining prior feature maps provided in any of the foregoing embodiments.

[0044] This application proposes a multi-source fusion continuous localization method, apparatus, and device that combines prior feature maps. The method includes scanning a target area based on a global localization algorithm to construct a sequential prior feature map containing keyframe basic information and feature point information; jointly initializing the visual-inertial system (VIS) positioning trajectory and the GNSS system positioning trajectory through a displacement difference alignment strategy to obtain a first transformation matrix; performing state estimation through the VIO algorithm; and fusing local pose with GNSS observations based on the ESIKF algorithm to obtain the pose state of the VIO positioning result corrected in the global coordinate system ENU. The VIO algorithm is a nonlinear optimization algorithm, and the fusion of local pose and GNSS observations is the ESIKF method. A single-frame geometric constraint accumulation-batch processing manifold alignment strategy is employed. This paper proposes a method combining the VIO algorithm and the prior feature map. It transforms the keyframe information and feature point information of the prior feature map into the global coordinate system. When a co-view relationship is detected between the current frame and a map keyframe, constraints are established using the current local 3D spatial point cloud from the feature point information of the paired co-view frames and historical 2D map observations. The inverse PnP algorithm is then used to solve for the current pose state of the current frame relative to the map reference camera coordinate system. Based on the current pose state, the corresponding latitude and longitude are calculated. These latitude and longitude are then filtered and fused with the current GNSS system positioning trajectory to obtain a new GNSS system positioning trajectory. Alternatively, if GNSS fails, the new trajectory can be used to replace the GNSS system positioning trajectory and construct a fused observation value with the visual-inertial system positioning trajectory. This fused observation value is then fused with the ESIKF algorithm and the VIO positioning results to achieve continuous positioning. This application eliminates the uncertainty of the initial translation during the joint initialization of the visual-inertial system and GNSS by using an improved trajectory displacement difference alignment strategy. Furthermore, this application utilizes a "single-frame geometric constraint accumulation-batch processing manifold alignment" strategy to achieve fast and robust alignment between the current local coordinate system and the prior map global coordinate system. By employing relative pose calculation based on inverse PnP, strong constraints are established using the current local 3D point cloud and historical 2D map observations. Error State Iterative Kalman Filtering (ESIKF) is used instead of traditional EKF for loosely coupled fusion, significantly improving the accuracy and convergence speed of state estimation. In situations where GNSS signals are unstable or malfunctioning (e.g., indoors, in tunnels, or urban canyons), this application can effectively eliminate the accumulated error of visual inertial odometry using the absolute position information provided by prior feature maps, preventing positioning drift and divergence, and achieving continuous high-precision positioning over long periods in complex environments without accumulated errors. Attached Figure Description

[0045] Figure 1 This is a flowchart illustrating an embodiment of the multi-source fusion continuous localization method combining prior feature maps provided in this application;

[0046] Figure 2This is a schematic diagram illustrating the principle of an embodiment of the multi-source fusion continuous localization method combining prior feature maps in this application;

[0047] Figure 3 This is a schematic diagram illustrating the principle of the improved visual-inertial / GNSS global positioning algorithm provided in an embodiment of the multi-source fusion continuous localization method combining prior feature maps in this application.

[0048] Figure 4 This is a diagram showing the relationship between the ECEF coordinate system, ENU coordinate system, and Local coordinate system provided in an embodiment of the multi-source fusion continuous localization method combining prior feature maps in this application.

[0049] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0050] It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to limit this application.

[0051] To overcome the shortcomings of existing technologies, this invention proposes a multi-source fusion continuous localization method that combines prior feature maps.

[0052] The main problem addressed by this invention is that existing visual inertial odometry (VIO) systems inevitably accumulate errors over long periods of operation, especially in environments where GNSS signals are rejected or unstable, such as indoors, tunnels, or urban canyons. These systems cannot correct drift using satellite signals, leading to a significant decrease or even divergence in positioning accuracy. Furthermore, existing map-based assisted positioning methods face challenges in practical applications, including difficulties in initializing and aligning the local coordinate system with the global map coordinate system, susceptibility to local minima during optimization, and the difficulty in smoothly integrating map matching results into the state estimator to eliminate drift without causing system oscillations. Therefore, existing technologies cannot achieve continuous, stable, and error-free high-precision positioning in GNSS failure scenarios.

[0053] This invention relates to a multi-source fusion continuous localization method combining prior feature maps, mainly comprising three parts: sequential construction of prior feature maps based on open-source algorithms, initialization of the joint prior feature map and visual-inertial / GNSS algorithm, and fusion localization based on ESIKF. First, this application utilizes an open-source visual-inertial / GNSS global localization algorithm to construct a global prior feature map containing information such as timestamps, keyframe indices, keyframe global poses and latitude / longitude, feature point descriptors, normalized coordinates, pixel coordinates, and spatial 3D coordinates. For the constructed map, during system operation, co-view frames of the feature map are found. The local coordinate system of the visual-inertial / GNSS system is aligned with the feature map coordinate system using the least squares method, thereby achieving mutual transformation between coordinate systems. The keyframes and spatial 3D feature points in the map coordinate system are then transformed to the current system coordinate system to complete initialization. Subsequently, the current frame image is acquired based on the visual sensor. The optimal co-view frame is selected from the prior map using the DBow library. The normalized coordinates of the feature points in the co-view frame are combined with the 3D spatial coordinates of the current frame. The relative pose of the current frame with respect to the map co-view frame is calculated using the inverse PnP algorithm. Then, the global pose of the current frame based on the prior feature map is recovered through coordinate system chain transformation. Finally, this matching result is used as the observation value and fused with GNSS or replaced by ESIKF and fused with the visual-inertial system. In the event that the GNSS signal is unstable or fails, the prior feature map is combined to eliminate accumulated errors and maintain continuous and stable high-precision positioning.

[0054] refer to Figure 4 Before introducing the technical solution of this application, it is necessary to introduce the coordinate system involved in the invention:

[0055] Local coordinate system The position is independently generated by the Visual-Inertial Odometry (VIO) algorithm, and its origin is usually set to the vehicle's position at the time the algorithm starts. The VIO algorithm processes camera and IMU data to output the vehicle's 6-DOF pose (3D position) in this coordinate system. and attitude matrix (and the coordinates of the road signs.)

[0056] ECEF coordinate system The ECEF coordinate system is established with the Earth's center of mass as its origin. This coordinate system is a Cartesian coordinate system fixed relative to the Earth. The extension of the axis passes through the intersection of the prime meridian and the equator. The extension of the axis passes through the North Pole. The axis is determined by the right-hand rule and passes through the equator and the 90-degree meridian.

[0057] ENU coordinate system This is a localized global coordinate system. Its origin is defined as the location corresponding to the first reliable GNSS observation. In this coordinate system, the X-axis points east, the Y-axis points north, and the Z-axis points upward. This coordinate system "localizes" global coordinates, making it easier to intuitively understand position and movement within a local space.

[0058] IMU coordinate system This coordinate system is fixed to the IMU and follows the front-left-top convention. Specifically, the X-axis points in the direction of IMU movement (front), the Y-axis points to the left of the IMU (left), and the Z-axis is vertically upward (top). This coordinate system is the native measurement coordinate system of the IMU sensor, and all inertial measurements (angular velocity, acceleration) are expressed in this coordinate system. In this application, the IMU coordinate system is considered to be consistent with the body coordinate system.

[0059] Camera coordinate system With the camera's optical center as the origin, following computer vision conventions: the Z-axis points towards the scene (front) along the optical axis, the X-axis is to the right, and the Y-axis is downward. For a binocular system, the coordinate system of the left camera is used as the reference.

[0060] Reference Figure 1 and Figure 3 The first embodiment of this application provides a multi-source fusion persistent localization method combining prior feature maps. This method can be executed by a processor, which can be a host or a server. The execution process of the multi-source fusion persistent localization method combining prior feature maps may include the following:

[0061] S101. Scan the target area based on the global localization algorithm to construct a serialized prior feature map containing key frame basic information and feature point information.

[0062] In one embodiment of this application, the basic information of the keyframe includes a timestamp, a keyframe index, the pose of the keyframe in the global coordinate system, and the corresponding latitude and longitude information. The feature point information includes a feature point index, a descriptor, normalized planar coordinates, pixel coordinates, and the spatial 3D coordinates of the feature point in the map coordinate system.

[0063] Among them, reference Figure 2The target area is scanned and mapped using open-source visual-inertial / GNSS global positioning algorithms (such as GVINS or algorithms based on loose coupling of visual-inertial and RTK). Specifically, the global positioning accuracy of the relevant algorithms directly affects the accuracy of the constructed prior map. The constructed prior feature map contains serialized keyframe information. The specific data structure includes: basic keyframe information: timestamp, keyframe index, pose of the keyframe in the global coordinate system (such as ENU Northeast-Sky coordinate system, the specific coordinate system depends on the open-source global positioning algorithm), and corresponding latitude and longitude information. Feature point information includes the index of the feature point in the keyframe, feature descriptor, coordinates of the feature point on the normalized plane, pixel coordinates, and spatial 3D coordinates of the feature point in the map coordinate system. In this embodiment, the keyframe information estimated by the algorithm is converted into binary information and serialized and stored using the boost library, which reduces storage space and facilitates the reuse of the prior feature map. It should be noted that the reliability of the established prior map is related to the accuracy of the open-source global positioning algorithm.

[0064] Clearly, by constructing a serialized prior feature map containing rich feature information, a reliable global position reference data foundation is provided for subsequent operations in GNSS failure environments, effectively avoiding the cumulative error divergence problem caused by the lack of global constraints in traditional visual inertial odometry.

[0065] S102. The visual-inertial system positioning trajectory and the GNSS system positioning trajectory are jointly initialized through the displacement difference alignment strategy to obtain the first transformation matrix. The state is estimated through the VIO algorithm, and the local pose and GNSS observation are fused based on the ESIKF algorithm to obtain the pose state of the visual-inertial odometry positioning result corrected in the global coordinate system ENU. The VIO algorithm is a nonlinear optimization algorithm.

[0066] The displacement difference alignment strategy involves calculating the displacement difference between adjacent time points based on a sequence of trajectory point pairs aligned to a given time, constructing an observation vector, and obtaining the displacement difference between the aligned visual-inertial system positioning trajectory and the GNSS system positioning trajectory at their respective adjacent time points.

[0067] The process of jointly initializing the visual-inertial system positioning trajectory and the GNSS system positioning trajectory using a displacement difference alignment strategy to obtain the first transformation matrix can include the following:

[0068] S1021. Determine the rigid body rotation constraint expression for relative displacement based on the displacement difference between adjacent times.

[0069] S1022. Construct a nonlinear least squares problem based on the rigid body rotation constraint expression.

[0070] S1023. The Levenburg-Marquardt method is used to solve the least squares problem, obtain the optimal rotation matrix, and recover the translation vector using the latest reliable GNSS observations.

[0071] S1024. Based on the recovered translation vector, obtain the first transformation matrix from the local coordinate system of the visual-inertial system to the global coordinate system ENU.

[0072] In the joint initialization process of the visual-inertial algorithm and the GNSS signal, the displacement difference between the visual-inertial system positioning trajectory and the GNSS positioning trajectory at their respective adjacent moments is aligned instead of directly aligning the two trajectories, thereby eliminating the impact of inaccurate initial translation estimation.

[0073] Specifically, for the selected GNSS observations, a nonlinear optimization method is used to solve for the rotation matrix R from the local coordinate system where the VIO algorithm is located to the ENU coordinate system required to describe the fusion algorithm, and on this basis, the translation vector t is calculated to obtain the representation of the VIO positioning results in the ENU coordinate system.

[0074] For example, given a series of time-aligned trajectory point pairs, where is the position of time VIO in the local coordinate system, is the position of the corresponding detected GNSS observation in the ENU coordinate system, and is the length of the aligned point pair sequence, the observation vector is constructed by calculating the displacement difference between adjacent time points, instead of directly using only two trajectories to establish residuals, in order to eliminate the influence of translation.

[0075] Theoretically, the relative displacements in the two coordinate systems satisfy rigid body rotation constraints. To obtain the optimal rotation matrix, a nonlinear least squares problem is constructed and solved using the Levenberg-Marquardt method. It should be noted that after defining the residual function and minimizing the objective function to estimate the optimal rotation matrix, the translation vector is recovered using the latest reliable GNSS observations after obtaining the rotation matrix, thus ensuring the alignment of the initial states.

[0076] It is understandable that after completing the joint initialization, the current pose state vector of VIO in the global coordinate system is derived using the obtained transformation matrix.

[0077] For example, a nonlinear optimization method is used to solve the problem from the local coordinate system. To the ENU coordinate system required to describe the fusion algorithm The rotation matrix R is calculated, and the translation vector t is calculated based on it to obtain the positioning result of VIO in the ENU coordinate system. The following is a representation. Its specific implementation is as follows:

[0078] To decouple the estimation of rotation and translation, the processor first converts the VIO and GNSS trajectory data into relative displacement vectors. This is given by a series of time-aligned trajectory point pairs. ,in for The position of VIO in the local coordinate system at any given time. This represents the position of the corresponding detected GNSS observation in the ENU coordinate system. The length of the aligned point pair sequence. This application constructs the observation vector by calculating the displacement difference between adjacent time points, instead of directly using only two trajectories to establish the residual, in order to eliminate the influence of translation:

[0079] (0.1)

[0080] Theoretically, the relative displacement between the two coordinate systems satisfies rigid body rotation constraints:

[0081] (0.2)

[0082] in To observe noise, and in order to solve for the optimal rotation matrix This application constructs a nonlinear least squares problem and solves it using the Levenberg-Marquardt method. Specifically, the residual function defined in this application is as follows:

[0083] (0.3)

[0084] in, This represents the rotation operation corresponding to the quaternion.

[0085] The objective function is:

[0086] (0.4)

[0087] The processor can estimate the optimal rotation matrix by minimizing the objective function. This application utilizes the latest reliable GNSS observations after obtaining the rotation matrix to recover the translation vector. This ensures alignment of the initial state:

[0088] (0.5)

[0089] Through the above process, this application obtains the result from the local coordinate system. To ENU coordinate system Transformation matrix :

[0090] (0.6)

[0091] The joint initialization of the visual-inertial / GNSS fusion algorithm was successfully completed.

[0092] This application uses the Error State Iterative Kalman Filter (ESIKF) algorithm instead of the Extended Kalman Filter (EKF) algorithm in the prior art to obtain more accurate and reliable fused localization results. After completing joint initialization, the processor uses the obtained transformation matrix... Derive the current pose state vector of VIO in the global coordinate system:

[0093] (0.7)

[0094] Combining the GNSS measurements at this time, the error state vector is obtained by subtraction:

[0095] (0.8)

[0096] in, , , , , , These are the three-dimensional spatial position, three-axis velocity, carrier attitude, acceleration bias, gyroscope bias, and gravity vector, respectively. This represents the error between the two observations. This represents the ENU coordinate system.

[0097] This application assumes that the nominal state of ESIKF, i.e., the truth value, is:

[0098] (0.9)

[0099] The processor can obtain the relationship between the true state and the error state through the existing state transition relationships as follows:

[0100] (0.10)

[0101] The processor is based on real-time IMU observations. And consider Gaussian white noise This allows us to obtain the relationship between the derivative of the nominal state vector and the sensor observations. Among these, and These are the measured acceleration and angular velocity, respectively. The conversion relationship is shown below:

[0102] (0.11)

[0103] in, Let be the derivative representing a vector, and be... This represents the antisymmetric matrix of its elements.

[0104] Regarding the rotation part in the error term This application can be represented in the form of Lie algebras. .

[0105] In this way, the processor can derive the dynamic model of the error state vector:

[0106] (0.12)

[0107] in The error vector, represented by the error state vector, can be obtained by discretizing the above equation. The linearized equations of motion are expressed as follows:

[0108] (0.13)

[0109] in, The linearized Jacobian matrix is... The process noise has a mean of zero and a mean squared error matrix of... It follows a normal distribution. ,in , , , Gaussian white noise representing velocity, attitude, acceleration bias, and angular velocity bias, respectively. , This is how you can get The expression is as follows:

[0110] (0.14)

[0111] Based on the above steps, this application can perform the error Kalman filtering prediction process, which uses the IMU integral value between consecutive observations to predict the error state:

[0112] (0.15)

[0113] in, This indicates the prior value of the parameter; otherwise, it indicates the posterior value. This is the prior covariance matrix predicted at the current time. This represents the error state covariance matrix of the previous time step.

[0114] When new reliable GNSS observations become available, the processor can update the state. The observation model is represented as follows:

[0115] (0.16)

[0116] in, This represents GNSS-based observations. For nonlinear observation functions, Gaussian white noise was introduced for the observation.

[0117] Measurement error can be expressed as:

[0118] (0.17)

[0119] in, It is about the error state vector of Jacobian matrix .

[0120] Unlike the standard ESKF which performs only one linearization update, the processor can optimize the error state vector through multiple iterations. To find the maximum posterior probability. This application defines... For the first The error state estimate of the next iteration, let the initial... Then in the first The iterative update steps at each moment are as follows:

[0121] Calculate the observation residuals in the current iteration state. :

[0122] (0.18)

[0123] Kalman gain during the construction of the iteration process:

[0124] (0.19)

[0125] in, For Kalman gain, Let be the covariance matrix of the observation noise.

[0126] To reduce the error caused by nonlinearity, the processor uses an iterative algorithm to update the error state for the next iteration. :

[0127] (0.20)

[0128] like ,in, This is the convergence threshold;

[0129] Alternatively, iteration can stop after reaching the required number of iterations, at which point the processor acquires... The optimal posterior estimate at time 1 Take the gain from the last iteration. To update the posterior covariance matrix:

[0130] (0.21)

[0131] Finally, the posterior error state vector is used to correct the nominal state vector:

[0132] (0.22)

[0133] By introducing this correction step, the VIO algorithm and ESIKF achieve effective closed-loop control of the error vector, accurately eliminating the accumulated error caused by IMU drift. Furthermore, real-time calibration of the IMU sensor bias optimizes the long-term stability of its measurement output, which improves the reliability of the front-end sensing module and ultimately ensures the overall accuracy and robustness of the global state estimation.

[0134] Clearly, by adopting a displacement difference alignment strategy, this application effectively decouples the estimation process of rotation and translation, eliminates the impact of initial translation uncertainty on joint initialization accuracy, significantly improves the robustness and accuracy of the alignment between the visual-inertial system and the GNSS system coordinate system, and provides a reliable initial state estimate for subsequent fusion positioning.

[0135] S103. A single-frame geometric constraint accumulation-batch processing manifold alignment strategy and VIO algorithm are adopted to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system. When a co-view relationship is detected between the current frame and the map keyframe, constraints are established using the current local 3D spatial point cloud in the feature point information of the paired co-view frames and the historical 2D observations of the map. The current pose state of the current frame relative to the map reference camera coordinate system is solved by the inverse PnP algorithm.

[0136] In one embodiment of this application, the process of transforming the keyframe basic information and feature point information of the prior feature map to the global coordinate system using a single-frame geometric constraint accumulation-batch processing manifold alignment strategy and VIO algorithm may include the following:

[0137] S1031. Using a single-frame geometric constraint accumulation-batch processing manifold alignment strategy, the relative pose estimate of the current camera in the map reference camera coordinate system is obtained.

[0138] S1032. Obtain the first three-dimensional spatial position, wherein the first three-dimensional spatial position is the three-dimensional spatial position in the pose state after correction under the global coordinate system ENU.

[0139] S1033. Determine the second three-dimensional spatial position based on the relative pose estimation of the current camera in the map reference camera coordinate system, wherein the second three-dimensional spatial position is the position of the current frame in the map coordinate system.

[0140] S1034. The first three-dimensional spatial position and the second three-dimensional position are processed by the displacement difference alignment strategy to obtain the second transformation matrix.

[0141] S1035. Perform state transformation using the second transformation matrix to convert the keyframe basic information and feature point information of the prior feature map to the global coordinate system.

[0142] In one embodiment of this application, the execution process of obtaining the relative pose estimation of the current camera in the map reference camera coordinate system by adopting a single-frame geometric constraint accumulation-batch processing manifold alignment strategy may include the following:

[0143] S1036. Based on the current GNSS observations, select spatially adjacent keyframes from the prior feature map as candidate keyframes.

[0144] S1037. Construct the bag-of-words vector of the current frame, and obtain the optimal candidate frame by performing similarity matching with the bag-of-words vectors of the candidate keyframes. The optimal candidate frame and the current frame are co-viewed frame pairs.

[0145] S1038. Based on Euclidean distance brute-force matching of co-view frame pairs, and using the 2D-2D epipolar geometry method, the rotation matrix and normalized translation vector of the camera coordinate system relative to the map reference camera coordinate system are obtained.

[0146] S1039. Recover the metric scale using the geographic distance between the current frame GNSS observation value and the map keyframe GNSS record value, and obtain the geographic distance between the current frame GNSS observation value and the map keyframe GNSS record value.

[0147] S10310. By collecting trajectory point pairs of the initial sampling frame image, a displacement difference alignment strategy is used to calculate the transformation matrix from the prior map coordinate system to the global coordinate system ENU, and the relative pose estimation of the current camera in the map reference camera coordinate system is obtained.

[0148] In one embodiment of this application, the process of solving the current pose state of the current frame relative to the map reference camera coordinate system using the inverse PnP algorithm may include the following:

[0149] Define the set of 3D feature points in the current frame in the current camera coordinate system, and construct matching point pairs using the local 3D feature point set of the current frame and the normalized 2D feature point set stored in the map keyframe.

[0150] Based on the pinhole camera model, a reprojection error function for matching point pairs is defined, and the Huber robust kernel function is used in the reprojection error function to suppress the influence of outliers.

[0151] The optimization problem of the reprojection error function is solved using the RANSAC framework to obtain the rotation matrix and translation vector of the current frame relative to the map keyframe, which represents the current pose state of the current frame relative to the map reference camera coordinate system.

[0152] To align the global coordinate system determined by the current visual-inertial / GNSS system with the prior map coordinate system, the processor employs a "single-frame geometric constraint accumulation - batch processing manifold alignment" strategy to solve for the transformation matrix. Specifically, firstly, based on the current frame image, the processor uses a bag-of-words (DBow) model to select the most similar co-view keyframes in the prior map. Considering that the feature map may contain tens of thousands of keyframes, directly performing brute-force computation based on the bag-of-words vectors of the keyframes would be too computationally intensive. Therefore, the processor initially selects spatially adjacent keyframes in the prior feature map based on the current GNSS observations as preliminary candidate keyframes.

[0153] For example, the optimal candidate frame is obtained by comparing the similarity with the bag-of-words vectors of the candidate keyframes. For successfully matched co-view frame pairs, feature points are matched by brute force based on Euclidean distance, and the rotation matrix and normalized translation vector of the front camera coordinate system relative to the map reference camera system are recovered by using the 2D-2D epipolar geometry method.

[0154] It should be noted that since the matching of 2D-2D epipolar geometry cannot recover scale information, the metric scale is recovered using GNSS prior information. The geographic distance between the current frame GNSS observation value and the map key frame GNSS record value is calculated and approximated as the displacement modulus between the two frames.

[0155] In order to solve the global transformation, the processor needs to construct a set of 3D trajectory points corresponding to different coordinate systems. For each initial sampling frame, the current frame can be extrapolated to the map world coordinate system by combining the position in the ENU coordinate system obtained by the vision-inertial / satellite coupling algorithm. By collecting the trajectory point pairs of the current frame and the map frame over a period of time, the transformation matrix from the prior feature map coordinate system to the ENU coordinate system is calculated again by using the displacement difference alignment strategy.

[0156] Specifically, for the current frame after map initialization, when a co-view relationship is detected between the current frame and a map keyframe, the processor establishes constraints using the current local 3D spatial point cloud and historical 2D map observations. Specifically, the processor defines the set of 3D feature points in the current frame in the current camera coordinate system, references the normalized 2D feature point set stored in the keyframe, and constructs a set of matching point pairs through descriptor matching. This step directly links the current local 3D structure with the historical 2D projection, providing physical constraints for subsequent calculations.

[0157] Unlike traditional PnP, the processor utilizes the constraints of the current local 3D structure and historical 2D observations, transforming the problem into solving the pose transformation of the current camera coordinate system relative to the map reference frame camera coordinate system.

[0158] Optionally, the processor can define a reprojection error function based on the pinhole camera model, employ the Huber robust kernel function to suppress outlier effects, and use the PnP algorithm combined with the RANSAC framework to solve the above optimization problem, obtaining the rotation and translation of the current frame relative to the map keyframe. It should be noted that the physical meaning of the aforementioned steps is the position of the current camera center in the map reference frame coordinate system.

[0159] For example, let the set of 3D feature points in the current frame be... (Defined in the current camera coordinate system), reference keyframe The normalized 2D feature point set stored in is The processor can construct a set of matching point pairs through descriptor matching. :

[0160] (0.23)

[0161] in, For the 3D points observed in the current frame, Projecting the corresponding pixel coordinates onto the map keyframe. This step represents the feature descriptor. It directly links the current local 3D structure with the historical 2D projection, providing physical constraints for subsequent solutions.

[0162] This application utilizes the constraints of the current local 3D structure and historical 2D observations, transforming the problem into solving for the current camera coordinate system. relative to the camera coordinate system of the map reference frame pose transformation In other words, the processor can define the reprojection error function based on the pinhole camera model as follows:

[0163] (0.24)

[0164] in, For the camera intrinsic parameter matrix, For projection function, A Huber robust kernel function is used to suppress the influence of outliers. The PnP algorithm combined with the RANSAC framework is employed to solve the above optimization problem, obtaining the rotation of the current frame relative to the map keyframes. Peaceful relocation It is important to note here that... The physical meaning is the position of the current camera center in the map reference frame coordinate system.

[0165] Clearly, by employing a single-frame geometric constraint accumulation-batch processing manifold alignment strategy, fast and robust alignment between the current local coordinate system and the prior map global coordinate system is achieved, avoiding optimization getting trapped in local minima. Simultaneously, the relative pose calculation based on reverse PnP utilizes the current local 3D point cloud and historical 2D map observations to establish strong constraints, significantly improving the accuracy and robustness of pose estimation, and providing a reliable geometric constraint foundation for maintaining high-precision positioning even in GNSS failure environments.

[0166] S104. Based on the current pose state, calculate the corresponding latitude and longitude at the current time. Filter and fuse the latitude and longitude with the GNSS system positioning trajectory at the current time to obtain a new GNSS system positioning trajectory. Alternatively, when the GNSS system fails, use it as a virtual GNSS observation. Use the ESIKF algorithm in conjunction with the VIO algorithm to fuse the positioning results to achieve continuous positioning.

[0167] In one embodiment of this application, the process of calculating the latitude and longitude of the current moment based on the current pose state may include the following:

[0168] Obtain the global pose and extrinsic parameters of the map keyframe image in the global coordinate system ENU.

[0169] Based on the global pose and extrinsic parameters, as well as the rotation matrix and translation vector, the global absolute position of the current machine in the global coordinate system ENU is obtained through coordinate system chain transformation. Using the GNSS coordinates of the map keyframe image as anchor points, and combined with the local Cartesian coordinate increments calculated based on the global absolute position, the latitude and longitude of the current moment are calculated inversely.

[0170] After initialization, the processor can transform keyframes and 3D feature points in the map coordinate system to establish a correlation in the current visual-inertial coordinate system. Specifically, after obtaining the relative pose, the processor can recover the global absolute position of the current frame through coordinate system chaining. Given the pose and extrinsic parameters of the map keyframe in the world frame (ENU), the current position of the machine in the world frame can be derived. Then, the processor can use the GNSS coordinates of the keyframe as anchor points and combine them with the calculated local Cartesian coordinate increments to back-calculate the high-precision latitude and longitude of the current moment. This high-precision latitude and longitude is then used to replace the observations in ESIKF fusion when GNSS fails. It should be noted that by introducing this correction step, this application achieves effective closed-loop control of the error vector, which can accurately eliminate the accumulated error caused by IMU drift.

[0171] For example, the pose of a known map keyframe in the world system (ENU) is... and external references The current position of the machine within the world system This can be deduced as:

[0172] (0.25)

[0173] in, This refers to the global position of the reference map frame in the ENU coordinate system. Further, using the GNSS coordinates of the reference keyframe as anchor points, and combining this with the calculated local Cartesian coordinate increments... It calculates the high-precision latitude and longitude of the current moment and uses it to replace the observations in ESIKF fusion when GNSS fails.

[0174] In fact, by smoothly integrating the absolute pose obtained from map matching into the state estimator as an observation constraint, it can not only improve the fusion positioning accuracy when the GNSS signal is stable, but also provide a reliable absolute position reference when the GNSS fails, effectively eliminate the cumulative error of visual inertial odometry, prevent positioning drift and divergence, and thus achieve continuous high-precision positioning without cumulative error for a long time in complex environments.

[0175] The second embodiment of this application also provides a multi-source fusion continuous localization device that combines prior feature maps, the device comprising:

[0176] The prior map construction module is used to scan the target area based on the global localization algorithm and construct a serialized prior feature map containing keyframe basic information and feature point information.

[0177] The pose correction module is used to jointly initialize the visual-inertial system positioning trajectory and the GNSS system positioning trajectory through a displacement difference alignment strategy to obtain the first transformation matrix. It then performs state estimation through the VIO algorithm and fuses local pose with GNSS observations based on the ESIKF algorithm to obtain the pose state of the visual-inertial odometry (VIO) positioning result in the global coordinate system ENU after correction. The VIO algorithm is a nonlinear optimization algorithm.

[0178] The pose calculation module employs a single-frame geometric constraint accumulation-batch manifold alignment strategy and the VIO algorithm to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system. When a co-view relationship is detected between the current frame and a map keyframe, constraints are established using the current local 3D spatial point cloud in the feature point information of the paired co-view frames and the historical 2D observations of the map. The inverse PnP algorithm is then used to solve for the current pose state of the current frame relative to the map reference camera coordinate system.

[0179] The fusion positioning module is used to calculate the latitude and longitude of the current time based on the current pose state, filter and fuse the latitude and longitude with the GNSS system positioning trajectory at the current time to obtain a new GNSS system positioning trajectory, or when the GNSS system fails, it is used as a virtual GNSS observation. The ESIKF algorithm is combined with the VIO algorithm to fuse the positioning results to achieve continuous positioning.

[0180] To achieve the above objectives, a third embodiment of this application also provides an electronic device, comprising: at least one processor, a memory, and an input / output unit. The memory stores a computer program, and the processor invokes the computer program stored in the memory to execute the multi-source fusion persistent localization method combining prior feature maps provided in any of the foregoing embodiments.

[0181] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A multi-source fusion continuous localization method combining prior feature maps, characterized in that, include: The target area is scanned based on a global localization algorithm, and a serialized prior feature map containing key frame basic information and feature point information is constructed. The visual-inertial system positioning trajectory and the GNSS system positioning trajectory are jointly initialized by a displacement difference alignment strategy to obtain the first transformation matrix. The state is estimated by the VIO algorithm, and the local pose and GNSS observation are fused based on the ESIKF algorithm to obtain the pose state of the visual-inertial odometry positioning result in the global coordinate system ENU after correction. The VIO algorithm is a nonlinear optimization algorithm. A single-frame geometric constraint accumulation-batch processing manifold alignment strategy and VIO algorithm are adopted to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system. When a co-view relationship is detected between the current frame and the map keyframe, constraints are established by using the current local 3D spatial point cloud in the feature point information of the paired co-view frames and the historical 2D observations of the map. The current pose state of the current frame relative to the map reference camera coordinate system is solved by the inverse PnP algorithm. Based on the current pose state, the latitude and longitude of the current moment are calculated. The latitude and longitude are then filtered and fused with the GNSS system positioning trajectory at the current moment to obtain a new GNSS system positioning trajectory. Alternatively, when the GNSS system fails, it can be used as a virtual GNSS observation. The ESIKF algorithm combined with the VIO algorithm is used to fuse the positioning results to achieve continuous positioning. The displacement difference alignment strategy includes calculating the displacement difference between adjacent moments based on a given time-aligned sequence of trajectory point pairs to construct an observation vector, thereby obtaining the displacement difference between the aligned visual-inertial system positioning trajectory and the GNSS system positioning trajectory at their respective adjacent moments. The joint initialization of the visual-inertial system positioning trajectory and the GNSS system positioning trajectory through the displacement difference alignment strategy, to obtain the first transformation matrix, includes: The rigid body rotation constraint expression is determined based on the displacement difference between adjacent time points. Constructing a nonlinear least squares problem based on rigid body rotation constraint expressions; The Levenberg-Marquardt method is used to solve the least squares problem, obtain the optimal rotation matrix, and recover the translation vector using the latest reliable GNSS observations; The first transformation matrix from the local coordinate system of the vision-inertial system to the global coordinate system ENU is obtained based on the recovered translation vector.

2. The multi-source fusion continuous localization method combining prior feature maps as described in claim 1, characterized in that, The method employs a single-frame geometric constraint accumulation-batch processing manifold alignment strategy and the VIO algorithm to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system, including: A single-frame geometric constraint accumulation-batch processing manifold alignment strategy is adopted to obtain the relative pose estimate of the current camera in the map reference camera coordinate system; Obtain the first three-dimensional spatial position, where the first three-dimensional spatial position is the three-dimensional spatial position in the pose state after correction under the global coordinate system ENU; The second three-dimensional spatial position is determined based on the relative pose estimation of the current camera in the map reference camera coordinate system, where the second three-dimensional spatial position is the position of the current frame in the map coordinate system; The first three-dimensional spatial position and the second three-dimensional position are processed by a displacement difference alignment strategy to obtain the second transformation matrix; The state transformation is performed using the second transformation matrix to convert the keyframe basic information and feature point information of the prior feature map to the global coordinate system.

3. The multi-source fusion continuous localization method combining prior feature maps as described in claim 2, characterized in that, The method employs a single-frame geometric constraint accumulation-batch processing manifold alignment strategy to obtain the relative pose estimate of the current camera in the map reference camera coordinate system, including: Based on current GNSS observations, spatially adjacent keyframes are selected as candidate keyframes from the prior feature map. Construct the bag-of-words vector of the current frame, and obtain the optimal candidate frame by performing similarity matching with the bag-of-words vectors of candidate keyframes. The optimal candidate frame and the current frame are co-viewed frame pairs. Based on Euclidean distance brute-force matching of co-view frame pairs, and using the 2D-2D epipolar geometry method, the rotation matrix and normalized translation vector of the camera coordinate system relative to the map reference camera coordinate system are obtained. The metric scale is recovered by using the geographic distance between the current frame GNSS observation value and the map keyframe GNSS record value, thus obtaining the geographic distance between the current frame GNSS observation value and the map keyframe GNSS record value; By collecting trajectory point pairs from the initial sampling frame images, the displacement difference alignment strategy is used to calculate the transformation matrix from the prior map coordinate system to the global coordinate system ENU, thereby obtaining the relative pose estimate of the current camera in the map reference camera coordinate system.

4. The multi-source fusion continuous localization method combining prior feature maps as described in claim 1, characterized in that, The step of solving the current pose state of the current frame relative to the map reference camera coordinate system using the inverse PnP algorithm includes: Define the set of 3D feature points in the current frame in the current camera coordinate system, and construct matching point pairs using the local 3D feature point set of the current frame and the normalized 2D feature point set stored in the map keyframe; Based on the pinhole camera model, a reprojection error function for matching point pairs is defined, and the Huber robust kernel function is used in the reprojection error function to suppress the influence of outliers. The optimization problem of the reprojection error function is solved using the RANSAC framework to obtain the rotation matrix and translation vector of the current frame relative to the map keyframe, which represents the current pose state of the current frame relative to the map reference camera coordinate system.

5. The multi-source fusion continuous localization method combining prior feature maps as described in claim 4, characterized in that, The step of calculating the latitude and longitude of the current moment based on the current pose state includes: Obtain the global pose and extrinsic parameters of the map keyframe image in the global coordinate system ENU; Based on the global pose and extrinsic parameters, as well as the rotation matrix and translation vector, the global absolute position of the current machine in the global coordinate system ENU is obtained through coordinate system chain transformation. Using the GNSS coordinates of the map keyframe image as anchor points, and combined with the local Cartesian coordinate increments calculated based on the global absolute position, the latitude and longitude of the current moment are calculated inversely.

6. The multi-source fusion persistent localization method combining prior feature maps as described in claim 1, characterized in that, The basic information of the keyframe includes timestamp, keyframe index, pose of the keyframe in the global coordinate system and corresponding latitude and longitude information. The feature point information includes feature point index, descriptor, normalized planar coordinates, pixel coordinates and spatial 3D coordinates of the feature point in the map coordinate system.

7. A multi-source fusion persistent localization device combining prior feature maps, employing the multi-source fusion persistent localization method combining prior feature maps as described in any one of claims 1 to 6, characterized in that, The device includes: The prior map construction module is used to scan the target area based on the global positioning algorithm and construct a serialized prior feature map containing key frame basic information and feature point information. The pose correction module is used to jointly initialize the visual-inertial system positioning trajectory and the GNSS system positioning trajectory through a displacement difference alignment strategy to obtain the first transformation matrix, and to perform state estimation through the VIO algorithm. Based on the ESIKF algorithm, the local pose and GNSS observation are fused to obtain the pose state of the visual-inertial odometry (VIO) positioning result in the global coordinate system ENU after correction. The VIO algorithm is a nonlinear optimization algorithm. The pose solving module is used to transform the keyframe basic information and feature point information of the prior feature map to the global coordinate system using a single-frame geometric constraint accumulation-batch processing manifold alignment strategy and VIO algorithm. When a co-view relationship is detected between the current frame and the map keyframe, constraints are established using the current local 3D spatial point cloud in the feature point information of the paired co-view frames and the historical 2D observations of the map. The current pose state of the current frame relative to the map reference camera coordinate system is solved by the inverse PnP algorithm. The fusion positioning module is used to calculate the latitude and longitude of the current time based on the current pose state, filter and fuse the latitude and longitude with the GNSS system positioning trajectory at the current time to obtain a new GNSS system positioning trajectory, or when the GNSS system fails, it is used as a virtual GNSS observation. The ESIKF algorithm is combined with the VIO algorithm to fuse the positioning results to achieve continuous positioning.

8. An electronic device, characterized in that, The electronic device includes: At least one processor, memory, and input / output unit; The memory is used to store computer programs, and the processor is used to call the computer programs stored in the memory to execute the multi-source fusion continuous localization method combining prior feature maps according to any one of claims 1 to 6.

Images