An inertial navigation-map fusion positioning method based on environmental landmark detection

Abstract

The application discloses a kind of inertial navigation-map fusion positioning methods based on environmental landmark detection, belong to fusion positioning technical field, this method includes obtaining global navigation satellite system GNSS observation data, and establishes landmark database;According to global navigation satellite system GNSS observation data, the initialization of inertial navigation system is completed, and according to inertial navigation system, system state prediction result is obtained;Satellite image data is obtained, and according to satellite image data and system state prediction result, through feature extraction algorithm, obtain key frame set;According to key frame set and landmark database, obtain the effectiveness result set of key frame based on matching;According to inertial navigation system and the effectiveness result set of key frame based on matching, the current pose of vehicle is obtained, and the inertial navigation-map fusion positioning based on environmental landmark detection is completed.The application solves the problem that the existing fusion positioning technology requires tracking satellite quantity and is easily disturbed by weather and environment.

Description

Technical Field

[0001] This invention belongs to the field of fusion positioning technology, and particularly relates to an inertial navigation-map fusion positioning method based on environmental landmark detection. Background Technology

[0002] One existing technology leverages the complementarity of Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and vision to achieve continuous and accurate navigation in urban environments through a tightly coupled integration method. However, this tightly coupled method introduces all raw GNSS measurement data into the Visual Inertial Navigation System (VINS), making the system dependent on the number of tracking satellites. Specifically, if fewer than four satellites are tracking during system initialization, the system performance deteriorates as the number of satellites decreases, and degenerates into VINS when no satellites are tracking, thus re-enacting the trajectory drift or open-loop problems inherent in the original VIO.

[0003] One existing technique utilizes real-time dynamic information from the RTK (Real-Time Kinematics) system of the GNSS (Global Navigation Satellite System) to perform Earth rotation compensation on the IMU (Inertial Measurement Unit) to improve the accuracy of the Advanced Inertial Measurement Unit (AMU). However, while the GNSS-assisted initialization method does not show dependence on the number of tracking satellites after only referencing GNSS RTK data, its reliance on IMU and visual data means that it may be affected by weather and other environmental factors in complex outdoor scenarios, leading to drift problems.

[0004] One existing technique fuses raw GNSS carrier phase and pseudorange measurements, IMU data, and visual features directly at the observation level using methods such as a centralized extended Kalman filter (EKF), thereby fully utilizing multi-sensor information and eliminating potential outlier measurements. However, the EKF fusion algorithm has high requirements for synchronization and calibration between sensors; otherwise, accurate fusion results cannot be guaranteed. Furthermore, the more complex fusion algorithm leads to higher computational resource consumption compared to the previous two methods. Summary of the Invention

[0005] To address the aforementioned shortcomings in existing technologies, this invention provides an inertial navigation-map fusion positioning method based on environmental landmark detection, which solves the problems of existing fusion positioning technologies requiring a certain number of tracking satellites and being susceptible to weather and environmental interference.

[0006] To achieve the aforementioned objectives, the present invention employs the following technical solution: an inertial navigation-map fusion positioning method based on environmental landmark detection, comprising the following steps:

[0007] S1. Acquire GNSS observation data from the Global Navigation Satellite System and establish a landmark database;

[0008] S2. Based on GNSS observation data from the Global Navigation Satellite System, initialize the inertial navigation system and obtain system state prediction results based on the inertial navigation system.

[0009] S3. Acquire satellite image data, and based on the satellite image data and system status prediction results, obtain a set of key frames through feature extraction algorithms;

[0010] S4. Based on the keyframe set and the landmark database, obtain the set of keyframe matching-based validity results;

[0011] S5. Based on the set of valid results of keyframe matching from the inertial navigation system and keyframes, obtain the current pose of the vehicle and complete the inertial navigation-map fusion positioning based on environmental landmark detection.

