Commercial vehicle high-precision positioning failure compensation method based on multi-source sensor fusion

Abstract

The application discloses a commercial vehicle high-precision positioning failure compensation method based on multi-source sensor fusion, realizes absolute and relative positioning function decoupling internal positioning sub-module structure through multi-source sensors; the confidence of the observation is determined through the state information of the GPS; the confidence of the monitoring positioning is evaluated through the confidence of the visual positioning matching; the confidence of the monitoring positioning system is evaluated through the timing and delay of each sensor, the positioning failure of the commercial vehicle is more reliably judged; the timing relationship and state information of the multi-source sensor data are comprehensively considered, the accuracy and robustness of the positioning failure detection are improved. According to the current positioning failure condition of the commercial vehicle and the multi-source sensor fusion information, the compensation strategy can be dynamically adjusted, more accurate positioning compensation results are provided, the optimization mode is selected for back-end fusion, the stability and precision of positioning are improved, and the execution efficiency of the algorithm is also optimized.

Description

Technical Field

[0001] The present invention relates to the field of vehicle positioning technology, and more specifically to a high-precision positioning failure compensation method for commercial vehicles based on multi-source sensor fusion. Background Art

[0002] Commercial vehicles play a vital role in logistics, transportation, and delivery. To ensure the safety and accurate positioning of commercial vehicles, positioning technologies such as the Global Positioning System (GPS) are commonly used. However, due to urban environments, building obstructions, signal interference, and other factors, autonomous commercial vehicles may experience positioning failures in certain situations, resulting in inaccurate or even impossible positioning.

[0003] Currently, the problem of commercial vehicle positioning failure has attracted widespread attention, and some solutions have been proposed. For example, the Chinese patent application publication number CN115932924A discloses a method and system based on IMU-assisted positioning. After the positioning of positioning systems such as GNSS / GPS / Beidou is delayed or lost, it no longer relies solely on the positioning system. The MCU can still obtain the vehicle's heading angle, pitch angle and other vehicle attitude parameters from the IMU module, calculate the vehicle's displacement based on the IMU information and vehicle speed information, obtain the vehicle's running trajectory, calculate the current position information based on the displacement information, obtain the vehicle's positioning information, and provide accurate navigation information for the vehicle. However, due to problems such as integral drift when using IMU, its positioning results are not accurate enough.

[0004] Although some solutions have been proposed to compensate for the failure of vehicle positioning, there are still some defects and limitations, which are mainly reflected in the following points:

[0005] 1. Positioning failure detection algorithms are often based on threshold settings or heuristic rules, which lack accuracy and robustness. This can lead to misjudgments or failure to detect positioning failures in a timely manner, thus affecting the accuracy of positioning compensation.

[0006] 2. Positioning failure compensation lacks flexibility and adaptability, often relying solely on historical data for interpolation or simple correction, and is unable to fully utilize other reliable sensor data to improve the accuracy and reliability of compensation. Summary of the Invention

[0007] The present invention provides a commercial vehicle high-precision positioning failure compensation method based on multi-source sensor fusion, aiming to improve the positioning accuracy and reliability of commercial vehicles in the event of positioning failure.

[0008] The present invention adopts the following technical solutions:

[0009] A high-precision positioning failure compensation method for commercial vehicles based on multi-source sensor fusion. The multi-source sensors include GPS, IMU, wheel speedometer, visual sensor, and perception sensor. The high-precision positioning failure compensation method includes:

[0010] The positioning module receives raw data from RTK, IMU, and wheel speedometers, senses the 3D position of road elements in the current frame, and high-precision map elements. By matching the sensed elements with those in the high-precision map and performing multi-sensor fusion, it obtains accurate position and posture results, providing absolute and relative positioning.

[0011] The pose initialization module is used to traverse the poses when only the RTK provides the initial position at the beginning of positioning, and match the perceived lane lines with the map lane lines at each pose;

[0012] The dead reckoning module uses IMU and wheel speed meter information to perform pose estimation based on Kalman filtering to provide smooth relative pose;

[0013] The single-frame positioning module is used to obtain the coordinates of each road element sent by the sensor, classify the elements by type, determine the presence or absence of each type of element, and assign a value to the corresponding status bit;

