A high-precision map-based visual slam vehicle positioning method for GNSS denial environment

Abstract

The application discloses a kind of high-precision map-based visual simultaneous localization and mapping (SLAM) vehicle positioning methods for GNSS denial environment, belong to vehicle navigation positioning field, the method first adopts visual lane recognition algorithm to realize the identification of road lane line and the estimation of vehicle lateral deviation distance and angle, then for the long-time accumulated error problem of SLAM, using the observability of road normal error, the front and rear position constraint constructed by inertial measurement unit (IMU) and the common view constraint of visual feature, a nonlinear optimization model aiming at eliminating SLAM error is constructed, so as to realize the reliable identification and elimination of accumulated error by map matching. This method greatly improves the vehicle positioning accuracy, and can maintain stable sub-meter level positioning under the condition of global navigation satellite system (GNSS) signal loss, ensuring the vehicle's lane-level path planning and lane-level navigation service capability.

Description

TECHNICAL FIELD

[0001] The application relates to a high-definition map-based visual simultaneous localization and mapping (SLAM) vehicle positioning method for a GNSS denial environment and belongs to the technical field of vehicle navigation and positioning. BACKGROUND

[0002] Precise and reliable vehicle positioning is crucial for intelligent vehicles. As the most commonly used positioning system, the global navigation satellite system (GNSS) can provide all-weather and unbiased absolute positioning capability, but its robustness to complex occlusion environment is poor and is easily affected by multipath effect and signal occlusion. GNSS / inertial navigation system (INS) integrated navigation can overcome the influence of GNSS loss in a short time, but the current inertial measurement unit (IMU) device cannot guarantee long-time high-precision positioning. The visual SLAM system can rely on loop detection to eliminate cumulative errors in the case of GNSS loss, but the positioning error gradually accumulates over time due to the lack of loop detection in normal outdoor driving behavior. SUMMARY

[0003] In view of the problems in the prior art, the application constructs an optimized visual / IMU / GNSS / map system framework, integrates a high-definition map (denoted as HD) into a visual inertial odometer for pose correction, and can realize precise positioning of a vehicle in a long time and a large range of complex environments. The main technical scheme is as follows:

[0004] (1) initializing visual inertial data and obtaining a rotation matrix and a translation vector of a carrier coordinate system (denoted as B-frame) to a transverse Mercator coordinate system (denoted as U-frame);

[0005] (2) in the case of poor GNSS observation state, a map matching method is used to associate the odometer data with the prior map information to obtain a lane-level lateral distance and a lane angle; in the case of good GNSS observation state, GNSS data are used to correct the pose;

[0006] (3) lane detection, lane transformation detection and Hough transformation are used to process the vehicle-mounted camera data to obtain distance and angle information of the current vehicle relative to the lane;

[0007] (4) Construct the visual re-projection, IMU pre-integration, GNSS, map lateral distance and map heading angle data item residual, and add it to the factor graph optimization for multi-information fusion, so as to realize the continuous and stable positioning of the odometer;

[0008] (5) Experimental test in complex urban environment and real world scene.

[0009] Further, in step (1), the visual inertial data needs to be integrated, the initialization is completed, and after the GNSS observation data and the map data are unified in the U-frame, the rotation matrix and the translation vector of the B-frame to the U-frame are obtained by using the GNSS data.

[0010] Further, the map matching method in step (2) constructs a lane-level map matching method based on hidden Markov, uses the lane data perceived from the video image of the camera and the historical trajectory information, and realizes more accurate matching by using the lane-level map matching method based on the lane-level map. The lane-level map model is mainly a directed graph composed of nodes, edges and their attributes, wherein the nodes include start nodes, end nodes and virtual intersection nodes. The specific steps of constructing the HD map model are as follows:

[0011] (2.1) Draw the HD map according to the orthographic projection map and the lane marks in the laser point cloud map;

[0012] (2.2) Use the R-tree-based lane segment topological relationship automatic generation method to realize the generation of topological relationship;

