Vehicle accurate positioning method used in GNSS denial environment

Abstract

The invention discloses a map-assisted vehicle online calibration and accurate positioning method for a GNSS denial environment, and belongs to the field of vehicle navigation and positioning. According to the method, firstly, a method for aligning a map matched with a reverse incremental map with the coordinates of the speedometer is adopted, so that online calibration of the conversion relation between the SLAM speedometer and a map coordinate system in a GNSS-free environment is realized; and then a real-time probability-based lane-level map matching algorithm is provided based on the topology directivity of the map, and the positioning precision and robustness in a complex urban environment are significantly improved by means of visual features and the lane-level topology map. And finally, a nonlinear optimization model of vision, inertia, a wheel speed meter and a vectorized HD map is constructed through a factor graph optimization technology. According to the method, online calibration can be rapidly completed by means of visual perception and a prior map in a GNSS denial environment, high-precision positioning is continuously realized by means of map constraint, and a new solution and a practical tool are provided for precise positioning and path planning of an automatic driving vehicle in a complex environment.

Description

Technical Field

[0001] The 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. As the most commonly used positioning system, the Global Navigation Satellite System (GNSS) can provide absolute positioning capability without offset in all weather conditions, but its robustness to complex occlusion environments is poor and it 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 the current Inertial Measurement Unit (IMU) devices cannot guarantee high-precision positioning for a long time. The visual SLAM system relies on loop detection to eliminate cumulative errors in the absence of GNSS, but in normal outdoor driving behavior, the positioning error often accumulates over time due to the lack of loop detection. Summary of the invention

[0003] In view of the problems existing in the above-mentioned prior art, the present invention constructs a vision / IMU / wheel speed meter / map system framework based on factor graph optimization, integrates high-precision maps (expressed as HD) into the visual inertial odometer for posture correction, and can achieve accurate positioning of vehicles in complex environments over a long period 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) In order to solve the problem of large positioning error 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 probability-based real-time lane-level map matching model is proposed. The maximum probability method is used to integrate the previous and next associations and information observations defined under the road network, screen and sort candidate points, 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.

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

[0009] Furthermore, in step (1), lane line detection is performed using an accurate 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 can be calculated as:

[0010]

[0011] 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.

[0012] For the detection of the stop line, the present invention adopts a method based on line segment detection. The camera placed in front of the vehicle captures the image in real time. When the vehicle approaches the stop line, the first pixel coordinate of the detected stop line will be stably output. 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:

[0013]

[0014] Among them, d sl Indicates 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 on the v axis in the pixel coordinate system, and c y Indicates the x-axis offset of the optical axis relative to the center of the projection plane coordinates.

[0015] As for the acquisition of the lane ID of the current vehicle, since most vehicles tend to start from the right road and enter the main road from the right when leaving the intersection area, the present invention calculates the lane ID with the rightmost lane as the starting point, and determines whether the vehicle has actively changed lanes and the direction of the lane change through lane detection and vehicle lane change behavior recognition. The change in the number of lanes is mainly reflected in the definition of the intersection in the prefabricated HD map. Finally, the lane ID of the vehicle is tracked and calculated in real time based on the characteristics of the vehicle's lane change and road changes, and the lane ID of the vehicle is estimated robustly and correctly in complex situations.

[0016] Furthermore, in step (2), the method of aligning the map and odometer coordinates based on reverse incremental map matching only requires an initial approximate position (i.e., anchor point) 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:

[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) Use the stop line distance obtained by visual detection and map matching method to accurately calculate the corresponding position of the vehicle;

[0019] (3.3) Using the vehicle position information and the increments between the odometer position points, reverse recursion is performed to obtain the vehicle trajectory corresponding to the odometer in the UTM (Universal Transverse Mercator) coordinate system, and ultimately determine the precise position 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, the lane ID is tracked based on the actual road conditions to achieve lane-level map matching. Then, the map matching process is regarded as the determination of the maximum probability of the previous and next associations and information observations under the fusion road network definition. Specifically, it is to search 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, more 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. It is expressed as lane weight, and the candidate points in the same lane have a large 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.