[0014] The pose fusion module is used to combine the poses of several historical frames with the relative pose relationship given by DR, and perform pose optimization again in the sliding window to obtain a smoother pose output;

[0015] The specific steps of the high-precision positioning failure compensation method are as follows:

[0016] Step 1: After receiving the sensor data and perception results, the positioning module checks whether the posture initialization has been completed. If not, the posture initialization module traverses the posture and enters step 2 after the posture initialization is completed.

[0017] Step 2: The dead reckoning module uses the IMU information and wheel speed meter information to perform pose estimation based on Kalman filtering;

[0018] Step 3: The single-frame positioning module establishes a correlation between the perception results and map elements, and calculates the lane line residual based on this correlation. Finally, it combines this residual, the predicted pose, and the RTK position information to calculate the pose of the current frame.

[0019] Step 4: Add relative constraints to the poses of several historical frames in step 3. First, determine whether the RTK observation is valid. If so, add RTK constraints. If not, then determine whether the visual positioning is valid. If visual positioning is valid, add visual constraints. If not, enter the optimization solution. Determine whether the pose after optimization solution is a key frame. If so, enter the sliding window for pose optimization. If not, directly output the fused pose.

[0020] The above positioning module also has sensor failure judgment logic and corresponding processing measures.

[0021] The above-mentioned sensor failures include single-source failures and multi-source failures. Single-source failures include the following: a. When the RTK fails, the lane lines still serve as constraints. In this case, the lateral direction can meet the indicator requirements and the longitudinal direction can meet the 0.1% accuracy requirement. b. When the vision sensor fails, it maintains normal operation for 2 seconds. If it does not recover after 2 seconds, a positioning failure is reported. c. When the sensor fails, a positioning failure is directly reported. d. When the wheel speedometer fails, a positioning failure is directly reported. Multi-source failures include the following: a. When a multi-source failure includes the IMU and wheel speedometer, a positioning failure is directly reported. b. When a multi-source failure does not include the IMU and wheel speedometer, normal positioning is maintained for 1 second with an accuracy indicator of 0.1%. If it does not recover after 1 second, a positioning failure is reported.

[0022] The pose estimation based on Kalman filtering in step 2 above includes the following algorithm flow:

[0023] a. Establish a filtering model

[0024] The Kalman filter model is established based on the DR error model. The state vector of the error model includes misalignment angle error, velocity error, position error, gyro bias, and table bias, totaling 15 dimensions. The Kalman filter model is established as follows:

[0025]

[0026] In the above formula: A is the error matrix; G is the error driving matrix; W is the sensor noise matrix, which satisfies the normal distribution; H is the measurement matrix; V is the observation noise, which satisfies the normal distribution;

[0027] b. Matrix discretization and one-step prediction

[0028] Matrix discretization includes the discretization of the state transfer matrix and the noise matrix, which are calculated using the following formula (3-2):

[0029]

[0030] In the above formula: F is the state transfer matrix, I is the identity matrix, and Δt is the discrete time;

[0031] The one-step prediction is completed by using the following formula (3-3):

[0032]

[0033] In the above formula, X k+1 is the state variable at k+1 time, X k is the state variable at k time; P k+1 is the covariance matrix at k+1 time, R k is the covariance matrix at k time;

[0034] c, observation update

[0035] The observation update is completed by calculating the information error according to the externally input speed observation, and on this basis, the chi-square check is calculated, and the chi-square check value is output, which is completed by using the following (3-4):

[0036]

[0037] In the above formula, y k is the information error at k time; z k is the observation value at k time; K k is the gain coefficient at k time; and lambda is the chi-square check value.

[0038] Further, the step three further includes special processing for the lane line, specifically, the relative relationship between the lines is recovered according to the 3D coordinates, the distance between the adjacent two lines is calculated in the order from left to right, the distance is divided by the lane width and then the nearest integer is taken, so as to obtain the number of the lane line.

