A method for precise positioning of a vehicle in a GNSS denial environment

Abstract

The application discloses a kind of map-assisted vehicle online calibration and accurate positioning method for GNSS denial environment, belong to vehicle navigation positioning field.The method first adopts a kind of reverse incremental map matching map and odometer coordinate alignment method, to realize the online calibration of SLAM odometer and map coordinate system conversion relationship under the environment without GNSS.Then based on the topological direction of map, a kind of real-time lane level map matching algorithm based on probability is proposed, with the aid of visual feature and lane level topological map, the positioning accuracy and robustness in complex urban environment are significantly improved.Finally, a nonlinear optimization model of vision, inertia, wheel speed meter and vectorized HD map is constructed by factor graph optimization technology.The method can rely on visual perception and prior map to complete online calibration under the environment of GNSS denial, and continuously realize high-precision positioning using map constraints, provide a new solution and practical tool for accurate positioning and path planning of autonomous vehicle in complex environment.

Description

Technical Field

[0001] The present invention relates to a map-assisted vehicle online calibration and precise positioning method for a GNSS-denied environment, and belongs to the technical field of vehicle navigation and positioning. Background Art

[0002] Accurate and reliable vehicle positioning is crucial for smart cars. The Global Navigation Satellite System (GNSS), as the most commonly used positioning system, can provide absolute positioning capabilities without offsets in all weather conditions. However, it has poor robustness in complex occluded environments and is easily affected by multipath effects and signal blocking. GNSS / Inertial Navigation System (INS) combined navigation can overcome the impact of GNSS loss in a short period of time, but current Inertial Measurement Unit (IMU) devices cannot guarantee high-precision positioning over a long period of time. Visual SLAM systems rely on loop detection to eliminate cumulative errors in the absence of GNSS. However, in normal outdoor driving behavior, positioning errors often accumulate over time due to the lack of loop detection. Summary of the Invention

[0003] To address the above-mentioned problems in the existing technology, this paper constructs a vision / IMU / wheel speedometer / map system framework based on factor graph optimization, integrating high-precision maps (HD) into the visual inertial odometry for posture correction. This enables accurate positioning of vehicles in complex environments over long periods of time and over a wide range. The main technical solutions are as follows:

[0004] A method for accurately positioning a vehicle in a GNSS-denied environment comprises the following steps:

[0005] (1) Use a visual camera to detect road signs, including lane line detection, vehicle lane change detection, and stop line detection, and obtain the lane ID of the current vehicle, the lateral distance between the vehicle and the left lane line, and the longitudinal distance between the vehicle and the stop line;

[0006] (2) To solve the problem of large positioning errors in GNSS-denied environments, a method for aligning map and odometer coordinates based on reverse incremental map matching is proposed. Stop line detection is used to search on a pre-constructed map, and the displacement increments between adjacent odometers are projected onto the actual map. Finally, the coordinate system transformation parameters are calculated through trajectory alignment.

[0007] (3) Combined with the vectorized road network map, a real-time lane-level map matching model based on probability is proposed, which fuses the front and rear correlation under the definition of road network and information observation through the maximum probability method, screens and sorts the candidate points, and continuously and accurately matches the lane-level map matching of the output of the odometer, and robustly processes the intersection data according to the connectivity of the virtual nodes of the intersection;

[0008] (4) A nonlinear optimization model of vision, inertia, wheel speed meter and vectorized HD map is constructed through factor graph optimization technology, so as to continuously correct the lateral distance and heading angle error of the odometer.

[0009] Further, in step (1), the lane line detection is performed by a precise and fast real-time lane detection network model, and the vehicle lane change is recognized according to the detected lane line. Although the lateral distance of the vehicle is affected by the relative angle of the vehicle and the lane line, the relative proportion is unchanged, so the distance calculation is not affected. Therefore, the lateral distance can be calculated as:

[0010]

[0011] Wherein, w l represents the distance between the camera center and the left lane line in the pixel coordinate system, w r represents the distance between the camera center and the right lane line in the pixel coordinate system, d w represents the actual width of the lane, and cosθ is the relative angle of the vehicle and the lane line.