[0024] According to 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, and uses the quadratic Bezier curve 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 stop line information is used to determine whether the vehicle has reached the intersection, and combined with the a priori stop line coordinates in the map to obtain the error between the odometer coordinates and the actual position of the vehicle. The front and rear lanes of the intersection are searched and matched according to the virtual nodes of the intersection, and the correct map matching is performed according to the driving state of the vehicle after passing the intersection. When a large deviation correction occurs, the stable map matching point is used instead of the odometer as the actual positioning data of the vehicle. After the odometer data error is corrected, the jointly optimized odometer data is used as the actual positioning data of the vehicle.

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

[0030] 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, the main basis for its determination is to multiply the transfer probability and observation probability of the accurate convergence point at the previous moment and the candidate point at the current moment. The formula is as follows:

[0031]

[0032] In the formula, score t Represents 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 the present invention, the transfer probability is described as the probability of the convergence point at the previous moment transferring 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 of the vehicle, a special treatment is performed for this situation, and the transfer 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 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.

[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 in combination 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 SLAM positioning framework of vision / inertia / map based on factor graph optimization, which 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, so as to realize 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 according to the connectivity of virtual nodes at intersections. And using the observations of map matching, a nonlinear optimization model of vision, inertia, wheel speed meter 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, experiments using public data sets and real-world environments are used to verify the system. The experimental results show that the proposed system can cope with the influence of complex environments and maintain accurate positioning over a large range for a long time outdoors. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a schematic diagram of lateral 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 It is a schematic diagram of the vectorized road network constructed by the present invention.

[0051] Figure 5It is a schematic diagram of the map matching incremental strategy of the present invention, wherein the left figure is a schematic diagram of directly projecting the map based on the distance at adjacent moments, and the right figure is a schematic diagram of projecting the map 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] In order to make the purpose and technical solution of the embodiments of the present invention clearer, the present invention is further explained below in conjunction with 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 through a robust and fast real-time lane detection network model, and the vehicle lane change is then identified based on the detected lane line. 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 will not be 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 FIG, a camera placed in front of the vehicle captures images in real time. When the vehicle approaches the stop line, the first pixel coordinate of the detected stop line is output stably. Figure 3 As shown in the figure, 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 center point of the stop line on the v axis in the pixel coordinate system, and c y Indicates the x-axis offset of the optical axis relative to the center of the projection plane coordinates.

[0061] As for the acquisition of the lane ID of the current vehicle, since most vehicles tend to start from the right road and enter the main road from the right when leaving the intersection area, the present invention calculates the lane ID with the rightmost lane as the starting point, and determines whether the vehicle has actively changed lanes and the direction of the lane change through lane detection and vehicle lane change behavior recognition. The change in the number of lanes is mainly reflected in the definition of the intersection in the prefabricated HD map. Finally, through algorithm tracking, the lane ID of the vehicle is robustly and correctly estimated in complex situations.

[0062] Step S2: The method of aligning the map and odometer coordinates based on reverse incremental map matching only requires an initial approximate position (i.e., anchor point) 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., 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 the map matching method to accurately calculate the corresponding position of the vehicle;

[0065] S2.4 performs reverse recursion based on the vehicle position 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 finally determines the precise position of the anchor point. Figure 5 As shown in the figure below, the direct mapping method in the left image will cause positioning deviation when the distance between the vehicle and the left lane line changes at adjacent moments, while the right image 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 The reverse recursive calculation is performed in the manner shown in the middle right figure, and finally an accurate positioning result is obtained;

[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 greater 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 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, the main basis for its determination is to multiply the transfer probability and observation probability of the accurate convergence point at the previous moment and the candidate point at the current moment. The formula is as follows:

[0070]

[0071] In the formula, score t Represents 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 Transfer probability: In the present invention, the transfer probability is described as the probability of the convergence point at the previous moment transferring 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 of the vehicle, special treatment is performed for this situation, and the transfer 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] According to the candidate points obtained by screening in 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, and uses the quadratic Bezier curve 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 stop line information is used to determine whether the vehicle has reached the intersection, and combined with the a priori stop line coordinates in the map to obtain the error between the odometer coordinates and the actual position of the vehicle. The front and rear lanes of the intersection are searched and matched according to the virtual nodes of the intersection, and the correct map matching is performed according to the driving state of the vehicle after passing the intersection. When a large deviation correction occurs, the stable map matching point is used instead of the odometer as the actual positioning data of the vehicle. After the odometer data error is corrected, the jointly optimized odometer data is used as the actual positioning data of the vehicle.

