Search

CN-121761881-B - Multi-source fusion continuous positioning method combined with priori feature map

CN121761881BCN 121761881 BCN121761881 BCN 121761881BCN-121761881-B

Abstract

The invention discloses a multisource fusion continuous positioning method combined with a priori feature map, which aims at the problem that the conventional vision-inertial odometer is easy to generate accumulated errors in a GNSS refused environment; the method comprises the steps of adopting a track displacement difference value alignment strategy to finish the combined initialization of a vision-inertial system and a GNSS, using ESIKF to replace EKF to improve fusion accuracy, realizing the steady alignment of a local coordinate system and a map coordinate system through a single-frame geometric constraint accumulation-batch processing manifold alignment strategy, solving relative pose based on reverse PnP by utilizing current 3D point cloud and historical 2D observation constraint, using map matching results as observation and GNSS fusion or replacing GNSS observation, and utilizing ESIKF loose coupling to eliminate accumulated errors to realize continuous high-accuracy positioning under a GNSS failure scene.

Inventors

  • ZHANG TONG
  • Zhou Bize
  • GAO HENG
  • YANG TAO

Assignees

  • 西北工业大学

Dates

Publication Date
20260508
Application Date
20260302

Claims (8)

  1. 1. A multisource fusion continuous positioning method combined with a priori feature map is characterized by comprising the following steps of: Scanning a target area based on a global positioning algorithm, and constructing a serialized prior feature map containing key frame basic information and feature point information; The method comprises the steps of carrying out joint initialization on a visual-inertial system positioning track and a GNSS system positioning track through a displacement difference alignment strategy to obtain a first transformation matrix, carrying out state estimation through a VIO algorithm, and carrying out local pose and GNSS observation fusion based on ESIKF algorithm to obtain a pose state of a visual inertial odometer VIO after the positioning result is corrected under a global coordinate system ENU, wherein the VIO algorithm is a nonlinear optimization algorithm; When the common view relation between the current frame and the map key frame is detected, the constraint is established between the current local 3D space point cloud in the characteristic point information of the paired common view frames and the historical 2D observation of the map, and the current pose state of the current frame relative to the reference camera coordinate system of the map is solved through the inverse PnP algorithm; Calculating longitude and latitude of the corresponding current moment based on the current pose state, filtering and fusing the longitude and latitude and the GNSS system positioning track of the current moment to obtain a new GNSS system positioning track, or fusing positioning results by adopting ESIKF algorithm combined with VIO algorithm when the GNSS system fails to realize continuous positioning, wherein the displacement difference alignment strategy comprises calculating displacement difference construction observation vectors of adjacent moments based on a track point pair sequence aligned at a given time to obtain displacement difference values of the aligned vision-inertial system positioning track and the GNSS system positioning track at the respective adjacent moments; the combined initialization of the visual-inertial system positioning track and the GNSS system positioning track is carried out through a displacement difference alignment strategy to obtain a first transformation matrix, which comprises the following steps: determining a rigid body rotation constraint expression of the relative displacement based on the displacement difference values of the respective adjacent moments; constructing a nonlinear least square problem based on a rigid body rotation constraint expression; Solving a least square problem by adopting a Levenberg-Marquardt method to obtain an optimal rotation matrix, and recovering a translation vector by utilizing the latest reliable GNSS observation; a first transformation matrix from the vision-inertial system local coordinate system to the global coordinate system ENU is obtained based on the recovered translation vector.
  2. 2. The multi-source fusion continuous positioning method combined with the prior feature map according to claim 1, wherein the step of converting key frame basic information and feature point information of the prior feature map into a global coordinate system by adopting a single-frame geometric constraint accumulation-batch manifold alignment strategy and a VIO algorithm comprises the following steps: Adopting a single-frame geometric constraint accumulation-batch processing manifold alignment strategy to obtain the relative pose estimation of the current camera under a map reference camera coordinate system; acquiring a first three-dimensional space position, wherein the first three-dimensional space position is a three-dimensional space position in the corrected pose state under the global coordinate system ENU; Determining a second three-dimensional space bit based on the relative pose estimation of the current camera under the map reference camera coordinate system, wherein the second three-dimensional space bit is the position of the current frame under the map coordinate system; Processing the first three-dimensional space position and the second three-dimensional position through a displacement difference alignment strategy to obtain a second transformation matrix; And carrying out state transformation through a second transformation matrix to convert key frame basic information and feature point information of the prior feature map into a global coordinate system.
  3. 3. The method for multi-source fusion continuous positioning combined with a priori feature map according to claim 2, wherein the step of obtaining the relative pose estimation of the current camera under the map reference camera coordinate system by adopting a single-frame geometric constraint accumulation-batch manifold alignment strategy comprises the following steps: screening the key frames close to the space in the prior characteristic map according to the current GNSS observation as candidate key frames; constructing a word bag vector of the current frame, and obtaining an optimal candidate frame by matching the word bag vector of the candidate key frame with the word bag vector of the candidate key frame in a similarity mode, wherein the optimal candidate frame and the current frame are a common-view frame pair; Based on Euclidean distance violent matching common view frame pairs, a 2D-2D epipolar geometry method is adopted to obtain a rotation matrix and a normalized translation vector of a camera coordinate system relative to a map reference camera coordinate system; restoring metric scales by using the geographic distance between the current frame GNSS observation value and the map key frame GNSS record value to obtain the geographic distance between the current frame GNSS observation value and the map key frame GNSS record value; And calculating a conversion matrix from the prior map coordinate system to the global coordinate system ENU by collecting track point pairs of the initialized sampling frame image and adopting the displacement difference alignment strategy to obtain the relative pose estimation of the current camera under the map reference camera coordinate system.
  4. 4. The method for multi-source fusion continuous positioning combined with a priori feature map according to claim 1, wherein the solving the current pose state of the current frame relative to the map reference camera coordinate system by the inverse PnP algorithm comprises: defining a 3D characteristic point set in a current frame under a current camera coordinate system, and constructing matching point pairs by utilizing the local 3D characteristic point set of the current frame and a normalized 2D characteristic point set stored in a map key frame; Defining a re-projection error function of the matched point pair according to the pinhole camera model, and adopting a Huber robust kernel function in the re-projection error function to inhibit outlier influence; And solving the optimization problem of the reprojection error function by using the RANSAC framework to obtain a rotation matrix and a translation vector of the current frame relative to the map key frame so as to represent the current pose state of the current frame relative to the map reference camera coordinate system.
  5. 5. The multi-source fusion continuous positioning method combined with a priori feature map according to claim 4, wherein the calculating the longitude and latitude of the corresponding current moment based on the current pose state comprises: acquiring the global pose and external parameters of the map key frame image under the global coordinate system ENU; Based on the global pose and the external parameters, and the rotation matrix and the translation vector are transformed through a coordinate tether, the global absolute position of the current organism under the global coordinate system ENU is obtained, the GNSS coordinates of the map key frame image are taken as anchor points, and the longitude and latitude of the current moment are reversely calculated by combining the local Cartesian coordinate increment calculated according to the global absolute position.
  6. 6. The multi-source fusion continuous positioning method combined with the prior feature map according to claim 1, wherein the key frame basic information comprises a time stamp, a key frame index, a pose of a key frame under a global coordinate system and corresponding longitude and latitude information, and the feature point information comprises a feature point index, a descriptor, a normalized plane coordinate, a pixel coordinate and a spatial 3D coordinate of a feature point under a map coordinate system.
  7. 7. A multi-source fusion continuous positioning device combined with a priori feature map, applying the multi-source fusion continuous positioning method combined with a priori feature map according to any one of claims 1 to 6, characterized in that the device comprises: The prior map construction module is used for scanning the target area based on a global positioning algorithm and constructing a serialized prior feature map containing key frame basic information and feature point information; The pose correction module is used for carrying out joint initialization on a visual-inertial system positioning track and a GNSS system positioning track through a displacement difference alignment strategy to obtain a first transformation matrix, carrying out state estimation through a VIO algorithm, and carrying out local pose and GNSS observation fusion based on ESIKF algorithm to obtain a pose state of a visual inertial odometer VIO positioning result corrected under a global coordinate system ENU, wherein the VIO algorithm is a nonlinear optimization algorithm; When the common view relation between the current frame and the map key frame is detected, the constraint is established between the current local 3D space point cloud in the characteristic point information of the paired common view frames and the historical 2D observation of the map, and the current pose state of the current frame relative to the coordinate system of the map reference camera is solved through the reverse PnP algorithm; And the fusion positioning module is used for reversely calculating the longitude and latitude of the corresponding current moment based on the current pose state, filtering and fusing the longitude and latitude with the GNSS system positioning track of the current moment to obtain a new GNSS system positioning track, or taking the new GNSS system positioning track as GNSS virtual observation when the GNSS system fails, and fusing positioning results by adopting ESIKF algorithm and VIO algorithm so as to realize continuous positioning.
  8. 8. An electronic device, the electronic device comprising: At least one processor, memory, and input output unit; The memory is used for storing a computer program, and the processor is used for calling the computer program stored in the memory to execute the multi-source fusion continuous positioning method combined with the prior characteristic map according to any one of claims 1-6.