[0012] For the detection of the stop line, the application adopts a method based on line segment detection, which captures images in real time through the camera placed in front of the vehicle. When the vehicle is close to the stop line, the first pixel coordinates of the detected stop line are stably output. Based on the monocular vision ranging principle, the actual distance between the vehicle and the stop line can be calculated according to the internal parameters of the camera and the camera height, which is calibrated in advance. The formula is as follows:

[0013]

[0014] Wherein, d sl represents the actual distance between the vehicle and the stop line, H v represents the height of the camera, f represents the focal length of the camera, v represents the value of the center point of the stop line in the v-axis in the pixel coordinate system, and c y represents the x-axis offset of the optical axis to the projection plane coordinate center.

[0015] To determine the vehicle's current lane ID, the present invention uses the rightmost lane as the starting point, given that most vehicles tend to depart from the right side of the road and enter the main road from the right when leaving an intersection. Lane detection and lane change recognition determine whether the vehicle has actively changed lanes and the direction of the change. Changes in the number of lanes are primarily reflected in the intersection definition in the prefabricated HD map. Finally, the vehicle's lane ID is tracked and calculated in real time based on the characteristics of lane changes and road conditions, enabling robust and accurate estimation of the vehicle's lane ID in complex situations.

[0016] Furthermore, in step (2), the method of aligning the map and odometry coordinates based on reverse incremental map matching only requires an initial approximate position (i.e., anchor point) to use the map to achieve visual-inertial initialization and online initialization and calibration between the local odometry coordinate system and the global coordinate system—that is, to calculate the accurate coordinate system transformation relationship. The specific steps are as follows:

[0017] (3.1) Find the stop line along the lane line on the map based on the rough anchor point and calculate the distance from the vehicle to the stop line;

[0018] (3.2) Using the stop line distance obtained by visual detection and map matching method, the corresponding position of the vehicle is accurately calculated;

[0019] (3.3) Using the vehicle's position information and the increments between each odometer position point, perform reverse recursion to obtain the vehicle trajectory corresponding to the odometer in the UTM (Universal Transverse Mercator) coordinate system, and ultimately determine the precise location of the anchor point;

[0020] (3.4) The corresponding trajectory can quickly complete the calculation of the transformation matrix between the local world coordinate system and the UTM coordinate system, thereby obtaining the coordinate system transformation between the local world coordinate system and the UTM coordinate system.

[0021] Furthermore, in step (3), a real-time probability-based lane-level map matching algorithm is proposed. First, lane IDs are tracked based on actual road conditions to achieve lane-level map matching. The map matching process is then considered to be the determination of the maximum probability of the preceding and following associations and information observations under the fusion road network definition. Specifically, it is performed by searching for candidate points for possible lane edges within a certain radius of the positioning point. Due to the changes in the search radius and the number of lane edges, a large number of candidate points are often generated. Therefore, the candidate points need to be screened and sorted according to priority. The formula for candidate point sorting is:

[0022]

[0023] where dE is the Euclidean distance from the trajectory point to the matching edge, represents the heading angle probability, which is determined by the trajectory heading and the true heading estimated by the map heading, h traj Indicates the trajectory heading angle, h map Represents the true vehicle heading angle estimated from the map. Expressed as lane weight, candidate points in the same lane have a higher weight, while those in opposite lanes have a lower weight. After sorting from small to large, a certain number of candidate points with higher priority are selected for subsequent probability screening.

[0024] Based on the candidate points obtained by screening, the present invention selects three control points with the matching edge of the entry intersection and the matching edge of the exit intersection as the control edges, which are the end point, the starting point, and the intersection of the two edges. The quadratic Bezier curve is used for trajectory approximation. The calculation formula of the Bezier curve fitting is as follows:

[0025]

[0026] Among them, p i is the i-th control point, b i,n is a Bernstein basic polynomial of degree n:

[0027]

[0028] Finally, the vehicle's arrival at the intersection is determined based on the stop line information and combined with the a priori stop line coordinates in the map to determine the error between the odometer coordinates and the vehicle's actual position. The intersection's virtual nodes are used to search and match the front and rear lanes. Accurate map matching is performed based on the vehicle's driving state after passing through the intersection. In the event of significant deviations, stable map matching points are used instead of the odometer as the vehicle's actual positioning data. Once the odometer data errors are corrected, the jointly optimized odometer data is used as the vehicle's actual positioning data.