[0012] The beneficial effects of this invention are as follows: feature extraction and association based on environmental landmarks can correct and rectify drift and error accumulation problems of inertial navigation systems through the location information of landmarks; this invention can maintain stability in complex environments with different weather conditions both indoors and outdoors. Since environmental landmarks are often easy to identify and have many feature points, the positioning results generated by the inertial navigation odometry will be more reliable after the environmental landmark feature points in the image data are identified.

[0013] Further, step S2 specifically includes:

[0014] S201. Maintain the vehicle's straight-line movement for a period of time to keep the inertial navigation system in an absolute attitude;

[0015] S202. Based on step S201, Earth rotation compensation is performed in the fused inertial measurement unit (IMU) to complete the initialization of the inertial navigation system.

[0016] S203. Obtain the system state prediction results based on the inertial measurement unit (IMU) of the inertial navigation system.

[0017] The beneficial effects of the aforementioned further solutions are as follows: by utilizing Global Navigation Satellite System (GNSS) observation data and initializing the inertial navigation system, more accurate navigation positioning and trajectory tracking can be provided, thereby improving navigation accuracy. Furthermore, compensation based on the Earth's rotation using the IMU can reduce errors caused by the Earth's rotation in the inertial navigation system, improving the accuracy and stability of the navigation system.

[0018] Furthermore, the expression for the system state prediction result in step S203 is:

[0019]

[0020] in, This refers to the system state prediction results; The derivative information of the predicted pose matrix; The derivative information for the predicted velocity; The derivative information for the predicted position error; It is a vector product; The Earth's rotation rate in the world coordinate system; This is the pose matrix predicted in the previous test; This is the rotation matrix of the vehicle coordinate system relative to the world coordinate system; The error measured by the gyroscope; f b This refers to the error measured by the accelerometer; This refers to the velocity information from the previous prediction; g w δ is the gravity vector; δ is the variational symbol.

[0021] The beneficial effects of the above-mentioned further scheme are as follows: through the above-mentioned system state prediction results, information such as the vehicle's position, velocity and attitude can be obtained in real time, providing initial data for the subsequent maximum a posteriori estimation of the state vector.

[0022] Further, step S3 specifically includes:

[0023] S301. Obtain satellite image data using the Global Navigation Satellite System (GNSS);

[0024] S302. Obtain the current image frame based on satellite image data;

[0025] S303. Use the Harris corner detector to obtain environmental landmark feature points of the current image frame;

[0026] S304. Based on the system state prediction results and the environmental landmark feature points of the current image frame, use the Lukas-Kanade optical flow algorithm to perform feature tracking and obtain the optical flow velocity vector;

[0027] S305. Multiply the optical flow velocity vector by the frame interval of the image data to obtain the relative position of the environmental landmarks and the vehicle in the current image frame;

[0028] S306. Based on the environmental landmark feature points of the current image frame, determine whether new environmental landmark feature points have appeared. If yes, proceed directly to step S307; otherwise, return to step S302 and obtain the next image frame.

[0029] S307. Based on the relative positions of the environmental landmarks and the vehicle in the current image frame, determine whether the average parallax change is less than the first threshold. If so, insert the current image frame as a key frame into the key frame set; otherwise, return to step S302 to obtain the next image frame.

[0030] The beneficial effects of the above-mentioned further scheme are: the Harris corner detector and Lukas-Kanade optical flow algorithm make full use of image data for the extraction and tracking of environmental landmark feature points, and provide initial data for the maximum a posteriori estimation of the subsequent state vector while determining key frames.

[0031] Further, step S4 specifically includes:

[0032] S401. Based on the keyframe set, obtain the first keyframe that successfully matches the landmark data in the landmark database, and use the first keyframe as the reference keyframe.

[0033] S402. Obtain the next keyframe from the keyframe set as the current keyframe;

[0034] S403. Determine whether there is any landmark data in the landmark database that successfully matches the environmental landmark feature points of the current keyframe. If yes, proceed directly to step S404. Otherwise, determine that the current keyframe is invalid, obtain the validity result of the current keyframe, and return to step S402 to obtain the next keyframe.