[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 odometer. 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 the 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 in combination 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 positioning of a vehicle 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 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. (3) A probability-based real-time lane-level map matching model is proposed. The model uses the maximum probability method to integrate the previous and next associations and information observations defined under 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) A nonlinear optimization model of vision, inertial, wheel speed meter, and vectorized HD map is constructed through factor graph optimization technology to continuously correct the lateral distance and heading angle errors of the odometer.

2. A method for precise vehicle positioning in a GNSS-denied environment according to claim 1, characterized in that: In the step (1), lane line detection is performed by a robust and fast real-time lane detection network model, and the lane change of the vehicle 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: 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, cosθ is the relative angle between the vehicle and the lane line; For the detection of the stop line, a method based on line segment detection is adopted. The camera placed in front of the vehicle captures the image in real time. When the vehicle approaches the stop line, the first and last pixel coordinates 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 according to the pre-calibrated camera internal parameters and camera height. The formula is as follows: Among them, d sl Indicates 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 on the v axis in the pixel coordinate system, and c y Indicates the x-axis offset of the optical axis relative to the center of the projection plane coordinates; As for obtaining the lane ID of the current vehicle, since most vehicles tend to start from the right road and enter the main road from the right when leaving the intersection area, the lane ID is calculated with the rightmost lane as the starting point. Through lane detection and vehicle lane change behavior recognition, it is determined whether the vehicle has actively changed lanes and the direction of the lane change. The change in the number of lanes is reflected in the definition of the intersection in the pre-made HD map. Finally, through algorithm tracking, the lane ID of the vehicle is robustly and correctly estimated in complex situations.

3. The method for precise vehicle positioning in a GNSS-denied environment according to claim 1, characterized in that: In step (2), only the initial approximate position, i.e., the anchor point, is needed 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) Use the stop line distance obtained by visual detection and map matching method to accurately calculate the corresponding position of the vehicle; (3.3) Using the vehicle’s position information and the increments between each odometer position point, reverse recursion is performed to obtain the vehicle trajectory corresponding to the odometer in the UTM coordinate system, and ultimately determine the precise position 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 positioning of a vehicle in a GNSS-denied environment according to claim 1, characterized in that: In step (3), the lane ID is first tracked based on the actual road condition requirements to achieve lane-level map matching; then the map matching process is regarded as the maximum probability of integrating the previous and next associations and information observations under the definition of the road network, which is specifically manifested in 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, more candidate points are often generated, so the candidate points need to be screened and sorted according to priority; the formula for sorting candidate points is: Among them, 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 actual vehicle heading angle estimated by the map; It is expressed as lane weight. Candidate points in the same lane have a large 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. According to the candidate points obtained by screening, three control points are selected with the matching edge of the entry intersection and the matching edge of the exit intersection as the control edge, 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: Among them, p i is the i-th control point, b i,n is a Bernstein basic polynomial of degree n: Finally, based on the stop line information, it is determined whether the vehicle has reached the intersection and combined with the a priori stop line coordinates in the map to obtain the error between the odometer coordinates and the actual position of the vehicle; the front and rear lanes of the intersection are searched and matched according to the virtual nodes of the intersection, and correct map matching is performed according to the driving status of the vehicle after passing the intersection. When a large deviation correction occurs, the stable map matching point is used instead of the odometer as the actual positioning data of the vehicle. After the odometer data error is corrected, the jointly optimized odometer data is used as the actual positioning data of the vehicle.

5. The method for precise positioning of a vehicle in a GNSS-denied environment according to claim 4, characterized in that: 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, and the formula is as follows: In the formula, score t Represents 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; The transfer probability is described as the probability of the convergence point at the previous moment transferring 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, a special treatment is given to this situation, and the transfer probability tp 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.

6. The method for precise positioning of a vehicle in a GNSS-denied environment according to claim 1, characterized in that: In step (4), the 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 x is as follows: 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; The cost function is minimized in the form of MAP estimation, and its formula is as follows: 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 in combination with the map, and m represents the number of lateral distance and heading angle measurements in the sliding window.

Images