[0029] Furthermore, the probability of the candidate points to be subsequently screened is specifically calculated as follows:

[0030] For real-time positioning systems, the positioning point at each moment must find the corresponding convergence point with the highest probability among multiple candidate points. For each map matching point at time t, the main basis for its determination is to multiply the transfer probability of the accurate convergence point at the previous moment and the candidate point at the current moment by the observation probability. The formula is as follows:

[0031]

[0032] In the formula, score t Indicates the maximum probability of all N candidate points at time t, tpti is the transition probability of the i-th candidate point at time t, ep ti is the observation probability of the i-th candidate point at time t;

[0033] In this invention, the transition probability is described as the probability that the convergence point at the previous moment will transition to the candidate point at the current moment. Assuming the vehicle has not changed lanes, the transition probability is calculated by taking the shortest distance between the topological connections between the convergence point at the previous moment and different candidate points at the current moment. Considering that lane IDs often change during lane changes, a special handling is provided for this situation. The transition probability is expressed as:

[0034]

[0035] Among them, E is the Euclidean distance between the convergence point at the previous moment and the candidate point at the current moment, and L is the shortest distance that satisfies the topological connection relationship.

[0036] The observation probability in this invention refers to the method described in the patent CN202410466762.4 "A lane-level map-assisted SLAM method for persistent positioning in GNSS-denied areas".

[0037] Furthermore, in step (4), the backend factor graph optimization used is based on the sliding window optimizer to fuse the various sensor data in the factor graph optimization framework, and the optimized state variable χ is as follows:

[0038]

[0039]

[0040]

[0041] Among them, the world coordinate system is represented by W-frame, the camera coordinate system is represented by C-frame, and x k is the IMU state at each time node k, which contains the position of the IMU in the world coordinate system speed Rotation Vector and gyroscope bias and acceleration bias n represents the sliding window size, Represents the transformation matrix between C-frame and B-frame, which contains the position vector between C-frame and B-frame and the rotation vector {γ i ,i=0,…,l} is the inverse depth of the landmark point since the i-th key frame, where l is the number of key frames;

[0042] The cost function is minimized in the form of MAP estimation, and its formula is as follows:

[0043]

[0044] Among them, r p is the prior information residual, H p is the matrix of prior information, r pre is the IMU pre-integrated measurement residual, is the pre-integrated residual model at adjacent moments, r Md and r Ma They represent the lateral distance residual and heading angle residual obtained by combining with the map, and m represents the number of lateral distance and heading angle measurements in the sliding window.

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] The present invention proposes a visual / inertial / map SLAM positioning framework based on factor graph optimization. It uses stop line detection to search on a pre-constructed map, projects the displacement increments between adjacent odometers into the actual map, and completes the calculation of coordinate system transformation parameters through trajectory alignment, realizing the alignment of map and odometer coordinates based on reverse incremental map matching. In addition, based on the characteristics of vectorized topological maps, a real-time probability-based lane-level map matching algorithm is proposed to perform continuous and accurate lane matching on the odometer output and robustly process intersection data based on the connectivity of virtual nodes at the intersection. And using the observations of map matching, a nonlinear optimization model of vision, inertia, wheel speedometer and map is constructed to continuously correct the lateral distance and heading angle errors of the odometer. In order to fully evaluate the effectiveness of the proposed system, the system is verified using experiments with public datasets and real-world environments. The experimental results show that the proposed system can cope with the influence of complex environments and maintain accurate positioning for a long time and a large range outdoors. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a schematic diagram of the horizontal distance calculation of the present invention.

[0048] Figure 2 It is the stop line detection diagram of the present invention.

[0049] Figure 3 It is a schematic diagram of the stop line distance calculation of the present invention.

[0050] Figure 4 This is a schematic diagram of the vectorized road network constructed by the present invention.