[0035] S404. Based on the system state prediction results of the reference keyframe, perform triangulation between the environmental landmark feature points of the reference keyframe and the environmental landmark feature points of the current keyframe to obtain the depth of the environmental landmark in the current keyframe.

[0036] S405. Update the reference keyframe to the current keyframe;

[0037] S406. Determine whether the depth of the environmental landmark in the current keyframe is less than the second threshold. If yes, proceed directly to step S407. Otherwise, determine that the current keyframe is invalid, obtain the validity result of the current keyframe, and return to step S402 to obtain the next keyframe.

[0038] S407. Based on the keyframe set, perform depth calculation on several keyframes before and after the current keyframe to obtain the depth results of the previous and next frames.

[0039] S408. Based on the depth results of the previous and next frames, determine whether the environmental landmark feature points of the current key frame are outliers. If so, determine that the current key frame is invalid, obtain the validity result of the current key frame, and return to step S402 to obtain the next key frame. Otherwise, determine that the current key frame is a valid frame and obtain the validity result of the current key frame.

[0040] S409. Based on the validity results of each keyframe, obtain the set of keyframe validity results based on matching.

[0041] The beneficial effects of the aforementioned further scheme are as follows: by triangulating the feature points of the reference keyframe and the current keyframe, the depth information of environmental landmarks in the current keyframe can be obtained. This helps to provide more accurate relative position and pose estimation, thereby improving the overall localization and navigation performance of the method. Furthermore, the depth-based validity judgment further enhances the scalability and applicability of the overall method.

[0042] Further, step S5 specifically includes:

[0043] S501. Based on the set of valid results of keyframe matching, set the weights of visual and GNSS pseudorange information.

[0044] S502. Based on the weights of visual and GNSS pseudorange information, obtain the state vector that satisfies the maximum a posteriori estimation.

[0045] S503. Based on the position and attitude quaternion information in the world coordinate system of the state vector that satisfies the maximum a posteriori estimation, the current pose of the vehicle is obtained, and the inertial navigation-map fusion positioning based on environmental landmark detection is completed.

[0046] The beneficial effects of the above-mentioned further solutions are as follows: by fusing measurement data from different sensors (visual and inertial navigation systems), the accuracy and robustness of the positioning results are improved. Furthermore, the inertial navigation-map fusion positioning method based on environmental landmark detection combines the measurement results of the inertial navigation system with information from environmental landmarks, which can improve positioning accuracy and robustness even in the absence of GNSS signals or visual obstruction.

[0047] Furthermore, in step S501, if the keyframe validity result is a valid frame or the GNSS signal is briefly lost, the dynamic weight of the visual residual is set to 3 and the dynamic weight of the GNSS residual is set to 0.8; otherwise, the dynamic weight of the visual residual is set to 0.8 and the dynamic weight of the GNSS residual is set to 3.

[0048] The beneficial effects of the above-mentioned further scheme are as follows: by setting weights, visual and GNSS pseudorange information can be used more rationally in the positioning process according to different scenarios and reliability considerations, thereby improving the accuracy and stability of positioning.

[0049] Further, the expression for the state vector satisfying the maximum a posteriori estimation in step S502 is:

[0050]

[0051] Where, χ * To satisfy the maximum a posteriori estimation of the state vector; argmin χ (·) represents the value of χ when it is minimized; r pH is the prior residual matrix from the previous sliding window; p χ is the prior Jacobian matrix from the previous sliding window; χ is the state vector, containing the state at each time point during IMU measurement, the Yaw angle bias, and the reciprocal of the landmark depth; k is the time point number in the visual sliding window; n is the number of time points in the visual sliding window; r I Pre-integration for IMU; This represents the system state at the (k-1)th time node in the visual sliding window; The square of the Mahalanobis norm for IMU pre-integration; α is the covariance of the IMU pre-integration at the (k-1)th and kth time points; VM The dynamic weights of the visual residuals; l represents the landmarks in the map; L represents the map landmarks in the sliding window; r V For visual reprojection; The system state is derived from keyframes i and j; i and j are both keyframes containing landmark l. The square of the Mahalanobis norm of the visual reprojection; To obtain the covariance of visual reprojection at keyframes i and j; α GM The dynamic weights of the GNSS residuals; r G The residual factor for GNSS-RTK; This represents the system state during GNSS-RTK measurements within a sliding window. is the square of the Mahalanobis norm of the GNSS-RTK residual factor; m is the number of GNSS-RTK measurements in the sliding window; h is the GNSS-RTK measurement number.