[0039] Specifically, the lane line number matching includes coarse matching and fine matching. The coarse matching is to match the first perceived lane line with the lane line in the map, and the lane line with a number greater than 1 is sequentially searched for a matching relationship in the map according to the distance relationship. When each perceived lane line can find a corresponding matching line in the map, and the lane line types are consistent, it is considered that the correct matching relationship is found. The fine matching includes the matching of lane lines, road boundaries, poles and signs. The matching of the lane lines has been completed through the coarse matching. The matching of the road boundaries is consistent with the matching of the lane lines. The matching of the poles calculates the distance between the center point of the perceived pole element and the center point of the perceived pole element, that is, the residual error of the pole. The matching of the signs calculates the geometric distance between the center point of the perceived sign element and the center point of the perceived sign element, that is, the residual error of the sign.

[0040] The specific process of the pose optimization in the step four is as follows: it is judged whether the window is out of the limit range. If it is out of the limit range, the oldest frame is marginalized, that is, the sliding window is removed.

[0041] From the above description of the present application, compared with the prior art, the present application has the following advantages:

[0042] 1. This invention utilizes data fusion from multiple sensors, including GPS, IMU, wheel speedometer, and vehicle-mounted camera. Compared with traditional single-sensor positioning systems, this invention decouples the internal positioning submodule structure by realizing absolute and relative positioning functions through multi-source sensors, thus achieving system redundancy and improving positioning accuracy and robustness.

[0043] 2. This invention uses GPS status information to determine the confidence level of observations, thereby evaluating the confidence level of the monitoring and positioning system. It also uses the confidence level of visual positioning matching to assess the confidence level of the monitoring and positioning system. Furthermore, it uses the timing and latency of individual sensors to assess the confidence level of the monitoring and positioning system, enabling more reliable determination of positioning failures in commercial vehicles. By comprehensively considering the timing relationships and status information of multi-source sensor data, the accuracy and robustness of positioning failure detection are improved. Field tests have shown that, under various complex environments, this invention improves reliability by 25% compared to methods that rely on a single sensor.

[0044] 3. This invention dynamically adjusts compensation strategies based on the commercial vehicle's current positioning failures and information from multi-source sensor fusion, providing more accurate positioning compensation results. By optimizing back-end fusion, this approach not only improves positioning stability and accuracy but also optimizes algorithm execution efficiency. Compared to traditional filtering methods, this solution achieves a 17% increase in algorithm processing speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 This is a functional interaction block diagram of the positioning module of the present invention.

[0046] Figure 2 This is a flow chart of the algorithm for dead reckoning of the present invention.

[0047] Figure 3 This is a schematic diagram of the normal lane line detection results of the present invention.

[0048] Figure 4 This is a schematic diagram of the present invention when lane markings are missed.

[0049] Figure 5 This is a schematic diagram of the correct lane line matching of the present invention.

[0050] Figure 6 This is an indication of lane line matching errors in the present invention. Figure 1 .

[0051] Figure 7 This is an indication of lane line matching errors in the present invention. Figure 2 .

[0052] Figure 8 Schematic diagram of rod matching of the present invention.

[0053] Figure 9 It is a schematic diagram of card matching of the present invention.

[0054] Figure 10 Flowchart for positioning failure compensation of the present invention.

[0055] Figure 11 This is a workflow diagram of the posture fusion of the present invention.

[0056] Figure 12 Schematic diagram of the optimization model of posture fusion of the present invention. DETAILED DESCRIPTION

[0057] The following describes specific embodiments of the present invention with reference to the accompanying drawings. Numerous details are provided below to provide a comprehensive understanding of the present invention, but those skilled in the art will appreciate that the present invention can be practiced without these details. Well-known components, methods, and processes are not described in detail below.

[0058] This embodiment provides a method for compensating for positioning failure of commercial vehicles based on multi-source sensor fusion, wherein the multi-source sensors include but are not limited to GPS, inertial measurement unit (i.e., IMU), wheel speed meter, visual sensor (mainly vehicle-mounted camera) and perception sensor (mainly vehicle-mounted radar).

[0059] The positioning failure compensation method mainly includes the following functional modules: positioning module, posture initialization module, track calculation module, single-frame positioning module and posture fusion module.

[0060] 1. Positioning module