[0051] Figure 5This is a schematic diagram of the map matching incremental strategy of the present invention, wherein the left figure is a schematic diagram of map projection directly based on the distance between adjacent moments, and the right figure is a schematic diagram of map projection based on the distance component along the lane.

[0052] Figure 6 It is a factor graph architecture diagram of the present invention. DETAILED DESCRIPTION

[0053] To make the purpose and technical solution of the embodiments of the present invention clearer, the present invention is further explained below with reference to the accompanying drawings in the embodiments of the present invention:

[0054] The present invention discloses a map-assisted vehicle online calibration and precise positioning method for a GNSS-denied environment, comprising the following steps:

[0055] Step S1: Use a visual camera to detect road signs, including lane line detection, vehicle lane change detection, and stop line detection, and obtain the lane ID of the current vehicle, the lateral distance between the vehicle and the left lane line, and the longitudinal distance between the vehicle and the stop line. Among them, lane line detection is performed using a robust and fast real-time lane detection network model, and the vehicle lane change is then identified based on the detected lane lines. Figure 1 As shown in the figure, although the lateral distance of the vehicle is affected by the relative angle between the vehicle and the lane line, the calculation of the distance is not affected if the relative ratio remains unchanged. Therefore, the lateral distance can be calculated as:

[0056]

[0057] Among them, w l Indicates the distance between the camera center and the left lane line in the pixel coordinate system, w r Indicates the distance between the camera center and the right lane line in the pixel coordinate system, d w represents the actual width of the lane, and cosθ is the relative angle between the vehicle and the lane line.

[0058] For the detection of the stop line, the present invention adopts a method based on line segment detection, such as Figure 2 As shown in , the camera placed in front of the vehicle captures images in real time, and when the vehicle approaches the stop line, it will stably output the first pixel coordinate of the detected stop line. Figure 3 As shown, based on the principle of monocular vision ranging, the actual distance between the vehicle and the stop line can be calculated according to the pre-calibrated camera internal parameters and camera height. The formula is as follows:

[0059]

[0060] Among them, d sl Indicates the actual distance between the vehicle and the stop line, H vrepresents the height of the camera, f represents the focal length of the camera, v represents the value of the stop line center point on the v axis in the pixel coordinate system, c y Indicates the x-axis offset of the optical axis relative to the center of the projection plane coordinate system.

[0061] To determine the vehicle's current lane ID, the present invention uses the rightmost lane as the starting point, given that most vehicles tend to depart from the right side of the road and enter the main road from the right when leaving an intersection. Lane detection and lane change behavior recognition determine whether the vehicle has actively changed lanes and the direction of the change. The change in the number of lanes is primarily reflected in the intersection definition in the pre-fabricated HD map. Finally, through algorithmic tracking, the vehicle's lane ID is robustly and accurately estimated in complex situations.

[0062] Step S2: The method of aligning the map and odometry coordinates based on reverse incremental map matching only requires an initial approximate position (i.e., anchor point) to use the map to achieve visual-inertial initialization and online initialization and calibration between the local odometry coordinate system and the global coordinate system—that is, to calculate the accurate coordinate system transformation relationship. The specific steps are as follows:

[0063] S2.1 searches for the stop line along the lane line on the map based on the rough anchor point and calculates the distance from the vehicle to the stop line;

[0064] S2.2 uses the stop line distance obtained by visual detection and map matching method to accurately calculate the corresponding position of the vehicle;

[0065] S2.4 performs reverse recursion based on the vehicle's location information and the increments between each odometer position point to obtain the vehicle trajectory corresponding to the odometer in the UTM (Universal Transverse Mercator System) coordinate system, and ultimately determines the precise location of the anchor point. Figure 5 As shown in the figure below, the direct mapping method on the left will cause positioning deviation when the distance between the vehicle and the left lane line changes at adjacent moments, while the right figure calculates the incremental distance component along the lane line based on the distance change between the vehicle and the left lane line, and then restores the vehicle position based on the angle. Figure 5 Perform reverse recursive calculations as shown in the middle right figure, and finally obtain accurate positioning results;