[0052] The beneficial effects of the above-mentioned further solutions are: the comprehensive utilization of vision, inertial navigation and landmark information improves the accuracy, robustness and reliability of positioning, enabling this fusion positioning method to cope with positioning needs in complex environments and provide more reliable positioning solutions for fields such as navigation, autonomous driving and robotics. Attached Figure Description

[0053] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0054] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0055] like Figure 1 As shown, in one embodiment of the present invention, an inertial navigation-map fusion positioning method based on environmental landmark detection includes the following steps:

[0056] S1. Acquire GNSS observation data from the Global Navigation Satellite System and establish a landmark database;

[0057] S2. Based on GNSS observation data from the Global Navigation Satellite System, initialize the inertial navigation system and obtain system state prediction results based on the inertial navigation system.

[0058] S3. Acquire satellite image data, and based on the satellite image data and system status prediction results, obtain a set of key frames through feature extraction algorithms;

[0059] S4. Based on the keyframe set and the landmark database, obtain the set of keyframe matching-based validity results;

[0060] S5. Based on the set of valid results of keyframe matching from the inertial navigation system and keyframes, obtain the current pose of the vehicle and complete the inertial navigation-map fusion positioning based on environmental landmark detection.

[0061] Step S2 specifically involves:

[0062] S201. Maintain the vehicle's straight-line movement for a period of time to keep the inertial navigation system in an absolute attitude;

[0063] S202. Based on step S201, Earth rotation compensation is performed in the fused inertial measurement unit (IMU) to complete the initialization of the inertial navigation system.

[0064] S203. Obtain the system state prediction results based on the inertial measurement unit (IMU) of the inertial navigation system.

[0065] The expression for the system state prediction result in step S203 is:

[0066]

[0067] in, This refers to the system state prediction results; The derivative information of the predicted pose matrix; The derivative information for the predicted velocity; The derivative information for the predicted position error; It is a vector product; The Earth's rotation rate in the world coordinate system; This is the pose matrix predicted in the previous test; This is the rotation matrix of the vehicle coordinate system relative to the world coordinate system; The error measured by the gyroscope; fb This refers to the error measured by the accelerometer; This refers to the velocity information from the previous prediction; g w δ is the gravity vector; δ is the variational symbol.

[0068] Step S3 specifically involves:

[0069] S301. Obtain satellite image data using the Global Navigation Satellite System (GNSS);

[0070] S302. Obtain the current image frame based on satellite image data;

[0071] S303. Use the Harris corner detector to obtain environmental landmark feature points of the current image frame;

[0072] S304. Based on the system state prediction results and the environmental landmark feature points of the current image frame, use the Lukas-Kanade optical flow algorithm to perform feature tracking and obtain the optical flow velocity vector;

[0073] S305. Multiply the optical flow velocity vector by the frame interval of the image data to obtain the relative position of the environmental landmarks and the vehicle in the current image frame;

[0074] S306. Based on the environmental landmark feature points of the current image frame, determine whether new environmental landmark feature points have appeared. If yes, proceed directly to step S307; otherwise, return to step S302 and obtain the next image frame.

[0075] S307. Based on the relative positions of the environmental landmarks and the vehicle in the current image frame, determine whether the average parallax change is less than the first threshold. If so, insert the current image frame as a key frame into the key frame set; otherwise, return to step S302 to obtain the next image frame.