[0061] The positioning module is functionally divided into three main parts: prediction, front-end observation, and back-end fusion. Prediction primarily involves dead reckoning (DR) comprised of an IMU and wheel speedometer. Visual sensors can form visual inertial (VIO), and perception sensors can form lane-reckoning (LIO) for prediction. There are three possible prediction methods: dead reckoning (DR), visual inertial (VIO), and lane-reckoning (LIO). However, since VIO offers no inherent accuracy advantage over DR, LIO generally performs poorly on solid-state radars, and both modules consume significant computing power, DR is selected for prediction.

[0062] Observation primarily refers to RTK and matching perception results with HD map elements. Observation can be performed using either RTK or matching perception results with maps, or both. Because lane markings only provide lateral constraints and not longitudinal constraints, RTK is required to assist with longitudinal error correction. Therefore, both elements coexist for observation.

[0063] There are generally two optional fusion methods: filtering and optimization. Since filtering only considers the current frame, while optimization can construct a sliding window, and since the constraints between multiple frames are taken into account, the sliding window is often more stable and more accurate than filtering. Therefore, this solution uses optimization for back-end fusion.

[0064] The functional interaction of the positioning module is as follows Figure 1 As shown in the figure, the positioning module receives raw data from RTK, IMU, and wheel speedometers, as well as the 3D position of road elements (including lane lines, poles, signs, ground road markings, and road boundaries) in the current frame as perceived. Simultaneously, the EHR sends high-precision map elements to the positioning module at a fixed frequency. Finally, by matching the perceived elements with those in the HD map and performing multi-sensor fusion, accurate position and pose results are obtained. It should be noted that due to the wide variety of ground road signs, positioning is uniformly represented by rectangular bounding boxes to simplify algorithm logic and communication logic.

[0065] The functions of the positioning module mainly include two parts:

[0066] 1. Absolute positioning: Its core is to use the perception results and map feature matching results as a strong reference to determine the vehicle's position in the map. When the lane position in the map deviates from the real geographic coordinate system, the map still prevails.

[0067] 2. Relative positioning: Dead reckoning outputs a smoothed ego-vehicle pose estimate, with the goal of maintaining relative pose accuracy over a short period of time. Accumulated errors are permitted for the absolute pose.

[0068] In addition to outputting a posture that meets performance requirements when the sensor is operating normally, the positioning module also needs to have sensor failure judgment logic and corresponding processing measures, including single-source failure and multi-source failure.

[0069] 1) Single source failure

[0070] a. RTK failure: When RTK fails, the system still has lane lines as constraints. At this time, the lateral direction can meet the index requirements and the longitudinal direction can meet the 0.1% accuracy requirement;

[0071] b. Vision failure: Since the entire autonomous driving system's position and posture are based on a map, when vision fails, the system lacks constraints with the map. The system cannot guarantee the correct position and posture within the map and can only rely on RTK for short-term constraints. Therefore, when vision fails, the system will maintain normal operation for 2 seconds, during which time the indicators still meet performance requirements. If it does not recover after 2 seconds, a positioning failure will be reported.

[0072] c. IMU failure: The IMU is the core component and strong dependency of the positioning module, so when it fails, a positioning fault is directly reported;

[0073] d. Wheel speed meter failure: The wheel speed meter is the core component and strong dependency of the positioning module, so when it fails, a positioning fault is directly reported.

[0074] 2) Multiple source failure

[0075] a. Multi-source failures involving IMUs and wheel speedometers: Since IMUs and wheel speedometers are core components and strong dependencies of the positioning system, they are directly reported as positioning faults when they fail.

[0076] b. Multi-source failure excluding IMU and wheel speed meter (referring to RTK and vision sensor failure): In this case, normal positioning is maintained for 1 second with an accuracy index of 0.1%. If it does not recover after 1 second, a positioning fault is reported.

[0077] 2. Pose Initialization Module

[0078] The pose initialization module is mainly responsible for traversing the poses within a certain distance range of the initial position when the positioning process is just started and only the initial position provided by RTK is available, but the precise pose is not yet available. At each pose, the module scores the quality of the matching results between the perceived lane lines and the map lane lines. The pose with the highest score is output as the initialization result.