[0066] The trajectory corresponding to S2.5 can quickly complete the calculation of the transformation matrix between the local world coordinate system and the UTM coordinate system, thereby obtaining the coordinate system transformation between the local world coordinate system and the UTM coordinate system. Step S3: Combined with the vectorized road network map, a real-time probability-based lane-level map matching algorithm is proposed. First, the lane ID is tracked based on the actual road conditions to achieve lane-level map matching. The map matching process is then regarded as the determination of the maximum probability of the previous and next associations and information observations under the fusion road network definition, which is specifically manifested in the search for candidate points for possible lane edges within a certain radius of the positioning point. Due to changes in the search radius and the number of lane edges, more candidate points are often generated, so the candidate points need to be screened and sorted according to priority. The formula for candidate point sorting is:

[0067]

[0068] where d E is the Euclidean distance from the trajectory point to the matching edge, represents the heading angle probability, which is determined by the trajectory heading and the true heading estimated by the map heading, h traj Indicates the trajectory heading angle, h map Represents the true vehicle heading angle estimated from the map. Expressed as lane weight, candidate points in the same lane have a higher weight, and vice versa. After sorting from small to large, a certain number of candidate points with higher priority are selected for subsequent probability screening. The specific calculation is as follows:

[0069] For real-time positioning systems, the positioning point at each moment must find the corresponding convergence point with the highest probability among multiple candidate points. For each map matching point at time t, the main basis for its determination is to multiply the transfer probability of the accurate convergence point at the previous moment and the candidate point at the current moment by the observation probability. The formula is as follows:

[0070]

[0071] In the formula, score t Indicates the maximum probability of all N candidate points at time t, tp ti is the transition probability of the i-th candidate point at time t, ep ti is the observation probability of the i-th candidate point at time t;

[0072] S3.1 Transition Probability: In this invention, the transition probability is described as the probability of the previous convergence point transitioning to the current candidate point. Assuming the vehicle has not changed lanes, the transition probability is calculated by taking the shortest distance between the previous convergence point and the different candidate points at the current moment. Considering that lane IDs often change during lane changes, and with special handling for this situation, the transition probability is expressed as:

[0073]

[0074] Among them, E is the Euclidean distance between the convergence point at the previous moment and the candidate point at the current moment, and L is the shortest distance that satisfies the topological connection relationship.

[0075] S3.2 Observation probability The present invention refers to the method described in the patent "A lane-level map-assisted SLAM method for persistent positioning in GNSS-denied areas" CN202410466762.4.

[0076] Based on the candidate points obtained by screening according to the above steps, the present invention selects three control points with the matching edge of the entry intersection and the matching edge of the exit intersection as the control edges, which are the end point, the starting point, and the intersection of the two edges. The quadratic Bezier curve is used for trajectory approximation. The calculation formula of the Bezier curve fitting is as follows:

[0077]

[0078] Among them, p i is the i-th control point, b i,n is a Bernstein basic polynomial of degree n:

[0079]

[0080] Finally, the vehicle's arrival at the intersection is determined based on the stop line information and combined with the a priori stop line coordinates in the map to determine the error between the odometer coordinates and the vehicle's actual position. The intersection's virtual nodes are used to search and match the front and rear lanes. Accurate map matching is performed based on the vehicle's driving state after passing through the intersection. In the event of significant deviations, stable map matching points are used instead of the odometer as the vehicle's actual positioning data. Once the odometer data errors are corrected, the jointly optimized odometer data is used as the vehicle's actual positioning data.

[0081] Step S4: Figure 6As shown in the figure, a nonlinear optimization model of vision, inertia, wheel speed meter and vectorized HD map is constructed through factor graph optimization technology, so as to continuously correct the lateral distance and heading angle errors of the odometry. The back-end factor graph optimization used is based on the sliding window optimizer to fuse the data of each sensor in the factor graph optimization framework. The optimized state variable χ is as follows:

[0082]

[0083]

[0084]

[0085] Among them, the world coordinate system is represented by W-frame, the camera coordinate system is represented by C-frame, and x k is the IMU state at each time node k, which contains the position of the IMU in the world coordinate system speed Rotation Vector and gyroscope bias b gk and acceleration bias n represents the sliding window size, Represents the transformation matrix between C-frame and B-frame, which contains the position vector between C-frame and B-frame and the rotation vector {γ i ,i=0,…,l} is the inverse depth of the landmark point since the i-th key frame, where l is the number of key frames;