[0013] (2.3) Since there is no real road network in the intersection area, there is a lack of real lane information constraint, and there is randomness and randomness when the vehicle turns, in order to eliminate the positioning deviation caused by the lack of constraint in the intersection area, a virtual node is used to represent the center position of the intersection;

[0014] (2.4) Based on the smooth circular arc trajectory characteristics of the vehicle passing through the intersection and the map matching information before and after the vehicle passing through the intersection, a multi-lane level intersection trajectory mathematical model is established, and a virtual trajectory generation algorithm is used to generate the trajectory of the vehicle at the turning place of the intersection;

[0015] (2.5) According to the lane-level road network characteristics, use related methods to realize automatic correction of error and suspicious topological information, and automatically generate node id, lane number, lane id and lane width and other attributes.

[0016] Further, in step (3), the lane detection, lane change detection and Hough transform are used to process the vehicle-mounted camera data to obtain the distance and angle information of the current vehicle relative to the lane, wherein the distance and angle information of the current vehicle relative to the lane is calculated as follows:

[0017] (3.1) Lateral distance: the lane detection method based on convolutional neural network is used to detect the road in front of the vehicle in the camera video image, and each lane line in the road is represented as a point set. In order to obtain accurate and robust lane line position, the use of RANSAC method and lane line historical position information can effectively supplement the missing lane line and prevent the lane line from interfering with the method due to wear and obstruction. Finally, the pixel distance between the image center and the left and right lane lines is calculated by the lane line points at the bottom of the image. In the case of known current lane width, the actual distance between the vehicle and the left and right lane lines can be obtained. In addition, the change rule of the lateral distance of the vehicle in the lane changing process can be used to accurately judge the lane changing behavior of the vehicle;

[0018] (3.2) Angle: after obtaining the lane by using the lane detection method, in order to accurately express the deviation and angle between the vehicle and the lane line, the camera image is first converted into a bird's eye view by Hough transform, and the lane detection point set is projected onto the bird's eye view. In order to prevent part of the abnormal points from affecting the linearization expression of the lane, RANSAC method is used for linearization fitting to obtain stable lane line and corresponding vehicle and lane line angle. The linearization formula is as follows:

[0019] f(x) = c0 + c1x

[0020] Wherein, c0 and c1 are lane line linear fitting equation parameters, and x is the horizontal coordinate in the pixel coordinate system.

[0021] Further, in step (4), the visual re-projection, IMU pre-integration, GNSS, lateral distance and map heading angle data item residuals are constructed and added to the factor graph optimization for multi-information fusion, so as to realize the continuous and stable positioning of the odometer. The back-end factor graph optimization used is based on a sliding window optimizer to fuse various sensor data in the factor graph optimization framework. The optimized state variable χ is as follows:

[0022]