[0079] 3. Dead Reckoning Module

[0080] The main function of the dead reckoning module is to use only the IMU and wheel speed meter information to perform pose estimation based on Kalman filtering. The main purpose of this pose estimation is to provide a smooth relative pose for the perception module and the control module. Although this method has cumulative errors, the pose estimated by dead reckoning is only used for short-term or short-distance relative pose estimation in the perception module and the control module, so the cumulative error does not constitute an obstacle to its use. The algorithm flow is as follows: Figure 2 As shown:

[0081] a. Filter model

[0082] The Kalman filter model is established based on the DR error model. The state vector of the error model includes misalignment angle (attitude) error, velocity error, position error, gyro bias, and table bias, totaling 15 dimensions. The Kalman filter model is established as follows:

[0083]

[0084] Upper Chinese style:

[0085] A is the error matrix;

[0086] G is the error driving matrix;

[0087] W is the sensor noise matrix, which satisfies the normal distribution;

[0088] H is the observation matrix;

[0089] V is the observation noise, which satisfies the normal distribution;

[0090] b. Matrix discretization and one-step prediction

[0091] Matrix discretization includes the discretization of the state transfer matrix and the noise matrix, which are calculated using the following formula (3-2).

[0092]

[0093] In the above formula: F is the state transfer matrix, I is the identity matrix, and Δt is the discrete time.

[0094] One-step prediction is completed using the following formula (3-3).

[0095]

[0096] In the above formula: X k+1 is the state variable at time k+1, X k is the state variable at time k;

[0097] R k+1 is the covariance matrix at time k+1, P k is the covariance matrix at time k.

[0098] c. Observation update

[0099] The observation update is mainly completed by calculating the information error based on the external input velocity observation quantity, and on this basis, the chi-square check is calculated and the chi-square check value is output, which is completed as follows (3-4).

[0100]

[0101] In the above formula: y k is the information error at time k;

[0102] z k is the observation value at time k;

[0103] K k is the gain coefficient at time k;

[0104] λ is the chi-square calibration value.

[0105] 4. Single frame positioning module

[0106] The core purpose of the single-frame positioning module is to establish a correlation between perception results and map elements, and to calculate the residual of the lane line based on this correlation. Finally, the pose of the current frame is calculated by combining this residual, predicted pose, RTK position and other information.

[0107] The main function of the single-frame positioning module is to obtain the coordinates of each road element sent by perception, classify the elements by type, determine the presence or absence of each type of element, and assign values ​​to the corresponding status bits for use in the subsequent matching module execution.

[0108] In addition, lane markings require special processing. Specifically, the relative relationship between lanes is restored based on 3D coordinates. This is done by calculating the distance between adjacent lanes from left to right, dividing this distance by the lane width, and rounding to the nearest integer to obtain the lane number. This method is used instead of directly numbering lanes from left to right to account for the possibility of missed lane line detection and to avoid relative relationship errors.

[0109] Take a typical three-lane highway scenario as an example to illustrate the results of this method: Figure 3 and Figure 4 As shown in the figure, black is the real lane line, red is the perceived lane line, the number represents the final number, d is the standard lane line distance (3.75m), and the solid line / dashed line represents the real lane line type. Figure 3 This is the result when all lane lines are detected normally. Figure 4 This is the result when there is a missed detection.

[0110] 1) Coarse Match

[0111] Lane matching involves matching the first lane line detected (i.e., lane line numbered 1 when lane relationship recovery is complete) with lane lines in the map. Lane lines numbered greater than 1 are then searched for a matching relationship in the map, based on their distance relationship. A correct match is considered found when each detected lane line has a matching line in the map, and the lane line types are consistent. The figure below illustrates this matching process. To fully demonstrate the effectiveness of the method, two lane lines may be missed in this example. Figure 5 Is the correct matching result, Figure 6 and Figure 7 The red color in the figure represents the perceived lane line, the black color represents the lane line in the map, and the blue arrow represents the association relationship.

[0112] 2) FineMatch