[0086] The cost function is minimized in the form of MAP estimation, and its formula is as follows:

[0087]

[0088] Among them, r p is the prior information residual, H p is the matrix of prior information, r pre is the IMU pre-integrated measurement residual, is the pre-integrated residual model at adjacent moments, and They represent the lateral distance residual and heading angle residual obtained by combining with the map, and m represents the number of lateral distance and heading angle measurements in the sliding window.

Claims

1. A method for precise vehicle positioning in a GNSS-denied environment, characterized in that: The steps include: (1) Use a visual camera to detect road signs, including lane line detection, vehicle lane change detection, and stop line detection, and obtain the lane ID of the current vehicle, the lateral distance between the vehicle and the left lane line, and the longitudinal distance between the vehicle and the stop line; (2) A method for aligning map and odometry coordinates based on reverse incremental map matching is proposed. Stop line detection is used to search on a pre-built map, and the displacement increments between adjacent odometry points are projected onto the actual map. Finally, the coordinate system transformation parameters are calculated through trajectory alignment. (3) A probability-based real-time lane-level map matching model is proposed. This model uses the maximum probability method to integrate the contextual associations and information observations defined by the road network, screens and sorts candidate points, performs continuous and accurate lane matching on the odometer output, and robustly processes intersection data based on the connectivity of virtual nodes at the intersection. (4) Using factor graph optimization technology to build a nonlinear optimization model of vision, inertial, wheel speed meter and vectorized HD map, so as to continuously correct the lateral distance and heading angle errors of the odometry; In step (3), lane IDs are first tracked based on actual road conditions to achieve lane-level map matching. The map matching process is then considered to maximize the probability of integrating the preceding and following associations and information observations under the definition of the road network. Specifically, candidate points are searched for possible lane edges within a certain radius of the positioning point. Due to the changes in the search radius and the number of lane edges, a large number of candidate points are often generated. Therefore, the candidate points need to be screened and sorted according to priority. The formula for candidate point sorting is: ； in, is the Euclidean distance from the trajectory point to the matching edge, represents the heading angle probability, which is determined by the actual heading estimated by the track heading and the map heading. represents the trajectory heading angle, Represents the actual vehicle heading angle estimated by the map; Expressed as lane weight, candidate points in the same lane have a higher weight, and vice versa. After sorting from small to large, a certain number of candidate points with higher priority are selected for subsequent probability screening. Based on the candidate points obtained by screening, three control points are selected with the matching edge of the entry and exit intersections as the control edges. They are the end point, the starting point, and the intersection of the two edges. The quadratic Bezier curve is used for trajectory approximation. The calculation formula of the Bezier curve fitting is as follows: ； in, is the i-th control point, is a Bernstein basic polynomial of degree n: ； Finally, the vehicle's arrival at the intersection is determined based on the stop line information and combined with the a priori stop line coordinates in the map to determine the error between the odometer coordinates and the vehicle's actual position. The front and rear lanes of the intersection are searched and matched based on the intersection's virtual nodes. The correct map matching is performed based on the vehicle's driving state after passing the intersection. When significant deviations occur, the odometer is replaced by a stable map matching point as the vehicle's actual positioning data. After the odometer data error is corrected, the jointly optimized odometer data is used as the vehicle's actual positioning data. The specific calculation of the probability involved in the subsequent probability screening of the candidate points is as follows: For the real-time positioning system, it is required that the positioning point at each moment must find the corresponding convergence point with the maximum probability among multiple candidate points; for each map matching point at time t, its determination is based on the product of the transfer probability and observation probability of the accurate convergence point at the previous moment and the candidate point at the current moment, as shown in the following formula: ； Where, express The maximum probability among all N candidate points at the moment, for The transition probability of the i-th candidate point at time, for The observation probability of the i-th candidate point at time; The transfer probability is described as the probability that the convergence point at the previous moment transfers to the candidate point at the current moment; when it is known that the vehicle has not changed lanes, the transfer probability is calculated by the shortest distance between the convergence point at the previous moment and the different candidate points at the current moment; considering that the lane ID often changes during the lane change process, special treatment is given to this situation, and the transfer probability is Expressed as: ； Among them, E is the Euclidean distance between the convergence point at the previous moment and the candidate point at the current moment, and L is the shortest distance that satisfies the topological connection relationship.