[0076] Step S4 specifically involves:

[0077] S401. Based on the keyframe set, obtain the first keyframe that successfully matches the landmark data in the landmark database, and use the first keyframe as the reference keyframe.

[0078] S402. Obtain the next keyframe from the keyframe set as the current keyframe;

[0079] S403. Determine whether there is any landmark data in the landmark database that successfully matches the environmental landmark feature points of the current keyframe. If yes, proceed directly to step S404. Otherwise, determine that the current keyframe is invalid, obtain the validity result of the current keyframe, and return to step S402 to obtain the next keyframe.

[0080] S404. Based on the system state prediction results of the reference keyframe, perform triangulation between the environmental landmark feature points of the reference keyframe and the environmental landmark feature points of the current keyframe to obtain the depth of the environmental landmark in the current keyframe.

[0081] S405. Update the reference keyframe to the current keyframe;

[0082] S406. Determine whether the depth of the environmental landmark in the current keyframe is less than the second threshold. If yes, proceed directly to step S407. Otherwise, determine that the current keyframe is invalid, obtain the validity result of the current keyframe, and return to step S402 to obtain the next keyframe.

[0083] S407. Based on the keyframe set, perform depth calculation on several keyframes before and after the current keyframe to obtain the depth results of the previous and next frames.

[0084] S408. Based on the depth results of the previous and next frames, determine whether the environmental landmark feature points of the current key frame are outliers. If so, determine that the current key frame is invalid, obtain the validity result of the current key frame, and return to step S402 to obtain the next key frame. Otherwise, determine that the current key frame is a valid frame and obtain the validity result of the current key frame.

[0085] S409. Based on the validity results of each keyframe, obtain the set of keyframe validity results based on matching.

[0086] Step S5 specifically involves:

[0087] S501. Based on the set of valid results of keyframe matching, set the weights of visual and GNSS pseudorange information.

[0088] S502. Based on the weights of visual and GNSS pseudorange information, obtain the state vector that satisfies the maximum a posteriori estimation.

[0089] S503. Based on the position and attitude quaternion information in the world coordinate system of the state vector that satisfies the maximum a posteriori estimation, the current pose of the vehicle is obtained, and the inertial navigation-map fusion positioning based on environmental landmark detection is completed.

[0090] In step S501, if the keyframe validity result is a valid frame or the GNSS signal is briefly lost, the dynamic weight of the visual residual is set to 3 and the dynamic weight of the GNSS residual is set to 0.8; otherwise, the dynamic weight of the visual residual is set to 0.8 and the dynamic weight of the GNSS residual is set to 3.

[0091] The expression for the state vector satisfying the maximum a posteriori estimation in step S502 is:

[0092]

[0093] Where, χ * To satisfy the maximum a posteriori estimation of the state vector; argmin χ (·) represents the value of χ when it is minimized; r p H is the prior residual matrix from the previous sliding window; p χ is the prior Jacobian matrix from the previous sliding window; χ is the state vector, containing the state at each time point during IMU measurement, the Yaw angle bias, and the reciprocal of the landmark depth; k is the time point number in the visual sliding window; n is the number of time points in the visual sliding window; r I Pre-integration for IMU; This represents the system state at the (k-1)th time node in the visual sliding window; The square of the Mahalanobis norm for IMU pre-integration; α is the covariance of the IMU pre-integration at the (k-1)th and kth time points; VM The dynamic weights of the visual residuals; l represents the landmarks in the map; L represents the map landmarks in the sliding window; r V For visual reprojection; The system state is derived from keyframes i and j; i and j are both keyframes containing landmark l. The square of the Mahalanobis norm of the visual reprojection; To obtain the covariance of visual reprojection at keyframes i and j; α GM The dynamic weights of the GNSS residuals; r G The residual factor for GNSS-RTK; This represents the system state during GNSS-RTK measurements within a sliding window. is the square of the Mahalanobis norm of the GNSS-RTK residual factor; m is the number of GNSS-RTK measurements in the sliding window; h is the GNSS-RTK measurement number.

