A visual inertial navigation positioning method for semantic description map constraint in urban beidou denial environment
Patent Information
- Application Number
- CN202611014639.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-09
- Publication Date
- 2026-10-09
AI Technical Summary
[0005]有鉴于此，本发明提供一种城市北斗拒止环境下语义描述地图约束的视觉惯性导航定位方法，用以解决现有视觉惯性导航定位模型在城市北斗拒止环境下难以长时连续全局定位的缺陷
[0095]1、针对传统视觉惯性导航仅依靠重投影误差约束、缺乏匹配可信度评估的缺陷，本发明通过图像语义分割筛选有效视觉要素，结合多重一致性匹配机制构建在线与地图特征匹配关联，求解综合匹配置信度，实现地图观测质量的动态量化评估，有效抑制光照变化、场景扰动带来的误匹配问题，自适应生成地图观测协方差，提升地图约束视觉量测的可靠性与自适应能力，规避定位漂移问题。
Smart Images
Figure CN122881741A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of autonomous navigation technology for mobile carriers, and in particular to a visual inertial navigation and positioning method constrained by semantic description maps in urban BeiDou denied environments. Background Technology
[0002] With the rapid development of intelligent transportation, accurate and continuous measurement of the six degrees of freedom (6-DoF) pose of a vehicle has become a key function supporting high-level autonomous driving (Li et al., 2025). Currently, the BeiDou Navigation Satellite System has completed the deployment of a nationwide network of satellite navigation and positioning reference stations, providing centimeter-level real-time absolute positioning services in open environments, laying a solid foundation for intelligent transportation and precise spatiotemporal information applications (Liu et al., 2022). However, in "BeiDou denied spaces" such as urban canyons, underground tunnels, and shaded environments, satellite signals are easily blocked, leading to loss of lock and consequently a sharp drop in vehicle positioning accuracy or even failure, restricting the long-term reliable and safe operation of unmanned vehicles (Nan et al., 2025). Therefore, for urban BeiDou satellite system denied scenarios, there is an urgent need for a navigation and positioning method that does not rely on satellite information. This method should integrate the measurement advantages of sensors such as vision, inertial, and prior semantic maps to establish a navigation and positioning model with strong generalization ability and high reliability. This method is of great significance for achieving long-term continuous and reliable positioning of carriers in BeiDou-denied environments, and can also provide a solid theoretical foundation for the engineering application of autonomous navigation technology for carriers in underground space scenarios.
[0003] Currently, numerous studies have been conducted by scholars both domestically and internationally regarding positioning methods that integrate prior maps and visual inertial navigation. Visual inertial navigation (VIO) technology is currently the core means for autonomous positioning of unmanned platforms in BeiDou-denied environments. Existing VIO algorithms based on prior map constraints are mainly divided into two major technical architectures: optimization-based and filtering-based. Both types of schemes have significant technical shortcomings. Among them, optimization-based VIO systems such as ORB-SLAM3 (C. Campos et al. 2021), VINS-Fusion (T. Qin et al. 2019), Kimera (A. Rosinol et al. 2020), and RTAB-Map (M. Labbe et al. 2013) achieve high-precision pose estimation by relying on loop closure detection and graph optimization. However, these methods exhibit non-causal estimation characteristics, leading to backtracking and correction of historical poses during backend optimization, resulting in attitude jumps and failing to meet the closed-loop requirements of real-time navigation control. Filtering-based methods such as MSCKF (A. I et al. 2007), ROVIO (M. Bloesch et al. 2015), and OpenVINS (P. Geneva et al. 2020) possess strict causal timing, strong real-time performance, and suitability for airborne platforms with limited capabilities. However, they largely rely on constraints of a single global map, and cross-regional positioning requires overlapping areas between sub-maps. This limits their ability to support large-scale deployment in scenarios with multiple isolated, non-overlapping maps, thus restricting their scenario adaptability. In the field of multi-map operations, existing multi-sub-map fusion schemes still have significant shortcomings. ORB-SLAM3 relies on map overlap for transition, and while Maplab 2.0 (A. Cramariuc et al. 2022) can achieve non-overlapping trajectory alignment, it relies on batch processing optimization, suffers from non-causal issues, and requires additional sensors such as lasers and semantic sensors, making it impossible to achieve autonomous positioning solely using visual inertial sensors.
[0004] In summary, existing map-constrained visual inertial navigation (VIO) technologies have significant shortcomings. First, traditional prior feature map data is massive and difficult to update and maintain, making it unsuitable for lightweight deployments in real-time field navigation. Second, existing methods do not dynamically model the uncertainty of map matching errors, resulting in insufficient positioning fault tolerance and robustness. Third, existing map-assisted VIOs largely rely on optimized architectures, leading to attitude jump issues and a lack of a real-time positioning scheme that tightly couples semantic maps and VIO based on filtering, making it difficult to meet the high-precision, high-real-time autonomous navigation requirements in BeiDou-denied environments. To address these shortcomings, this invention proposes a semantically descriptive map-constrained VIO positioning method for urban BeiDou-denied environments. Summary of the Invention
[0005] In view of this, the present invention provides a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment, in order to solve the defect of existing visual inertial navigation and positioning models that are difficult to achieve long-term continuous global positioning in an urban BeiDou denied environment.
[0006] This invention provides a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment, comprising:
[0007] Collect visual inertial data and perform visual inertial odometry prior propagation; determine the triggering of prior map constraints based on the positioning status.
[0008] Deep learning technology is used to segment semantic elements of online visual images in real time and extract feature points to construct online visual feature vectors.
[0009] By combining online observation feature vectors with map point feature vectors, multiple consistency matching is performed to obtain the comprehensive matching confidence.
[0010] The observation covariance of map observation factors is determined based on the comprehensive matching confidence and the basic pixel measurement covariance, and then the overall map observation covariance is derived.
[0011] Construct a Map-VIO compact combination navigation model, perform filtering updates, and obtain the final accurate and reliable navigation and positioning results.
[0012] According to the present invention, a visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment is provided, which involves collecting visual inertial data, performing visual inertial odometry prior propagation, and determining the triggering of prior map constraints based on the positioning status, including:
[0013] A spatiotemporally synchronized visual inertial data acquisition platform was used to acquire visual and INS data in urban BeiDou denied environments. The visual data included stereo images, and the INS data included acceleration and angular velocity data measured in real time by the inertial measurement unit.
[0014] Based on the MSCKF theory, visual-inertial odometry (VIO) is used to track the carrier coordinate system with the IMU as the origin. Relative to global navigation coordinate system The camera pose is used to propagate the system state and covariance. When a new image is observed, the camera pose estimate is augmented into the system state vector. Therefore, the system state vector includes the IMU state vector. and Given N camera poses, when a new image participates in the system localization at time k, the filter state vector will have N camera poses. Therefore, the system state vector can be expressed as:
[0015]
[0016] in, This includes the carrier's pose, velocity, triaxial acceleration, and angular velocity offset. and , , are the estimated values of the camera pose.
[0017] Then, the system error state vector is defined. ,in, .
[0018] During state vector propagation, the expectation operator is used to represent the IMU state estimation during propagation as follows:
[0019]
[0020] in, For system noise, The covariance matrix is This can be obtained by calibration using Alla variance. and These represent the system state transition matrix and gain matrix, respectively.
[0021] In practical applications, the state estimation of the IMU can be represented by the fourth-order Runge-Kutta numerical integral of the state propagation equation, thus yielding the discretized state transition matrix and gain matrix as follows:
[0022]
[0023]
[0024] in, Let the continuous-time noise covariance of the system be represented, then the pre-integral covariance of the IMU state can be expressed as:
[0025]
[0026] Based on the covariance propagation law of the Extended Kalman Filter (EKF), and combining the IMU and visual state vectors, the system state covariance can be defined as:
[0027]
[0028] in, It is in IMU status. covariance matrix It is camera pose estimation The covariance matrix, The correlation between IMU state and camera pose estimation error.
[0029] After the IMU and image states are updated at the new time step, the covariance matrix of the propagation state can be expressed as:
[0030]
[0031] When recording new images, the system combines the corresponding feature points from the binocular images with the camera pose to obtain the 3D coordinates of the feature points in the world coordinate system, and then projects them onto the camera coordinate system to obtain:
[0032]
[0033] in, and They represent The pose of the left camera in the global n-frame at any given time. , In order The rotation matrix and translation vector from the left camera to the right camera at each time step. , These represent the positions of the feature points in the left and right camera systems, respectively.
[0034] By reprojecting the camera coordinates, a visual observation model can be constructed, and the observation model can be linearized to construct the visual measurement residuals:
[0035] To avoid the same feature point Because the superimposed observations at different times affect the measurement residuals, the residuals are projected onto... From the left null space, we can obtain:
[0036]
[0037] By further superimposing multiple observations of all feature points, we can obtain .
[0038] To reduce the computational complexity of EKF updates, QR decomposition yields ,Pick For EKF updates, there are , Then calculate the Kalman gain. and update the system error status. and system status Simultaneously update the system state covariance matrix. This completes the state propagation.
[0039] Based on the covariance of the carrier pose in the system state and The carrier positioning uncertainty can be defined. And based on the uncertainty results at different times, the uncertainty growth rate is statistically analyzed. Therefore, the average visual residual of the current VIO system is defined as:
[0040]
[0041] in, and These are the position and attitude sub-state covariances, respectively. , , and These are the pixel coordinates of the visual feature observation and the pixel coordinates of the feature point reprojection, respectively. For pixel observation noise covariance; For pixel observation noise covariance;
[0042] comprehensive , and Evaluation factors can be used to construct system positioning degradation indicators:
[0043]
[0044] in, The chi-square threshold of the two-dimensional pixel residual at the confidence level; and This is a normalization scale.
[0045] According to the present invention, a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment employs deep learning technology to segment semantic elements of online visual images in real time and extract feature points to construct an online visual feature vector, including:
[0046] First, a semantic segmentation network for urban environmental traffic elements was designed based on the VGG16-Unet network to acquire traffic elements with uniform spatial distribution and good geometric configuration in the urban environment from binocular images. and The network embeds a convolutional attention mechanism module at the skip connection between the encoder and decoder to adaptively weight the channel and spatial dimensions to enhance the attention to the details of traffic and road elements. In the decoder part, a multi-branch feature fusion module is used to fuse receptive field features of different scales to fully capture local details and alleviate feature instability caused by scale changes and illumination differences.
[0047] Then, Superpoint is used to... Time segmentation region and Feature points are extracted sequentially, and feature matching is performed using Lightglue to obtain pairs of feature points with the same name. And count the pixel coordinates of each feature point. semantic features and descriptor features ;
[0048] Based on the acquired feature point pairs Obtained by forward tracing using optical flow method The feature observation trajectory of this feature point is constructed as follows: If a feature point cannot be tracked, the feature trajectory is terminated. The descriptors corresponding to each feature point are counted sequentially. Polar geometric error Forward and backward tracking error Stereo reprojection error Tracking length Local brightness average Local gradient intensity For each trajectory Extracting temporal feature vectors LSTM is used for time-series updates, and the confidence scores corresponding to the observed trajectories of feature points are output, which are the quality scores of the online feature points. Therefore, the online feature vector of each feature point is determined as follows: .
[0049] According to the present invention, a visual inertial navigation and positioning method constrained by semantic description maps in an urban BeiDou denied environment is provided, for each trajectory Extracting temporal feature vectors includes:
[0050] Similarity distance is calculated based on descriptors of left and right feature points. ;
[0051] Using the calibration parameters of a binocular camera, epipolar line correction was performed on the left and right images, and the epipolar geometric errors of corresponding points on the left and right sides were statistically analyzed. ;
[0052] Based on the corresponding point pairs of inter-frame features, construct the forward and backward tracking error. ;
[0053] Through multi-view Figure 3 Feature points obtained by keratinization The corresponding 3D coordinates are then used to reproject the point onto the image using the pose and intrinsic parameters of the l-th camera, thus obtaining the predicted image point coordinates. The actual observed image point coordinates are Then construct the reprojection error ;
[0054] Tracking length of statistical feature points ;
[0055] To consider the robustness of feature points to brightness variations, a local window is taken centered on the pixel coordinates of the feature point. Statistical analysis of local brightness mean ;
[0056] To consider the robustness of feature points to variations in texture intensity, a local window is taken centered on the pixel coordinates of the feature point. Calculate the average gradient magnitude within a local window. .
[0057] According to the present invention, a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment combines online observation feature vectors and map point feature vectors to perform multiple consistency matching to obtain a comprehensive matching confidence score, including:
[0058] First, based on the online carrier pose information, search for a set of prior map points within 60m in front of the carrier. Each map point's feature vector is composed of global 3D coordinates. Depth descriptor Semantic categories and map point quality score Joint construction, i.e. ;
[0059] Then, based on online point pixel coordinates Candidate map points are obtained through projection gating. That is, combining the prior pose of the carrier to backproject the map candidate points onto the image to obtain the predicted pixels. By designing projection gating, points that are relatively close can be retained: , Indicates the pixel gate radius;
[0060] Finally, multiple consistency checks are performed on the online feature points and candidate map points, including semantic consistency checks. Let the semantic label of the online feature be... Map point category is Then semantic consistency can be defined as: ;
[0061] Online feature point and candidate map point descriptor consistency determination, assuming the current image feature descriptor is... Map point descriptors are Define features to describe Euclidean distance ;
[0062] Based on the ray directions constructed from map points and camera centers, and the back-projected ray directions constructed from online features and image principal points, viewpoint consistency is determined. Then, based on the camera pose, the map points are... Transform to camera coordinate system The corresponding unit predicts the line of sight direction as follows: Let the online pixels be... Combined with camera internal parameters The projection direction of the online features in the camera coordinate system can be obtained as follows:
[0063]
[0064] The line-of-sight consistency determination between online pixels and map points is then performed as follows: , .
[0065] According to the present invention, a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment determines the observation covariance of map observation factors based on the comprehensive matching confidence and the basic pixel measurement covariance, and then derives the overall map observation covariance, including:
[0066] Based on online feature points and map point feature vectors, the map observation confidence is defined as follows, through semantic consistency, descriptor consistency, line-of-sight consistency, and reprojection geometric consistency:
[0067]
[0068] in, , ;
[0069] Considering pixel measurement error, the fundamental covariance can be written as Furthermore, by comprehensively considering the confidence level of map observations, the covariance of map observation factors is determined as follows: Then a single online feature point With map points The constructed map observation factors are: And the contribution of a single map matching to the information matrix of camera pose is derived as follows:
[0070]
[0071] in, , .
[0072] Furthermore, the information matrix contributions and state covariances of all matched online features and map points to the camera pose in a single frame image can be derived as follows:
[0073]
[0074]
[0075] in, Index all matched online point and map point pairs. is the damping coefficient.
[0076] According to the present invention, a visual-inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou-denied environment is provided, which constructs a Map-VIO compact combination navigation model, performs filtering updates, and obtains a final accurate and reliable navigation and positioning result, including:
[0077] Camera pose based on VIO prediction and Projected candidate map feature points Corresponding pixel coordinates for:
[0078]
[0079] in, , It is the rotation matrix from the carrier system to the camera system;
[0080] Combining ideal pixel coordinates of online visual feature points and measurement noise Determine the observed pixels This leads to the derivation of map measurement errors:
[0081]
[0082] Simultaneously, the measurement equation is constructed based on map factors as follows:
[0083]
[0084] in, It is the Jacobian matrix describing the relationship between pixel error observations and error state variables, i.e. The partial derivative with respect to the carrier camera pose is specifically expressed as:
[0085]
[0086] in, , Camera coordinates The partial derivative with respect to the carrier pose is obtained using perturbation analysis. .
[0087] In summary, the observation matrix constrained by the feature map can be extended to the VIO system measurement model to construct the measurement equations for the Map-VIO navigation model. The specific expansion is as follows:
[0088]
[0089] When performing multi-state constraint updates, the residual vector can be computed. , And the corresponding feature measurement matrix, and superimposed into a single vector. Filtering is performed to solve the problem using QR decomposition. Then, EKF is performed to calculate the Kalman gain. .
[0090] Finally, the state correction vector And update the state covariance matrix to obtain an accurate and reliable pose.
[0091]
[0092] in, This represents the dimension of the covariance matrix.
[0093] This invention provides a visual inertial navigation and positioning method with semantic description map constraints in urban BeiDou denied environments. It relies on a lightweight semantic prior map to achieve global constraints. First, visual inertial observation data is collected to complete visual inertial odometry state prior propagation. The current positioning deviation is then used to determine whether to activate semantic map constraints. Deep learning technology is used to perform real-time semantic segmentation of stereo images and extract effective features, constructing an online feature vector. The online features are matched with the lightweight semantic map using multiple consistency matching methods (semantics, viewpoint, and features) to solve for the comprehensive matching confidence. Based on the matching confidence, the pixel basic covariance is dynamically corrected, and the overall observation covariance of map factors is adaptively solved to quantify the map constraint uncertainty. A Map-VIO tightly coupled navigation model is built to complete state filtering updates, outputting continuous and stable navigation and positioning results. This method solves the shortcomings of traditional map-constrained visual inertial navigation and positioning methods, such as map data redundancy and difficulty in maintenance, inability to quantify map matching uncertainty, and the lack of a filtered semantic map visual inertial tightly coupled positioning scheme. It effectively improves the long-term reliable positioning accuracy in urban BeiDou denied scenarios and achieves rapid global repositioning.
[0094] Beneficial effects of the present invention
[0095] 1. To address the shortcomings of traditional visual inertial navigation, which relies solely on reprojection error constraints and lacks matching reliability assessment, this invention filters effective visual elements through image semantic segmentation, constructs online matching associations with map features using a multi-consistency matching mechanism, solves the comprehensive matching confidence score, and achieves dynamic quantitative assessment of map observation quality. This effectively suppresses mismatch problems caused by illumination changes and scene disturbances, adaptively generates map observation covariance, improves the reliability and adaptability of map-constrained visual measurements, and avoids positioning drift problems.
[0096] 2. Construct a Map-VIO filter compact combination navigation model with semantic map constraints. Relying on the prior constraints of the semantic map to make up for the lack of satellite positioning in the BeiDou denied environment, it integrates the advantages of real-time dead reckoning by visual inertial odometry and global constraints of the semantic map. The filter state is updated through adaptive observation covariance, which effectively overcomes the problems of environmental changes, feature degradation and cumulative errors in complex urban scenarios, and achieves high-precision, high-robustness and real-time continuous navigation and positioning output in the absence of BeiDou signal. Attached Figure Description
[0097] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0098] Figure 1 This is a flowchart illustrating a visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment, provided by the present invention.
[0099] Figure 2 This is the overall framework diagram provided by the present invention;
[0100] Figure 3 This invention employs a priori semantic 3D description map;
[0101] Figure 4 This invention proposes a convolutional attention mechanism module in the semantic segmentation network for urban traffic elements.
[0102] Figure 5 This invention proposes a multi-branch feature fusion module in the semantic segmentation network for urban traffic elements.
[0103] Figure 6 These are the semantic segmentation, feature point extraction, and matching results of urban road elements obtained by this invention;
[0104] Figure 7 It is a matching graph of prior map reprojection features and online feature points;
[0105] Figure 8 This is a comparison of the positioning results between the method of this invention and mainstream navigation methods. Detailed Implementation
[0106] To better understand this application, various aspects of this application will be described in more detail with reference to the accompanying drawings. It should be understood that these detailed descriptions are merely illustrative of exemplary embodiments of this application and are not intended to limit the scope of this application in any way. Throughout the specification, the same reference numerals refer to the same elements. The expression "and / or" includes any and all combinations of one or more of the associated listed items.
[0107] In the accompanying drawings, the size, dimensions, and shapes of the elements have been slightly adjusted for ease of illustration. The drawings are for illustrative purposes only and are not strictly to scale. As used herein, the terms “approximately,” “about,” and similar terms are used to indicate approximation, not degree, and are intended to illustrate inherent deviations in measured or calculated values that will be recognized by one of ordinary skill in the art. Furthermore, the order in which the steps are described in this application does not necessarily indicate the order in which these steps occur in actual operation, unless otherwise expressly defined or deduced from the context.
[0108] It should also be understood that expressions such as "comprising," "including," "having," "containing," and / or "comprising" are open-ended rather than closed-ended expressions in this specification, indicating the presence of the stated features, elements, and / or components, but not excluding the presence of one or more other features, elements, components, and / or combinations thereof. Furthermore, when expressions such as "at least one of..." appear after a list of listed features, they modify the entire list of features, not just individual elements in the list. Additionally, when describing embodiments of this application, the word "may" is used to mean "one or more embodiments of this application." And the term "exemplary" is intended to refer to examples or illustrations.
[0109] Unless otherwise specified, all terms used herein (including engineering and technical terms) shall have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It should also be understood that, unless expressly stated herein, terms defined in common dictionaries shall be interpreted as having the meaning consistent with their meaning in the context of the relevant art, and not as having an idealized or overly formalized meaning.
[0110] It should be noted that, where there is no conflict, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.
[0111] refer to Figures 1-8This embodiment provides a visual inertial navigation and positioning method constrained by a semantic description map in urban BeiDou-denied environments. It addresses the issue that traditional visual inertial odometry, facing urban BeiDou-denied spaces, suffers from positioning drift due to accumulated errors, leading to global positioning failure and failing to meet long-term global positioning and rapid repositioning requirements. This invention relies on a priori lightweight semantic 3D description map and constructs a Map-VIO tightly coupled navigation and positioning framework through an extended Kalman filter. This invention provides a visual inertial navigation and positioning method constrained by a semantic description map in urban BeiDou-denied environments to achieve long-term global navigation and positioning for carriers in BeiDou-denied spaces. The specific processing steps are as follows:
[0112] Figure 1 This is a flowchart illustrating a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment, according to an embodiment of the present invention, comprising:
[0113] Step 100: Collect visual inertial data, perform visual inertial odometry prior propagation, and determine the triggering of prior map constraints based on the positioning status;
[0114] Step 200: Use deep learning technology to segment the semantic elements of online visual images in real time and extract feature points to construct online visual feature vectors;
[0115] Step 300: Perform multiple consistency matching by combining the online observation feature vector and the map point feature vector to obtain the comprehensive matching confidence.
[0116] Step 400: Determine the observation covariance of map observation factors based on the comprehensive matching confidence and the basic pixel measurement covariance, and then derive the overall map observation covariance;
[0117] Step 500: Construct a Map-VIO compact combination navigation model, perform filtering updates, and obtain the final accurate and reliable navigation and positioning results.
[0118] Specifically, such as Figure 2 The overall framework diagram shown illustrates the general concept of this invention as follows:
[0119] First, a spatiotemporally synchronized multi-sensor integrated acquisition platform is used to acquire stereo images and IMU data in urban BeiDou denied scenarios, and data preprocessing is performed. Unit conversion and integrity checks are performed on the IMU's angular velocity and acceleration data, and angular velocity and acceleration noise used for filter measurement updates are initialized according to the IMU calibration parameters. Continuous VIO positioning is carried out based on MSCKF, and the prior map constraints are determined based on the carrier pose covariance.
[0120] Triggering prior map constraints refers to establishing reprojection error by performing multiple feature matching with online visual features based on the provided prior navigation semantic description map, according to its global coordinates and feature description vectors, and then constructing map factors for subsequent construction of the Map-VIO navigation model;
[0121] Secondly, a semantic segmentation network for urban traffic elements was designed based on the VGG16-Unet network to acquire traffic elements with uniform spatial distribution and good geometric configuration in urban environments. Superpoint was used for feature point extraction, and Lightglue was used for feature matching. The combination of these two methods effectively acquires lightweight urban traffic elements and improves the feature detection capability of traffic elements.
[0122] Based on the stereo geometric relationship and local feature invariance of online feature points, feature trajectories are defined again, and temporal feature vectors are statistically analyzed. LSTM is used to estimate the matching confidence of each feature trajectory as a quality factor of online feature points to improve the robustness of visual features.
[0123] Then, by combining the online visual observation feature vector with the map point feature vector, multiple consistency matching is performed, including spatial geometry, semantics, viewpoint consistency, and descriptor constraints, to obtain the comprehensive matching confidence of map factors. This confidence is used as a map factor that can be used for filtering optimization, and then the overall map observation covariance is derived.
[0124] Finally, by integrating prior maps, INS, and visual measurement models, a Map-VIO compact combination navigation model is constructed, and filtering updates are performed to obtain the final accurate and reliable navigation and positioning results.
[0125] In one embodiment, visual-inertial data is collected to perform visual-inertial odometry prior propagation, and prior map constraints are triggered based on the positioning status. The specific implementation process includes:
[0126] Based on the MSCKF theory, visual-inertial odometry (VIO) is used to track the carrier coordinate system with the IMU as the origin. Relative to global navigation coordinate system The camera pose is used to propagate the system state and covariance. When a new image is observed, the camera pose estimate is augmented into the system state vector. Therefore, the system state vector includes the IMU state vector. and Given N camera poses, when a new image participates in the system localization at time k, the filter state vector will have N camera poses. Therefore, the system state vector can be expressed as:
[0127] (1)
[0128] in, This includes the carrier's pose, velocity, triaxial acceleration, and angular velocity offset. and , which are the estimated values of the camera pose;
[0129] Then, the system error state vector is defined. ,in, .
[0130] During state vector propagation, the expectation operator is used to represent the IMU state estimation during propagation as follows: (2)
[0131] in, For system noise, The covariance matrix is This value can be obtained by calibration using Alla variance. and These represent the system state transition matrix and gain matrix, respectively.
[0132] In practical applications, the state estimation of the IMU can be represented by the fourth-order Runge-Kutta numerical integral of the state propagation equation, thus yielding the discretized state transition matrix and gain matrix as follows:
[0133] (3)
[0134] (4)
[0135] in, Let the continuous-time noise covariance of the system be represented, then the pre-integral covariance of the IMU state can be expressed as: .
[0136] Based on the covariance propagation law of the Extended Kalman Filter (EKF), and combining the IMU and visual state vectors, the system state covariance can be defined as:
[0137] (5)
[0138] in, It is in IMU status. covariance matrix It is camera pose estimation The covariance matrix, The correlation between IMU state and camera pose estimation error.
[0139] After the IMU and image states are updated at the new time step, the covariance matrix of the propagation state can be expressed as:
[0140] (6)
[0141] When recording new images, the system combines the corresponding feature points from the binocular images with the camera pose to obtain the 3D coordinates of the feature points in the world coordinate system, and then projects them onto the camera coordinate system to obtain:
[0142] (7)
[0143] in, and They represent The pose of the left camera in the global n-frame at any given time. , In order The rotation matrix and translation vector from the left camera to the right camera at each time step. , These represent the positions of the feature points in the left and right camera systems, respectively.
[0144] By reprojecting the camera coordinates, a visual observation model can be constructed. Then, by linearizing the observation model, the visual measurement residuals can be constructed. .
[0145] To avoid the same feature point Because the superimposed observations at different times affect the measurement residuals, the residuals are projected onto... Left null space:
[0146] (8)
[0147] By further superimposing multiple observations of all feature points, we can obtain .
[0148] To reduce the computational complexity of EKF updates, QR decomposition yields ,Pick For EKF updates, there are , Then calculate the Kalman gain. and update the system error status. and system status Simultaneously update the system state covariance matrix. This completes the state propagation.
[0149] Based on the covariance of the carrier pose in the system state and The carrier positioning uncertainty can be defined. And based on the uncertainty results at different times, the uncertainty growth rate is statistically analyzed. Therefore, the average visual residual of the current VIO system is defined as:
[0150] (9)
[0151] in, and These are the position and attitude sub-state covariances, respectively. , , and These are the pixel coordinates of the visual feature observation and the pixel coordinates of the feature point reprojection, respectively. For pixel observation noise covariance; For pixel observation noise covariance;
[0152] comprehensive , and Evaluation factors can be used to construct system positioning degradation indicators:
[0153] (10)
[0154] in, The chi-square threshold of the two-dimensional pixel residual at the confidence level; , For normalization scale, such as Figure 3 The prior semantic 3D description map is used.
[0155] In one embodiment, deep learning technology is used to segment semantic elements of online visual images in real time and extract feature points to construct online visual feature vectors. The specific implementation process includes:
[0156] First, a semantic segmentation network for urban environmental traffic elements was designed based on the VGG16-Unet network to acquire binocular traffic elements with uniform spatial distribution and good geometric configuration in urban environments. and .like Figure 4 As shown, a Convolutional Block Attention Module (CBAM) is embedded at the jump connection between the encoder and decoder of the network to adaptively weight the channel and spatial dimensions to enhance the attention to the details of traffic road elements. In the decoder part, a Multi-branch Feature Fusion Module (MBFF) is used to fuse receptive field features of different scales to fully capture local details and alleviate feature instability caused by scale changes and illumination differences.
[0157] The CBAM module consists of two concatenated stages: Channel Attention (CA) and Spatial Attention (SA), as shown in the diagram. Figure 4 As shown, given input features F, they are first subjected to global averaging and max pooling, and then fused using a shared multilayer perceptron (MLP) to obtain channel weights. And multiply by F to get Then, the spatial weights are obtained by convolution and activation of the spliced results of different pooling methods through SA. The final output feature map This two-level mechanism simultaneously highlights semantically salient channels and key spatial locations, improving the segmentation robustness of low-contrast traffic elements. The overall computational process is as follows:
[0158] (11)
[0159] (12)
[0160] in, It is the Sigmoid activation function. For convolution operations with k=7, This is for splicing operations.
[0161] Furthermore, to extract traffic element contours more accurately and alleviate the problem of limited scale in single-path feature representation, this paper introduces MBFF in the decoder stage, the structure of which is shown below. Figure 5 First, the upsampled features of the k-th decoding layer are concatenated with the skip connection features of the corresponding encoder along the channel dimension, denoted as the input. Subsequently, three convolutional branches with complementary receptive fields and expressive functions are constructed in parallel; after the results of the three branches are fused, ReLU activation is applied to obtain the module output. .
[0162] The multi-branch cooperative mechanism enables the network to maintain both the local discriminative ability of shallow edges and the consistency of scale semantics. The overall process described above can be represented as follows:
[0163] (13)
[0164] (14)
[0165] (15)
[0166] (16)
[0167] in, Input features for the module, , , The output features of the three branches are respectively. Output features for the module. and These represent convolution operations of size 1×1 and 3×3, respectively.
[0168] Then, Superpoint is used to... Time segmentation region and Feature points are extracted sequentially, and feature matching is performed using Lightglue to obtain pairs of feature points with the same name. And count the pixel coordinates of each feature point. semantic features and descriptor features .like Figure 6 The results of semantic segmentation, feature point extraction, and matching of urban road elements.
[0169] Based on the acquired feature point pairs Obtained by forward tracing using optical flow method The feature observation trajectory of this feature point is constructed as follows: If a feature point cannot be tracked, the feature trajectory is terminated. The descriptors corresponding to each feature point are counted sequentially. Polar geometric error Forward and backward tracking error Stereo reprojection error Tracking length Local brightness average Local gradient intensity For each trajectory Extracting temporal feature vectors LSTM is used for time-series updates, and the confidence scores corresponding to the observed trajectories of feature points are output, which are the quality scores of the online feature points. Therefore, the online feature vector of each feature point is determined as follows: .
[0170] Each feature trajectory is provided with a temporal feature vector, including:
[0171] Similarity distance is calculated based on descriptors of left and right feature points. ;
[0172] Using the calibration parameters of a binocular camera, epipolar line correction was performed on the left and right images, and the epipolar geometric errors of corresponding points on the left and right sides were statistically analyzed. ;
[0173] Based on the corresponding point pairs of inter-frame features, construct the forward and backward tracking error. ;
[0174] Through multi-view Figure 3 Feature points obtained by keratinization The corresponding 3D coordinates are then used to reproject the point onto the image using the pose and intrinsic parameters of the l-th camera, thus obtaining the predicted image point coordinates. The actual observed image point coordinates are Then construct the reprojection error ;
[0175] Tracking length of statistical feature points ;
[0176] To consider the robustness of feature points to brightness variations, a local window is taken centered on the pixel coordinates of the feature point. Statistical analysis of local brightness mean ;
[0177] To consider the robustness of feature points to variations in texture intensity, a local window is taken centered on the pixel coordinates of the feature point. Calculate the average gradient magnitude within a local window. .
[0178] In one embodiment, multiple consistency matching is performed by combining online observation feature vectors and map point feature vectors to obtain a comprehensive matching confidence score. The specific implementation process includes:
[0179] First, based on the online carrier pose information, search for a set of prior map points within 60m in front of the carrier. Each map point's feature vector is composed of global 3D coordinates. Depth descriptor Semantic categories and map point quality score Joint construction, i.e. .
[0180] Then, based on online point pixel coordinates Candidate map points are obtained through projection gating. That is, combining the prior pose of the carrier to backproject the map candidate points onto the image to obtain the predicted pixels. By designing projection gating, points that are relatively close can be retained: , Indicates the pixel gate radius.
[0181] Finally, multiple consistency checks are performed on the online feature points and candidate map points, including semantic consistency checks. Let the semantic label of the online feature be... Map point category is Then semantic consistency can be defined as: .
[0182] Online feature point and candidate map point descriptor consistency determination, assuming the current image feature descriptor is... Map point descriptors are Define features to describe Euclidean distance .
[0183] Based on the ray directions constructed from map points and camera centers, and the back-projected ray directions constructed from online features and image principal points, viewpoint consistency is determined. Then, based on the camera pose, the map points are... Transform to camera coordinate system The corresponding unit predicts the line of sight direction as follows: Let the online pixels be... Combined with camera internal parameters The projection direction of the online features in the camera coordinate system can be obtained as follows:
[0184] (17)
[0185] The line-of-sight consistency determination between online pixels and map points is then performed as follows: , , Figure 7 The matching graph between prior map reprojection features and online feature points is shown.
[0186] In one embodiment, the observation covariance of map observation factors is determined based on the comprehensive matching confidence and the basic pixel measurement covariance, and then the overall map observation covariance is derived. The specific implementation process includes:
[0187] Combining semantic consistency, descriptor consistency, line-of-sight consistency, and reprojection geometric consistency, the map observation confidence level is defined as:
[0188] (18)
[0189] in, , .
[0190] Considering pixel measurement error, the fundamental covariance can be written as Furthermore, by comprehensively considering the confidence level of map observations, the covariance of map observation factors is determined as follows: Then a single online feature point With map points The constructed map observation factors are: And the contribution of a single map matching to the information matrix of camera pose is derived as follows:
[0191] (19)
[0192] in, , .
[0193] Furthermore, the information matrix contributions and state covariances of all matched online features and map points to the camera pose in a single frame image can be derived as follows:
[0194] (20)
[0195] (twenty one)
[0196] in, This represents the index of all matched online point and map point pairs. is the damping coefficient.
[0197] In one embodiment, a Map-VIO compact combination navigation model is constructed, filtering updates are performed, and the final accurate and reliable navigation and positioning results are obtained. The specific implementation process includes:
[0198] Camera pose based on VIO prediction and Projected candidate map feature points Corresponding pixel coordinates for:
[0199] (twenty two)
[0200] in, , It is a rotation matrix from the carrier system to the camera system.
[0201] Combining ideal pixel coordinates of online visual feature points and measurement noise Determine the observed pixels This leads to the derivation of map measurement errors:
[0202] (twenty three)
[0203] Simultaneously, the measurement equation is constructed based on map factors as follows:
[0204] (twenty four)
[0205] in, It is the Jacobian matrix describing the relationship between pixel error observations and error state variables, i.e. The partial derivative with respect to the carrier camera pose is specifically expressed as:
[0206] (25)
[0207] in, , Camera coordinates The partial derivative with respect to the carrier pose is obtained using perturbation analysis. .
[0208] In summary, the observation matrix constrained by the feature map can be extended to the VIO system measurement model to construct the measurement equations for the Map-VIO navigation model. The specific expansion is as follows:
[0209] (26)
[0210] When performing multi-state constraint updates, the residual vector can be computed. , And the corresponding feature measurement matrix, and superimposed into a single vector. Filtering is performed to solve the problem using QR decomposition. Then, EKF is performed to calculate the Kalman gain. .
[0211] Finally, the state correction vector And update the state covariance matrix to obtain an accurate and reliable pose.
[0212] (27)
[0213] in, This represents the dimension of the covariance matrix.
[0214] Figure 8 The diagram shows the positioning trajectory error statistics of the method of this invention and the current classic combined navigation method in urban BeiDou denied space. The statistics show that the positioning result of the method of this invention is the best, with better accuracy and reliability, and can provide a reliable technical reference for navigation in urban BeiDou denied scenarios.
[0215] According to an embodiment of this application, a visual inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou denied environment achieves continuous and reliable positioning and rapid repositioning in urban BeiDou denied scenarios through position state estimation, urban traffic semantic element segmentation and feature extraction, matching confidence evaluation, and Map-VIO compact combination navigation and positioning model construction. Compared with traditional methods, it significantly improves positioning continuity and repositioning rate, and is suitable for autonomous navigation tasks in BeiDou denied scenarios.
[0216] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the invention. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A visual-inertial navigation and positioning method constrained by a semantic description map in an urban BeiDou-denied environment, characterized in that, include: Collect visual inertial data and perform visual inertial odometry prior propagation; determine the triggering of prior map constraints based on the positioning status. Deep learning technology is used to segment semantic elements of online visual images in real time and extract feature points to construct online visual feature vectors. Multiple consistency matching is performed by combining online observation feature vectors with map point feature vectors to obtain comprehensive matching confidence. The observation covariance of map observation factors is determined based on the comprehensive matching confidence and the basic pixel measurement covariance, and then the overall map observation covariance is derived. Construct a Map-VIO compact combination navigation model, perform filtering updates, and obtain the final accurate and reliable navigation and positioning results.
2. The visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment as described in claim 1, characterized in that, Visual inertial data is collected to perform visual inertial odometry. Based on the positioning status, prior map constraints are triggered, including: A spatiotemporally synchronized visual inertial data acquisition platform was used to acquire visual and INS data in urban BeiDou denied environments. The visual data included stereo images, and the INS data included acceleration and angular velocity data measured in real time by the inertial measurement unit. Based on the MSCKF theory, visual-inertial odometry (VIO) is carried out, which tracks the carrier coordinate system with the IMU as the origin. Relative to global navigation coordinate system The camera pose is used to propagate the system state and covariance. When a new image is observed, the camera pose estimate is augmented into the system state vector, which then includes the IMU state vector. and Given N camera poses, when a new image participates in the system localization at time k, the filter state vector will have N camera poses. Therefore, the system state vector can be expressed as: in, This includes the carrier's pose, velocity, triaxial acceleration, and angular velocity offset. and , where are the estimated values of the camera pose, and the system error state vector is defined accordingly. ,in, During state vector propagation, the expectation operator is used to represent the IMU state estimation during propagation as follows: in, For system noise, The covariance matrix is This value can be obtained by calibration using Alla variance. and Let represent the system state transition matrix and gain matrix, respectively. In practical applications, the state estimation of the IMU can be represented by the fourth-order Runge-Kutta numerical integral of the state propagation equation, thus yielding the discretized state transition matrix and gain matrix as follows: in, Let the continuous-time noise covariance of the system be represented, then the pre-integral covariance of the IMU state can be expressed as: Based on the covariance propagation law of the Extended Kalman Filter (EKF), and combining the IMU and visual state vectors, the system state covariance can be defined as: in, It is in IMU status. covariance matrix It is camera pose estimation The covariance matrix, To establish the correlation between IMU state and camera pose estimation error, the covariance matrix of the propagation state can be expressed as follows after the IMU and image states are updated at a new time: When recording new images, the system combines the corresponding feature points from the binocular images with the camera pose to obtain the 3D coordinates of the feature points in the world coordinate system, and then projects them onto the camera coordinate system to obtain: in, and They represent The pose of the left camera in the global n-frame at any given time. , In order The rotation matrix and translation vector from the left camera to the right camera at each time step. , These represent the positions of the feature points in the left and right camera systems, respectively. By reprojecting the camera coordinates, a visual observation model can be constructed, and the observation model can be linearized to construct the visual measurement residuals. To avoid the same feature point Because the superimposed observations at different times affect the measurement residuals, the residuals are projected onto... The left null space is: By further superimposing multiple observations of all feature points, we can obtain To reduce the computational complexity of EKF updates, QR decomposition yields ,Pick For EKF updates, then there are , Then calculate the Kalman gain. and update the system error status. and system status Simultaneously update the system state covariance matrix. State propagation is completed based on the covariance of the carrier pose in the system state. and The carrier positioning uncertainty can be defined. And based on the uncertainty results at different times, the uncertainty growth rate is statistically analyzed. Therefore, the average visual residual of the current VIO system is defined as: in, and These are the position and attitude sub-state covariances, respectively. , , and These are the pixel coordinates of the visual feature observation and the pixel coordinates of the feature point reprojection, respectively. For pixel observation noise covariance; For pixel observation noise covariance; comprehensive , and Evaluation factors can be used to construct system positioning degradation indicators: in, The chi-square threshold of the two-dimensional pixel residual at the confidence level; , This is the normalization scale.
3. The visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment as described in claim 1, characterized in that, Deep learning techniques are used to segment semantic elements of online visual images in real time and extract feature points to construct online visual feature vectors, including: First, a semantic segmentation network for urban environmental traffic elements was designed based on the VGG16-Unet network to acquire traffic elements with uniform spatial distribution and good geometric configuration in stereo images. The network embeds a convolutional attention mechanism module at the jump connection between the encoder and decoder to adaptively weight the channel and spatial dimensions to improve the attention to the details of traffic road elements. In the decoder part, a multi-branch feature fusion module is used to fuse receptive field features of different scales to fully capture local details and alleviate feature instability caused by scale changes and illumination differences. Then, the Superpoint method is used to... Segmented regions of the initial binocular image at time step and Feature points are extracted sequentially, and feature matching is performed using Lightglue to obtain pairs of feature points with the same name. And count the pixel coordinates of each feature point. semantic features and descriptor features ; Based on the acquired feature point pairs Obtained by forward tracing using optical flow method The feature observation trajectory of this feature point is constructed as follows: If a feature point cannot be tracked, the feature trajectory is terminated, and the descriptors corresponding to the feature points are counted sequentially. Polar geometric error Forward and backward tracking error Stereo reprojection error Tracking length Local brightness mean Local gradient intensity ; and then for each trajectory Extracting temporal feature vectors LSTM is used for time-series updates, and the confidence scores corresponding to the observed trajectories of feature points are output, which are the quality scores of the online feature points. This allows us to determine the online feature vector of each feature point. .
4. The visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment as described in claim 1, characterized in that, By combining online observation feature vectors with map point feature vectors, multiple consistency matching is performed to obtain a comprehensive matching confidence score, including: First, based on the online carrier pose information, search for a set of prior map points within 60m in front of the carrier. Each map point's feature vector is composed of global 3D coordinates. Depth descriptor Semantic categories and map point quality score Joint construction, i.e. ; Then, based on online point pixel coordinates Candidate map points are obtained through projection gating. That is, combining the prior pose of the carrier to backproject the map candidate points onto the image to obtain the predicted pixels. By designing projection gating, points that are relatively close can be retained: , Indicates the pixel gate radius; Finally, multiple consistency checks are performed on the online feature points and candidate map points, including semantic consistency checks. Let the semantic label of the online feature be... Map point category is Then semantic consistency can be defined as: ; Online feature point and candidate map point descriptor consistency determination, assuming the current image feature descriptor is... Map point descriptors are Define features to describe Euclidean distance ; Based on the ray directions constructed from map points and camera centers, and the back-projected ray directions constructed from online features and image principal points, viewpoint consistency is determined. Then, based on the camera pose, the map points are... Transform to camera coordinate system The corresponding unit predicts the line of sight direction as follows: Let the online pixels be Combined with camera internal parameters The projection direction of the online features in the camera coordinate system can be obtained as follows: The line-of-sight consistency determination between online pixels and map points is then performed as follows: , .
5. The visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment according to claim 1, characterized in that, The observation covariance of map observation factors is determined based on the comprehensive matching confidence level and the covariance of basic pixel measurements, and then the overall map observation covariance is derived, including: Based on the online feature vectors and map point feature vectors obtained in the previous step, and through semantic consistency, descriptor consistency, line-of-sight consistency, and reprojection geometric consistency, the map observation confidence is defined as: in, , ; Considering pixel measurement error, the fundamental covariance can be written as Furthermore, by comprehensively considering the confidence level of map observations, the covariance of map observation factors is determined as follows: Then a single online feature point With map points The constructed map observation factors are: And the contribution of a single map matching to the information matrix of camera pose is derived as follows: in, , Furthermore, the information matrix contributions and state covariances of all matched online features and map points to the camera pose in a single frame image can be derived as follows: in, This represents the index of all matched online point and map point pairs. is the damping coefficient.
6. The visual inertial navigation and positioning method with semantic description map constraints in an urban BeiDou denied environment according to claim 1, characterized in that, Construct a Map-VIO compact combination navigation model, perform filtering updates, and obtain the final accurate and reliable navigation and positioning results, including: Camera pose based on VIO prediction and Projected candidate map feature points Corresponding pixel coordinates for: in, , It is the rotation matrix from the carrier system to the camera system; Combining ideal pixel coordinates of online visual feature points and measurement noise Determine the observed pixels This leads to the derivation of map measurement errors: Simultaneously, the measurement equation is constructed based on map factors as follows: in, It is the Jacobian matrix describing the relationship between pixel error observations and error state variables, i.e. The partial derivative with respect to the carrier camera pose is specifically expressed as: in, , Camera coordinates The partial derivative with respect to the carrier pose is obtained using perturbation analysis. ; In summary, the observation matrix constrained by the feature map can be extended to the VIO system measurement model to construct the measurement equations for the Map-VIO navigation model. The specific expansion is as follows: When performing multi-state constraint updates, the residual vector can be computed. , And the corresponding feature measurement matrix, and superimposed into a single vector. Filtering is performed to solve the problem using QR decomposition. Then, EKF is performed to calculate the Kalman gain. ; Finally, the state correction vector And update the state covariance matrix, thereby solving for the accurate and reliable pose: in, This represents the dimension of the covariance matrix.