2. The method for precise vehicle positioning in a GNSS-denied environment according to claim 1, wherein: In step (1), lane line detection is performed using a robust and fast real-time lane detection network model, and the vehicle lane change is then identified based on the detected lane line. Although the lateral distance of the vehicle is affected by the relative angle between the vehicle and the lane line, the calculation of the distance is not affected if the relative ratio remains unchanged. Therefore, the lateral distance is calculated as: ； in, Indicates the distance between the camera center and the left lane line in the pixel coordinate system, Indicates the distance between the camera center and the right lane line in the pixel coordinate system, Indicates the actual width of the lane, is the relative angle between the vehicle and the lane line; For stop line detection, a line segment detection method is used. A camera placed in front of the vehicle captures images in real time. When the vehicle approaches the stop line, the coordinates of the first and last pixels of the detected stop line are stably output. Based on the principle of monocular vision ranging, the actual distance between the vehicle and the stop line is calculated based on the pre-calibrated camera internal parameters and camera height. The formula is as follows: ； in, Indicates the actual distance between the vehicle and the stop line. Indicates the height of the camera, represents the focal length of the camera, Indicates that the center point of the stop line in the pixel coordinate system is The value of the axis, Represents the optical axis relative to the center of the projection plane coordinates Axis offset; To determine the vehicle's current lane ID, the system uses the rightmost lane as the starting point, as most vehicles tend to depart from the right side and enter the main road from the right when leaving the intersection. Lane detection and lane change behavior recognition are used to determine whether the vehicle has actively changed lanes and the direction of the change. Changes in the number of lanes are reflected in the intersection definition in the prefabricated HD map. Finally, through algorithmic tracking, the vehicle's lane ID is robustly and accurately estimated in complex situations.

3. The method for precise vehicle positioning in a GNSS-denied environment according to claim 1, wherein: In step (2), only the initial approximate position, i.e., the anchor point, is required to use the map to realize visual inertial initialization and online initialization and calibration between the local odometer coordinate system and the global coordinate system, i.e., to calculate the accurate coordinate system transformation relationship. The specific steps are as follows: (3.1) Find the stop line along the lane line on the map based on the rough anchor point and calculate the distance from the vehicle to the stop line; (3.2) Accurately calculate the vehicle's corresponding position using the stop line distance obtained through visual detection and map matching methods; (3.3) Using the vehicle’s position information and the increments between each odometer position point, perform reverse recursion to obtain the vehicle trajectory corresponding to the odometer in the UTM coordinate system and ultimately determine the precise location of the anchor point; (3.4) The corresponding trajectory can quickly complete the calculation of the transformation matrix between the local world coordinate system and the UTM coordinate system, thereby obtaining the coordinate system transformation between the local world coordinate system and the UTM coordinate system.

4. The method for precise vehicle positioning in a GNSS-denied environment according to claim 1, wherein: In step (4), the factor graph optimization used is based on the sliding window optimizer to fuse the sensor data in the factor graph optimization framework, and the optimized state variables are , as shown below: ； Among them, the world coordinate system is represented by W-frame, and the camera coordinate system is represented by C-frame. For each time point The IMU state at , which contains the position of the IMU in the world coordinate system ,speed , rotation vector and gyroscope bias and acceleration bias , represents the sliding window size, Represents the transformation matrix between C-frame and B-frame, which contains the position vector between C-frame and B-frame and the rotation vector , is the inverse depth of the landmark point since the i-th 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: ； in, is the prior information residual, is the matrix of prior information, is the IMU pre-integrated measurement residual, is the pre-integrated residual model at adjacent moments, and They represent the lateral distance residual and heading angle residual obtained by combining with the map, and m represents the number of lateral distance and heading angle measurements in the sliding window.

Images