[0094] In this embodiment, the present invention performs fusion positioning based on dynamic weights and environmental landmarks using Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and satellite image data. This includes acquiring GNSS data and using it to initialize the assisted inertial navigation system; acquiring image data, extracting feature points of environmental landmarks using a feature extraction algorithm, and matching and associating these points with a pre-established landmark database; calculating the depth of the landmark in the keyframe based on the current keyframe and the first keyframe in which the environmental landmark was detected, and determining its validity; adjusting the weights of image data and GNSS pseudorange information based on the matching and association results, thereby adjusting the sliding window optimizer of the factor graph at the current time, and solving for the maximum a posteriori estimate to obtain the current pose.

Claims

1. An inertial navigation-map fusion positioning method based on environmental landmark detection, characterized in that, Includes the following steps: S1. Acquire GNSS observation data from the Global Navigation Satellite System and establish a landmark database; S2. Based on GNSS observation data from the Global Navigation Satellite System, initialize the inertial navigation system and obtain system state prediction results based on the inertial navigation system. S3. Acquire satellite image data, and based on the satellite image data and system status prediction results, obtain a set of keyframes through a feature extraction algorithm; step S3 specifically includes: S301. Obtain satellite image data using the Global Navigation Satellite System (GNSS); S302. Obtain the current image frame based on satellite image data; S303. Use the Harris corner detector to obtain environmental landmark feature points of the current image frame; S304. Based on the system state prediction results and the environmental landmark feature points of the current image frame, use the Lukas-Kanade optical flow algorithm to perform feature tracking and obtain the optical flow velocity vector; S305. Multiply the optical flow velocity vector by the frame interval of the image data to obtain the relative position of the environmental landmarks and the vehicle in the current image frame; S306. Based on the environmental landmark feature points of the current image frame, determine whether new environmental landmark feature points have appeared. If yes, proceed directly to step S307; otherwise, return to step S302 and obtain the next image frame. S307. Based on the relative position of the environmental landmarks and the vehicle in the current image frame, determine whether the average parallax change is less than the first threshold. If so, insert the current image frame as a key frame into the key frame set. Otherwise, return to step S302 and obtain the next image frame. S4. Based on the keyframe set and the landmark database, obtain the set of keyframe matching-based validity results; S5. Based on the set of valid results of keyframe matching from the inertial navigation system and keyframes, obtain the current pose of the vehicle and complete the inertial navigation-map fusion positioning based on environmental landmark detection.

2. The inertial navigation-map fusion positioning method based on environmental landmark detection according to claim 1, characterized in that, Step S2 specifically involves: S201. Maintain the vehicle's straight-line movement for a period of time to keep the inertial navigation system in an absolute attitude; S202. Based on step S201, Earth rotation compensation is performed in the fused inertial measurement unit (IMU) to complete the initialization of the inertial navigation system. S203. Obtain the system state prediction results based on the inertial measurement unit (IMU) of the inertial navigation system.

3. The inertial navigation-map fusion positioning method based on environmental landmark detection according to claim 2, characterized in that, The expression for the system state prediction result in step S203 is: in, This refers to the system state prediction results; The derivative information of the predicted pose matrix; The derivative information for the predicted velocity; The derivative information for the predicted position error; It is a vector product; The Earth's rotation rate in the world coordinate system; This is the pose matrix predicted in the previous test; This is the rotation matrix of the vehicle coordinate system relative to the world coordinate system; This refers to the measurement error of the gyroscope. This refers to the error measured by the accelerometer; This refers to the speed information from the previous prediction; It is the gravity vector; This is a variational symbol.