[0113] Fine matching specifically refers to the matching of lane lines, road boundaries, poles, and signs. Lane line matching has already been completed through coarse matching, and the road boundaries are expressed in the same way as lane lines, so the matching method is also consistent and will not be repeated here. Only the matching of poles and signs is introduced here.

[0114] Rod matching and residual calculation methods are as follows Figure 8As shown, the red part is the map pole element, the blue part is the perception pole element, and the distance between the center points of the two is the residual of the pole.

[0115] The card matching and residual calculation method is as follows Figure 9 As shown, red is the card element in the map, blue is the perceived card element, the geometric center of the four corner points is defined as the center point of the card, and the geometric distance between the two card center points is the residual of the card.

[0116] 5. Posture Fusion Module

[0117] The core purpose of the pose fusion module is to combine the poses of a certain number of historical frames with the relative pose relationship given by DR, and perform pose optimization again in the sliding window to obtain a smoother pose output.

[0118] The present invention is based on the commercial vehicle positioning failure compensation method of multi-source sensor fusion, referring to Figure 10 , the specific steps are as follows:

[0119] Step 1: After receiving the sensor data and perception results, the positioning module checks whether the posture initialization has been completed. If not, the posture initialization module traverses the posture. When the posture initialization is completed, it enters step 2.

[0120] Step 2: The dead reckoning module uses the IMU information and wheel speed meter information to perform pose estimation based on Kalman filtering.

[0121] Step 3: Post-perception processing: The single-frame positioning module establishes a correlation between the perception results and map elements, and calculates the lane line residual based on this correlation. Finally, it combines this residual with the predicted pose, RTK position and other information to calculate the pose of the current frame.

[0122] Step 4: Combine the poses of several historical frames in step 3 with the relative pose relationship given by DR and perform another pose optimization in the sliding window.

[0123] The pose optimization of the pose fusion module above adopts the graph optimization algorithm, which is to establish node information and constraints between nodes and obtain the fusion pose based on the least squares idea. The specific algorithm flow is as follows: Figure 11 As shown in the figure: relative constraints are added (constraints between nodes, i.e., adding binary edges in graph optimization algorithms); first, the validity of the RTK observation is determined. If so, RTK constraints are added; otherwise, the validity of the visual positioning is determined; if the visual positioning is valid, visual constraints are added; otherwise, optimization is performed; and finally, the pose after optimization is determined to be a keyframe. If so, the pose is optimized through a sliding window; otherwise, the fused pose is directly output. The above RTK and visual constraints are absolute constraints, i.e., unary edge constraints are added to a single node.

[0124] The optimization model of pose fusion is as follows Figure 12 As shown: Determine whether the window is out of the limit range. If it is exceeded, marginalize the oldest frame, that is, remove it from the sliding window.

[0125] The present invention can cover more scenarios, such as complex scenarios such as overpasses, tunnels, and urban areas. The positioning error is reduced by 40% compared with traditional methods, the reliability is improved by 25% compared with a single sensor, and the processing speed is increased by 17% compared with filtering solutions.

[0126] The above is only a specific implementation of the present invention, but the design concept of the present invention is not limited to this. Any non-substantial changes to the present invention using this concept shall be deemed as an infringement of the protection scope of the present invention.

Claims