Description

Multi-source fusion continuous positioning method combined with priori feature map Technical Field The application relates to the field of fusion positioning, in particular to a multi-source fusion continuous positioning method, device and equipment combined with a priori feature map. Background The simultaneous localization and mapping (Simultaneous Localization AND MAPPING, SLAM) technique is a core technique of intelligent autonomous robots when performing task targets. In recent years, this technology has been widely studied and applied, and has been successfully applied to various fields such as micro unmanned aerial vehicles, intelligent driving, virtual Reality (VR) and Augmented Reality (AR). With research and open source of various parties in recent years, SLAM technology is also developed in the direction of multi-sensor fusion, wherein a Visual Inertial Odometer (VIO) fusion method is one of the most mainstream positioning schemes at present due to the advantages of light weight, low cost and low power consumption. However, the pure vision inertial system is essentially a dead reckoning (Dead Reckoning) method, which can only calculate the relative pose using the surrounding environmental information, and as the running time and distance increase, an accumulated error (Drift) is inevitably generated, resulting in a significant decrease in positioning accuracy after long running time. To suppress the accumulated error, it has become an effective means to correct the drift of the VIO by fusing global navigation satellite system (GNSS/RTK) signals on the basis of the visual inertial system, which uses absolute position information. However, such tight-coupling or loose-coupling schemes are highly dependent on the quality of the satellite signals. In GNSS rejection environments where satellite signals such as indoor, tunnel, urban canyon and the like are blocked or multipath effect is serious, the system is degraded back to the pure VIO mode, so that the problem of accumulated error divergence is faced again, and positioning is discontinuous or severe jump occurs. In order to solve the problem of long-time high-precision positioning in a satellite signal-free environment, auxiliary positioning by using a pre-built high-precision environment map (prior map) becomes an important trend. However, the existing map-based positioning method still faces many challenges in practical application, such as how to efficiently construct a lightweight feature map containing rich information, how to quickly and robustly complete the initialization alignment of the local coordinate system of the current visual inertial system and the global coordinate system of the prior map in the system start-up or repositioning stage, avoid the optimization from being trapped in local minima, and how to smoothly integrate the absolute pose obtained by map matching into a state estimator (such as ESIKF) as an observation constraint to eliminate drift without causing drift. Disclosure of Invention The application mainly aims to provide a multi-source fusion continuous positioning method, device and equipment combined with a priori feature map, and aims to realize continuous positioning of a visual-inertial algorithm under a GNSS unstable or failure scene without accumulated errors through sequential map construction, steady initialization alignment based on gravity manifold and a tight coupling fusion strategy based on reverse PnP. In order to achieve the above object, the present application provides a multi-source fusion continuous positioning method combined with a priori feature map, including: Scanning a target area based on a global positioning algorithm, and constructing a serialized prior feature map containing key frame basic information and feature point information; The method comprises the steps of carrying out joint initialization on a visual-inertial system positioning track and a GNSS system positioning track through a displacement difference alignment strategy to obtain a first transformation matrix, carrying out state estimation through a VIO algorithm, and carrying out local pose and GNSS observation fusion based on ESIKF algorithm to obtain a pose state of a visual inertial odometer VIO after the positioning result is corrected under a global coordinate system ENU, wherein the VIO algorithm is a nonlinear optimization algorithm; When the common view relation between the current frame and the map key frame is detected, the constraint is established between the current local 3D space point cloud in the characteristic point information of the paired common view frames and the historical 2D observation of the map, and the current pose state of the current frame relative to the reference camera coordinate system of the map is solved through the inverse PnP algorithm; And reversely calculating the longitude and latitude of the corresponding current moment based on the current pose state, filtering and fusing the longitu