4. The inertial navigation-map fusion positioning method based on environmental landmark detection according to claim 1, characterized in that, Step S4 specifically involves: S401. Based on the keyframe set, obtain the first keyframe that successfully matches the landmark data in the landmark database, and use the first keyframe as the reference keyframe. S402. Obtain the next keyframe from the keyframe set as the current keyframe; S403. Determine whether there is any landmark data in the landmark database that successfully matches the environmental landmark feature points of the current keyframe. If yes, proceed directly to step S404. Otherwise, determine that the current keyframe is invalid, obtain the validity result of the current keyframe, and return to step S402 to obtain the next keyframe. S404. Based on the system state prediction results of the reference keyframe, perform triangulation between the environmental landmark feature points of the reference keyframe and the environmental landmark feature points of the current keyframe to obtain the depth of the environmental landmark in the current keyframe. S405. Update the reference keyframe to the current keyframe; S406. Determine whether the depth of the environmental landmark in the current keyframe is less than the second threshold. If yes, proceed directly to step S407. Otherwise, determine that the current keyframe is invalid, obtain the validity result of the current keyframe, and return to step S402 to obtain the next keyframe. S407. Based on the keyframe set, perform depth calculation on several keyframes before and after the current keyframe to obtain the depth results of the previous and next frames. S408. Based on the depth results of the previous and next frames, determine whether the environmental landmark feature points of the current key frame are outliers. If so, determine that the current key frame is invalid, obtain the validity result of the current key frame, and return to step S402 to obtain the next key frame. Otherwise, determine that the current key frame is a valid frame and obtain the validity result of the current key frame. S409. Based on the validity results of each keyframe, obtain the set of keyframe validity results based on matching.

5. The inertial navigation-map fusion positioning method based on environmental landmark detection according to claim 1, characterized in that, Step S5 specifically involves: S501. Based on the set of valid results of keyframe matching, set the weights of visual and GNSS pseudorange information. S502. Based on the weights of visual and GNSS pseudorange information, obtain the state vector that satisfies the maximum a posteriori estimation. S503. Based on the position and attitude quaternion information in the world coordinate system of the state vector that satisfies the maximum a posteriori estimation, the current pose of the vehicle is obtained, and the inertial navigation-map fusion positioning based on environmental landmark detection is completed.

6. The inertial navigation-map fusion positioning method based on environmental landmark detection according to claim 5, characterized in that, In step S501, if the keyframe validity result is a valid frame or the GNSS signal is briefly lost, the dynamic weight of the visual residual is set to 3 and the dynamic weight of the GNSS residual is set to 0.8; otherwise, the dynamic weight of the visual residual is set to 0.8 and the dynamic weight of the GNSS residual is set to 3.

7. The inertial navigation-map fusion positioning method based on environmental landmark detection according to claim 6, characterized in that, The expression for the state vector satisfying the maximum a posteriori estimation in step S502 is: in, To satisfy the state vector of the maximum a posteriori estimation; For when Minimum The value of ; This is the prior residual matrix from the previous sliding window; The prior Jacobian matrix from the previous sliding window; The state vector contains the state at each time point during IMU measurement, the yaw angle deviation, and the reciprocal of the landmark depth; Number the time nodes in the visual sliding window; The number of time nodes in the visual sliding window; Pre-integration for IMU; For the first in the visual sliding window The system state at each time point; The square of the Mahalanobis norm for IMU pre-integration; For the first The and the first Covariance of IMU pre-integration at each time point; The dynamic weights of the visual residuals; Landmarks on the map; For map landmarks in a sliding window; For visual reprojection; For keyframes and keyframes The resulting system state; and All include landmarks Keyframes; The square of the Mahalanobis norm of the visual reprojection; To obtain keyframes and keyframes Covariance of visual reprojection at time; The dynamic weights of the GNSS residuals; This is the residual factor for GNSS-RTK; This represents the system state during GNSS-RTK measurements within a sliding window. The square of the Mahalanobis norm of the GNSS-RTK residual factor; The number of GNSS-RTK measurements taken within the sliding window; This is the GNSS-RTK measurement number.

Images