1. A high-precision positioning failure compensation method for commercial vehicles based on multi-source sensor fusion, characterized by: The multi-source sensor includes GPS, IMU, wheel speed meter, visual sensor and perception sensor, and the high-precision positioning failure compensation method includes: The positioning module is used to receive the raw data of RTK, IMU, and wheel speed meter, the 3D position of the current frame road elements given by perception, and high-precision map elements, and obtain accurate posture results by matching the perception elements with the elements in the high-precision map and performing multi-sensor fusion, providing absolute positioning and relative positioning; the positioning module also has sensor failure judgment logic and corresponding processing measures. Sensor failure includes single-source failure and multi-source failure. Single-source failure is as follows: a. When RTK fails, there are still lane lines as constraints. At this time, the lateral direction can meet the index requirements and the longitudinal direction can meet the 0.1% accuracy requirement; b. When the visual sensor fails, it maintains normal operation for 2 seconds. If it does not recover after 2 seconds, a positioning fault will be reported; c. When the IMU fails, a positioning fault will be directly reported; d. When the wheel speed meter fails, a positioning fault will be directly reported; multi-source failure is as follows: a. Multi-source failure including IMU and wheel speed meter will directly report a positioning fault; b. Multi-source failure excluding IMU and wheel speed meter will first maintain normal positioning for 1 second with an accuracy index of 0.1%. If it does not recover after 1 second, a positioning fault will be reported; The pose initialization module is used to traverse the poses when only the RTK provides the initial position at the beginning of positioning, and match the perceived lane lines with the map lane lines at each pose; The dead reckoning module uses IMU and wheel speed meter information to perform pose estimation based on Kalman filtering to provide smooth relative pose; The single-frame positioning module is used to obtain the coordinates of each road element sent by the sensor, classify the elements by type, determine the presence or absence of each type of element, and assign a value to the corresponding status bit; The pose fusion module is used to combine the poses of several historical frames with the relative pose relationship given by DR, and perform pose optimization again in the sliding window to obtain a smoother pose output; The specific steps of the high-precision positioning failure compensation method are as follows: Step 1: After receiving the sensor data and perception results, the positioning module checks whether the posture initialization has been completed. If not, the posture initialization module traverses the posture and enters step 2 after the posture initialization is completed. Step 2: The dead reckoning module uses the IMU information and wheel speed meter information to perform pose estimation based on Kalman filtering; Step 3: The single-frame positioning module establishes a correlation between the perception results and map elements, and calculates the lane line residual based on this correlation. Finally, it combines this residual, the predicted pose, and the RTK position information to calculate the pose of the current frame. Step 4. Combine the poses of several historical frames in step 3 with the relative pose relationship given by DR, and perform pose optimization again in the sliding window. The specific process is as follows: add relative constraints, first determine whether the RTK observation is valid, if the RTK observation is valid, add RTK constraints, if not, then determine whether the visual positioning is valid; if the visual positioning is valid, add visual constraints, if not, enter the optimization solution; determine whether the pose after optimization solution is a key frame, if so, enter the sliding window for pose optimization, otherwise directly output the fused pose; the specific process of pose optimization is as follows: determine whether the window exceeds the limit range, if it exceeds, marginalize the oldest frame, that is, remove it from the sliding window.

2. The method for compensating for high-precision positioning failure of commercial vehicles based on multi-source sensor fusion according to claim 1, characterized in that: The pose estimation based on Kalman filtering in step 2 includes the following algorithm steps: a. Establish a filtering model A Kalman filter model is established based on the DR error model. The state vector of the error model includes misalignment angle error, velocity error, position error, gyro bias, and table bias, totaling 15 dimensions. b. Matrix discretization and one-step prediction Matrix discretization includes the discretization of the calculated state transfer matrix and noise matrix; c. Observation update The information error is calculated based on the external input velocity observation to complete the observation update, and the chi-square check is calculated on this basis and the chi-square check value is output.

3. The method for compensating for high-precision positioning failure of commercial vehicles based on multi-source sensor fusion according to claim 1, characterized in that: Step three also includes special processing for lane lines. Specifically, the relative relationship between lines is restored based on the 3D coordinates, the distance between two adjacent lines is calculated from left to right, and this distance is divided by the lane width and rounded to the nearest integer to obtain the lane line number.

4. The method for compensating for high-precision positioning failure of commercial vehicles based on multi-source sensor fusion according to claim 3, characterized in that: The lane line number matching includes coarse matching and fine matching. The coarse matching is to match the first lane line perceived with the lane line in the map. For lane lines with numbers greater than 1, matching relationships are searched in the map in sequence according to the distance relationship. When each perceived lane line can find a corresponding matching line in the map and the lane line types are consistent, it is considered that the correct matching relationship has been found; the fine matching includes matching of lane lines, road boundaries, poles, and signs, among which the lane line matching has been completed through coarse matching; the road boundary matching is consistent with the lane line matching; the pole matching calculates the distance between the center point of the pole element and the center point of the perceived pole element, that is, the pole residual; and the card matching calculates the geometric distance between the center point of the card element and the center point of the perceived card element, that is, the card residual.

Images