[0023] Wherein, the world coordinate system is represented by W-frame, the camera coordinate system is represented by C-frame, x k is the IMU state at each time node k, which includes the position velocity rotation and gyroscope bias and acceleration bias n represents the size of the sliding window, denotes the transformation matrix between C-frame and B-frame, which contains the position vector between C-frame and B-frame and rotation vector {γ i is the inverse depth of the landmark point since the ith key frame, where l is the number of key frames;

[0024] and minimize the cost function in the form of MAP estimation:

[0025]

[0026] where, r p is the prior information residual, H p is the matrix of prior information, r pre is the IMU pre-integration measurement residual, r C is the visual feature point re-projection residual, r G denotes the GNSS measurement residual, and denote the lateral distance residual and the heading angle residual obtained by combining with the map, respectively, is the pre-integration residual model of adjacent time, is the re-projection error model of a single visual feature point, is the measurement error model of single point GNSS, is the measurement error model of single point visual odometry and HD map, m denotes the number of lateral distance and heading angle measurement values in the sliding window, denotes the number of GNSS measurement values in the sliding window;

[0027] Further, the GNSS, map lateral distance and map heading angle data item residual is specifically calculated as follows:

[0028] (4.1) GNSS residual:

[0029]

[0030] where, is the translation vector of B-frame to the world coordinate system (denoted by W-frame), denotes the rotation matrix of B-frame to W-frame, is the translation vector of U-frame to W-frame, is the arm value between U-frame and B-frame;

[0031] (4.2) Lateral distance residual:

[0032] The lateral distance between the vehicle and the lane line obtained by map matching is as follows:

[0033]

[0034] where, is the transformation matrix between W-frame and U-frame, P is the odometry position point coordinate, f(·) denotes map matching, P m is the point obtained by map matching, d l is half of the lane width, E(·) denotes the Euclidean distance function, κ is the direction parameter, which is positive when the odometry estimated position is less than d l from the left lane, and negative when it is greater than d l ;

[0035] The lateral distance between the vehicle and the lane line obtained by vision detection is as follows:

[0036]

[0037] where, is the rod arm value between the camera coordinate system (C-frame) and B-frame, d w is the lane width, θ v is the vision angle, w l and w r respectively represent the pixel distance from the camera optical center to the left and right lane boundaries;

[0038] The lateral distance residual error is:

[0039]

[0040] The angle residual error is:

[0041]

[0042] where, θ o is the odometry estimated heading angle, φ is the vision detected heading angle of the vehicle and the lane line;

[0043] Further, the step (5) is tested in the public data set and the real world scene under the complex urban environment, in order to fully verify and evaluate the proposed method framework, respectively, in the public data set and the real world data set are tested, and respectively using x and y axis error under U-frame, vehicle center transverse and longitudinal error, maximum error and absolute trajectory error (ATE) and other indicators to verify the experimental results, the experimental data analysis result shows that, under the complex urban environment (even GNSS signal is refused), the proposed algorithm can realize real-time, accurate and robust global positioning, which has important significance for the vehicle to realize long-distance non-offset positioning technology research without GNSS.

[0044] Compared with the prior art, the beneficial effects of the present application are that:

[0045] The present application proposes a visual / inertial / map SLAM positioning framework based on factor graph optimization, which utilizes the prior HD map and the rich semantic scene perception of the visual camera to constrain the lateral distance and heading angle of the vehicle, integrates the HD map into the visual inertial odometer for pose correction, can realize accurate positioning of the vehicle in a long time and a large range of complex environment, and constructs a real-time and robust pose estimator, so that in the complex environment where GNSS is large-scale missing or even unavailable, the error correction of position and heading is provided by using the map. In order to fully evaluate the effectiveness of the proposed system, the public data set and the real world environment experiment are used for system verification. The experimental results show that the proposed system can cope with the influence of complex environment, and maintain accurate positioning in a long time and a large range outdoors. BRIEF DESCRIPTION OF DRAWINGS

[0046] Figure 1 It is a flowchart of the method provided by the present application.

[0047] Figure 2 It is a multi-vehicle road section using road-level map matching diagram of the present application.

[0048] Figure 3 It is a HD map intersection virtual node interpretation diagram constructed by the present application.

[0049] Figure 4 It is a comparison result diagram of various algorithms trajectories under the denial environment of the present application.

[0050] Figure 5 It is a lane detection diagram of the present application, wherein the left diagram is a camera original image, and the right diagram is an image after lane line detection.

[0051] Figure 6is the angle calculation diagram after inverse perspective transformation of the application, wherein the left diagram is a lane line image when the vehicle is parallel to the lane line, and the right diagram is a lane line image when the vehicle is not parallel to the lane line.

[0052] Figure 7 is the factor diagram framework of the application.

[0053] Figure 8 is the result comparison diagram of the application under a public data set.

[0054] Figure 9 is the result comparison diagram of the application under a real scene. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical scheme of the embodiments of the application more clear, the following further illustrates the application in combination with the drawings in the embodiments of the application:

[0056] As shown in Figure 1 , the application discloses a visual SLAM vehicle positioning method based on high-precision maps in a GNSS denial environment, comprising the following steps:

[0057] Step S1: integrate visual inertial data, complete initialization, and after unifying GNSS observation data and map data under a horizontal Mercator coordinate system (denoted by U-frame), obtain a rotation matrix and a translation vector of a carrier coordinate system (denoted by B-frame) to the U-frame by using GNSS data.

[0058] Step S2: in the case of poor GNSS observation state, utilize lane data perceived from a video image of a camera and historical trajectory information, and on the basis of a lane-level map, use a lane-level map matching algorithm to associate odometer data and HD map information, as shown in Figure 2 , to obtain lane-level horizontal distance and lane angle, and in the case of good GNSS observation state, correct the pose by using GNSS data; wherein the map matching method is a lane-level map matching method based on a hidden Markov, the lane-level map model is mainly a directed graph composed of nodes, edges and their attributes, the nodes include a start node, an end node and a virtual intersection node set, the edges include a lane center line and an extended edge in an intersection area, the definition of the edge attribute can add prior information of a road network in addition to the topological relationship, and the attribute further adds definition of lane-level road attributes such as a lane id, a road lane number and a lane width in addition to basic information such as a start node id and an end node id, and the specific steps of constructing the HD map model are as follows:

[0059] S2.1 draw an HD map according to lane marks in an orthographic projection map and a laser point cloud map;

[0060] S2.2 The S2.2 is automatically generated by using the lane segment topology relationship based on the R-tree method, and the topology relationship is generated.

[0061] S2.3 Since the intersection area does not have a real road network, there is a lack of real lane information constraints, and there is randomness and randomness when the vehicle turns, in order to eliminate the positioning deviation caused by the lack of constraints in the intersection area, a virtual node is used to represent the center position of the intersection, as shown in Figure 3

[0062] S2.4 Based on the smooth circular arc trajectory characteristics of the vehicle passing through the intersection and the map matching information before and after the vehicle passing through the intersection, a multi-lane level intersection trajectory mathematical model is established, and a virtual trajectory generation algorithm is used to generate the trajectory of the vehicle at the intersection turning point, as shown in Figure 4

[0063] S2.5 According to the lane level road network characteristics, the related method is used to realize the automatic correction of the error and suspicious topology information, and the node id, lane number, lane id and lane width and other attributes are automatically generated.

[0064] Step S3: The lane detection, lane change detection and Hough transform are used to process the vehicle-mounted camera data, and the distance and angle information of the current vehicle relative to the lane are obtained, wherein the distance and angle information of the current vehicle relative to the lane are calculated as follows:

[0065] S3.1 Lateral distance: the lane detection method based on convolutional neural network is used to detect the lane in front of the vehicle in the camera video image, and each lane line in the road is represented as a point set, as shown in Figure 5 In order to obtain accurate and robust lane line position, the use of RANSAC method and lane line historical position information can effectively supplement the missing lane line and prevent lane line from interfering with the method due to wear and obstruction; finally, the pixel distance between the image center and the left and right lane lines is calculated by the lane line points at the bottom of the image, and the actual distance between the vehicle and the left and right lane lines can be obtained under the condition that the current lane width is known; in addition, the lateral distance change rule of the vehicle during lane changing can be used to accurately judge the lane changing behavior of the vehicle;

[0066] S3.2 Angle: after obtaining the lane by using the lane detection method, in order to accurately express the deviation and angle between the vehicle and the lane line, the camera image is converted into a bird's eye view by Hough transform, and the lane detection point set is projected onto the bird's eye view, as shown in Figure 6 In order to prevent part of the abnormal points from affecting the linearization expression of the lane, RANSAC method is used for linearization fitting to obtain stable lane line and corresponding vehicle and lane line angle, and the linearization formula is as follows:

[0067] f(x) = c0 + c1x​​

[0068] where c0, c1 are the parameters of the lane line linear fitting equation, and x is the horizontal coordinate in the pixel coordinate system.

[0069] Step S4: Constructing the visual re-projection, IMU pre-integration, GNSS, map lateral distance, and map heading angle data item residuals, and adding them to the factor graph optimization for multi-information fusion, thereby realizing the continuous and stable positioning of the odometer. The backend factor graph optimization used is based on a sliding window optimizer to fuse various sensor data in the factor graph optimization framework. The optimized state variable χ is as follows:

[0070]

[0071] where W-frame represents the world coordinate system, C-frame represents the camera coordinate system, x k is the IMU state at each time node k, which includes the position velocity rotation and gyroscope bias and acceleration bias n represents the size of the sliding window, represents the transformation matrix between C-frame and B-frame, which includes the position vector and the rotation vector {γ i between the i-th key frame and the landmark point, where l is the number of key frames;

[0072] and the cost function is minimized in the form of MAP estimation:

[0073]

[0074] where r p is the prior information residual, H p is the matrix of prior information, r pre is the IMU pre-integration measurement residual, r C is the visual feature point re-projection residual, r G represents the GNSS measurement residual, and represent the lateral distance residual and the heading angle residual obtained by combining with the map, respectively, is the pre-integration residual model of adjacent time points, is the re-projection error model of a single visual feature point, is the measurement error model of a single GNSS point, m represents the number of lateral distance and heading angle measurements in the sliding window, n represents the number of GNSS measurements in the sliding window.

[0075] As shown in FIG. 4, the GNSS, lateral distance and map heading angle data item residuals are calculated as follows: Figure 7

[0076] S4.1 GNSS Residuals:

[0077]

[0078] where, is the translation vector from B-frame to W-frame, is the rotation matrix from B-frame to U-frame, is the translation vector from U-frame to W-frame, is the lever arm value between U-frame and B-frame;

[0079] S4.2 Map Lateral Distance Residuals:

[0080] The vehicle-to-lane-line lateral distance from map matching is as follows:

[0081]

[0082] where, is the transformation matrix between W-frame and U-frame, P is the odometry position point coordinate, f(·) represents map matching, P m is the point from map matching, d l represents half of the lane width, E(·) represents the Euclidean distance function, and K is the direction parameter, which is positive when the odometry estimated position is less than d l from the left lane and negative when greater than d l ;

[0083] The vehicle-to-lane-line lateral distance from visual detection is as follows:

[0084]

[0085] where, is the lever arm value between C-frame and B-frame, d w is the lane width, Q v is the visual angle, w l and w r respectively represent the pixel distance from the camera optical center to the left and right lane boundaries.

[0086] ​The lateral distance residual is:

[0087]

[0088] S4.3 Angle residual: After obtaining the accurate lane lines on both sides of the vehicle through the lane detection algorithm, the angle is calculated according to the lane line expression obtained by linear fitting formula, and the angle value θ v The specific calculation is:

[0089]

[0090] Where c0 and c1 are the parameters of the lane line linear fitting equation, respectively.

[0091] On the basis of map matching to obtain the accurate lane line on the map, the angle formula between the lane line vector V l = (e x , e y ) and the x-axis vector V U = (u x , u y ) in the U-frame coordinate system is as follows:

[0092]

[0093] Adjust it to the range of [-π, π], then:

[0094]

[0095] Therefore, the real angle of the vehicle θ = θ m + θ v , then the attitude Euler angle of the vehicle can be expressed as: A U = (α, β, θ). By converting A U to the direction cosine matrix R U , and converting to the unified W-frame coordinate system, R W can be obtained, and its calculation formula is as follows:

[0096]

[0097] According to the Rodrigues formula, the direction cosine matrix R U is converted to the Euler angle , and the heading angle is φ;

[0098] The vehicle attitude estimated by the odometer is expressed as a quaternion:

[0099] q W = q w + q x i + q y j + q z k

[0100] where i, j, k are the imaginary parts of the quaternion, q x , q y , q z are the imaginary coefficients of the quaternion, q w is the real part of the quaternion;

[0101] The formula for converting it to Euler angles is:

[0102]

[0103] Therefore, the angle residual term can be:

[0104]

[0105] where θ o is the heading angle estimated by the odometry, and φ is the heading angle of the vehicle and the lane line detected by vision;

[0106] Step S5: The proposed method is evaluated using the public dataset KAIST Complex Urban Dataset, and the sequence urban 39 with a trajectory length of 10678 meters is selected for actual evaluation, as shown in Figure 8 The proposed algorithm is compared with VINS-Mono and ORB-SLAM3 in terms of trajectory, and it can be seen that the proposed algorithm only has a small drift, while VINS-Mono and ORB-SLAM3 have large offset errors, and in the absence of effective absolute position constraints, the error is gradually accumulated and becomes larger. In addition, real-world data sets are used for verification, and the data set is collected and processed in a real scene in a certain urban area in Nanjing, with a collection trajectory distance of 5.78 kilometers, as shown in Figure 9 The trajectories of various algorithms in the real map scene are shown, and VINS-Mono has a smaller positioning error than ORB-SLAM3 in the initial stage due to better heading estimation, while the positioning error becomes larger in the later stage due to the deviation of the heading estimation. Overall, the trajectories of the two algorithms have a large trajectory deviation compared to the ground truth, and have lost the function of vehicle positioning. In contrast, the proposed algorithm has a very small error trajectory due to the correction of the heading angle and lateral distance error.

Claims

1. A high-definition map-based visual SLAM vehicle positioning method for GNSS denial environment, characterized in that, The method comprises the following steps: (1) initializing visual inertial data, and obtaining a rotation matrix and a translation vector of a carrier coordinate system to a U-frame, wherein the carrier coordinate system is represented by a B-frame, and the U-frame is a U-frame; (2) in the case that a global navigation satellite system (GNSS) observation state is poor, a map matching method is used to associate the odometer data with prior high-definition (HD) map information to obtain a lane-level lateral distance and a lane angle; in the case that the GNSS observation state is good, GNSS data is used to correct a pose; (3) lane detection, lane change detection and Hough transformation are used to process vehicle camera data to obtain a lateral distance and an angle information of a current vehicle relative to a lane; (4) visual re-projection, inertial measurement unit (IMU) pre-integration, GNSS, map lateral distance and map heading angle data item residuals are constructed and added to a factor graph optimization for multi-information fusion, so that the odometer can be continuously and stably positioned accurately; (5) test experiments are performed on a public data set and a real-world scene in a complex urban environment.

2. The high-definition map based visual SLAM vehicle positioning method for GNSS denial environment according to claim 1, characterized in that, In the step (1), the visual inertial data is integrated, the initialization is completed, the GNSS observation data and the map data are unified in the U-frame, and the rotation matrix and the translation vector of the B-frame to the U-frame are obtained by using the GNSS data.

3. The high-definition map based visual SLAM vehicle positioning method for GNSS denial environment according to claim 1, characterized in that, The map matching method in the step (2) constructs a lane-level map matching method based on a hidden Markov model, lane data perceived from a video image of a camera and historical trajectory information are used, and a lane-level map matching method is used to achieve more accurate matching based on a lane-level map. The lane-level map model is a directed graph composed of nodes, edges and attributes, wherein the nodes include a start node, an end node and a virtual intersection node set, and the specific steps of constructing the HD map model are as follows: (2.1) the HD map is drawn according to the orthographic projection map and the lane marks in the laser point cloud map; (2.2) the topological relationship is generated by using a lane segment topological relationship automatic generation method based on an R-tree; (2.3) since there is no real road network in the intersection area, there is a lack of real lane information constraint, and there is randomness and randomness when the vehicle turns, in order to eliminate the positioning deviation caused by the lack of constraint in the intersection area, a virtual node is used to represent the center position of the intersection; (2.4) based on the smooth circular arc trajectory characteristics of the vehicle passing through the intersection and the map matching information before and after the vehicle passing through the intersection, a multi-lane-level intersection trajectory mathematical model is established, and a virtual trajectory generation algorithm is used to generate the trajectory of the vehicle at the intersection turning place; (2.5) according to the lane-level road network characteristics, the related method is used to automatically correct the wrong and suspicious topological information, and the node id, the lane number, the lane id and the lane width attributes are automatically generated.

4. The high-definition map based visual SLAM vehicle positioning method for GNSS denial environment according to claim 1, wherein, In the step (3), the lane detection, the lane change detection and the Hough transformation are used to process the vehicle camera data to obtain the distance and the angle information of the current vehicle relative to the lane, and the current vehicle angle information is calculated as follows: After obtaining the lane by using the lane detection method, in order to accurately express the offset and angle between the vehicle and the lane line, the camera image is obtained by Hough transform to get the bird's eye view, and the lane detection point set is projected to the bird's eye view; in order to prevent part of the abnormal points from affecting the linear expression of the lane, the RANSAC method is used for linear fitting to obtain the stable lane line and the corresponding vehicle and lane line angle, and the linearization formula is as follows: f(x)=c0+c1x Wherein, c0, c1 are the lane line linear fitting equation parameters, and x is the horizontal coordinate in the pixel coordinate system.

5. The high-definition map based visual SLAM vehicle positioning method for GNSS denial environment according to claim 1, wherein, In step (4), the visual re-projection, IMU pre-integration, GNSS, map lateral distance and map heading angle data item residuals are constructed and added to the factor graph optimization for multi-information fusion, so as to realize the continuous and stable positioning of the odometer, wherein the rear-end factor graph optimization used is based on a sliding window optimizer to fuse various sensor data in the factor graph optimization framework, and the optimized state variable χ is as follows: where W-frame represents the world coordinate system, C-frame represents the camera coordinate system, x k is the IMU state at each time node k, which contains the position of the IMU in the world coordinate system velocity rotation vector and gyroscope bias and acceleration bias n represents the size of the sliding window, represents the transformation matrix between C-frame and B-frame, which contains the position vector between C-frame and B-frame and rotation vector {γ i is the inverse depth of the landmark point since the ith key frame, where l is the number of key frames; The cost function is minimized in the form of MAP estimation, and its formula is as follows: where r p is the prior information residual, H p is the matrix of prior information, r pre is the IMU pre-integration measurement residual, r C is the visual feature point re-projection residual, r G represents the GNSS measurement residual, and respectively represent the lateral distance residual and the heading angle residual obtained by combining with the map, is the pre-integration residual model of adjacent time, is the re-projection error model of a single visual feature point, is the measurement error model of a single GNSS point, is the measurement error model of a single visual odometry and HD map, m represents the number of lateral distance and heading angle measurement values in the sliding window, represents the number of GNSS measurement values in the sliding window.

6. The high-definition map based visual SLAM vehicle positioning method for GNSS denial environment according to claim 5, characterized in that, The map heading angle data item residual is calculated as follows: After the lane lines on the left and right sides of the vehicle are accurately obtained by the lane detection algorithm, the angle is calculated according to the lane line expression obtained by linear fitting formula, and the angle value θ v The specific calculation is as follows: Wherein, c0, c1 are the lane line linear fitting equation parameters; On the basis of map matching to obtain accurate lane lines on the map, the angle formula between the lane line vector V l =(e x ,e y ) in the U-frame coordinate system and the x-axis vector V U =(u x ,u y ) is as follows: Adjust it to [-pi, pi] range, then: Thus, the true angle of the vehicle θ = θ m + θ v The attitude Euler angles of the vehicle can be expressed as: A U = (α, β, θ); by converting A U to a direction cosine matrix R U , and converting to a unified W-frame coordinate system, R W is obtained, whose calculation formula is as follows: According to the Rodrigues formula, the direction cosine matrix R U is converted into Euler angles to obtain the heading angle φ; The vehicle pose estimated by the odometer is represented by a quaternion as: q W = q w + q x i + q y j + q z k where i, j, k are imaginary parts of quaternions, q x , q y , q z are imaginary coefficients of quaternions, q w is a real part of a quaternion; The formula for converting it to Euler angle is: Therefore, the angle residual term is: where θ o is the heading angle estimated by the odometry and φ is the heading angle of the vehicle and the lane line detected by vision.

7. The high-definition map based visual SLAM vehicle positioning method for GNSS denial environment according to claim 1, characterized in that, In step (5), the experiment test is carried out in the public data set and the real world scene under the complex urban environment, in order to fully verify and evaluate the proposed method framework, the test is carried out on the public data set and the real world data set respectively, and the experimental results are analyzed by using the x and y axis error under the U-frame, the lateral and longitudinal error of the vehicle center, the maximum error and the absolute trajectory error ATE index.

Images