Automatic driving multi-sensor fusion positioning system and method

Abstract

The invention discloses an automatic driving multi-sensor fusion positioning system and method. The system comprises a fusion positioning module, a laser radar point cloud positioning module, a combined inertial navigation positioning module, an odometer module, a foresight camera module and a map module. By comprehensively judging the positioning state of the combined inertial navigation module and the state of the laser radar point cloud positioning module, the foresight camera module recognizes lane line data, odometer module data and map module data, and a positioning mode state adapting to a current operation scene is automatically switched; the positioning mode state comprises: state 1, pure combination inertial navigation positioning; state 2, pure laser radar point cloud positioning; in the third state, combined inertial navigation positioning serves as a main positioning system, and laser radar point cloud positioning serves as a calibration system; (4) carrying out combined inertial navigation positioning and lane line calibration; and (5) carrying out laser radar point cloud positioning and lane line calibration. The method can achieve a high-precision, high-stability and high-adaptability positioning function, and is effectively suitable for various automatic driving scenes.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and more specifically, to an autonomous driving multi-sensor fusion positioning system and method. Background Art

[0002] Positioning technology is a key fundamental technology for autonomous driving systems. The accuracy of the positioning system directly affects the reliability of the autonomous driving system and has become one of the key factors restricting the commercial application of autonomous driving technology. Currently, most positioning technology solutions for autonomous driving systems adopt single integrated navigation solutions, single lidar point cloud positioning solutions, integrated navigation and lidar point cloud fusion positioning solutions, etc.

[0003] However, working conditions such as urban canyons, underpass sections of elevated roads, and dense tree-lined roads have always been pain points for in-vehicle high-precision positioning systems. The reason is that when the vehicle is driving in the above complex environments, satellite positioning signals are blocked or interfered, resulting in a significant decrease in the positioning accuracy of the integrated navigation system, or even the formation of a positioning blind area. Positioning system solutions based on integrated inertial navigation technology are prone to problems such as position drift and inability to position, seriously affecting driving safety. At the same time, the single lidar point cloud positioning system solution is easily affected by rain and fog weather and has low sensitivity to repetitive scenes such as open squares, making it difficult to meet the full-scenario positioning requirements. After some manufacturers' integrated inertial navigation systems integrate signals such as wheel speed and differential, the positioning accuracy can be greatly improved under the condition of continuous optimization in specific scenarios, but the human and material costs paid are also huge. For the fusion positioning solution based on the integrated inertial navigation system and the lidar point cloud positioning system, due to involving multiple technical fields, the integration difficulty is relatively high, and there is no completely mature solution yet.

[0004] Chinese Patent with Application No. 202410158149.6 discloses a high-precision positioning method, system and vehicle based on multi-source fusion technology. By obtaining the point cloud around the vehicle and creating a point cloud map, the position information of the vehicle in the point cloud map is obtained. The actual point cloud position information is compared with the pre-correction point cloud position information corresponding to the vehicle's position before correction in the point cloud map to obtain the positioning offset, and finally the vehicle's position is corrected through the positioning offset to obtain the final positioning of the vehicle. Although this patent solves the problem of poor applicability of multi-source fusion positioning methods in the prior art to a certain extent, its effectiveness is questionable for high-price culvert sections with poor laser point cloud positioning reliability (repetitive scenes). Summary of the Invention

[0005] The present invention provides an autonomous driving multi-sensor fusion positioning system and method to solve the above-mentioned deficiencies existing in the positioning technology of existing autonomous driving systems.

[0006] The present invention adopts the following technical solutions: An autonomous driving multi-sensor fusion positioning system, including a fusion positioning module, a lidar point cloud positioning module, an integrated inertial navigation positioning module, an odometer module, a front-view camera module, and a map module; wherein: The fusion positioning module controls the normal operation of the positioning mode switching state machine, switches the positioning mode at a reasonable time, and ensures that the vehicle can smoothly and safely pass through the signal occlusion specific working condition section in the entire operation section; this fusion positioning module conducts data interaction and forwarding with the rest of the modules; The lidar point cloud positioning module collects lidar point cloud data during the mapping stage to create a point cloud map, and collects real-time point cloud data during the autonomous driving operation stage and compares it with the point cloud map to obtain the coordinate position of the current vehicle in the map; The integrated inertial navigation positioning module collects the longitude and latitude coordinate data of the operation route during the mapping stage to create a map containing longitude and latitude coordinates, and can obtain the real-time longitude and latitude coordinate position of the vehicle in real time during the autonomous driving operation stage, and provides it to the control system, and then compares the longitude and latitude coordinate map to obtain positioning data and related control parameters; The odometer module outputs high-precision wheel speed and mileage information for the integrated inertial navigation positioning module and the lidar point cloud positioning module to call; The front-view camera module can monitor the road lane line information in real time, obtain the lane line confidence, and output the position information of the lane line relative to the vehicle center line, which is used as the basis for calibrating the vehicle's lateral coordinates under certain working conditions; The map module can record the point cloud data, longitude and latitude coordinate data, and marker data of the operation route, and inject them into the fusion positioning module when the system starts, as an objective basis for vehicle positioning matching.

[0007] Specifically, the above positioning modes include: pure integrated inertial navigation positioning, pure lidar point cloud positioning, integrated inertial navigation positioning as the main positioning system and lidar point cloud positioning as the calibration system, integrated inertial navigation positioning and lane line calibration, lidar point cloud positioning and lane line calibration.

[0008] Furthermore, the above integrated inertial navigation positioning module utilizes the position, speed, and time information provided by the satellite navigation system, fuses it with the inertial navigation system, and combines the wheel speed information output by the odometer module to achieve precise positioning and navigation of the vehicle through a specific algorithm.

[0009] Further, the above odometer module outputs high-precision wheel speeds based on the calibration parameters calculation and measurement method of a high-precision odometer system for wheel speeds. Specifically, it includes: obtaining the equivalent rolling radius of the tire under all working conditions such as tire pressure from the tire manufacturer; verifying the measured rolling radius data under several tire pressure conditions through experiments; calculating a conversion coefficient based on the above data to convert the manufacturer's rolling radius table into the rolling radius table during actual operation; and obtaining the equivalent rolling radius under the current tire pressure condition by looking up the table during operation, and then calculating the current wheel speed.

[0010] Further, the above odometer module includes a wheel speed sensor and a controller. The wheel speed sensor is used to detect the rotation speed of the tire or directly output a PWM rectangular wave, and send relevant information to the controller; the controller stores the parameters of the tire equivalent radius calibration table, and is used to obtain the wheel speed or the original pwm rectangular wave data sent by the wheel speed sensor, calculate and publish the equivalent speed of each wheel and the vehicle driving distance.

[0011] Further, the above front view camera module is a detection camera installed at the front of the vehicle. The detection camera can specifically monitor the lane line data in front of the vehicle during driving, and re-calibrate the current positioning information through the lane line assisted calibration and positioning method.

[0012] The above lane line assisted calibration and positioning method is specifically as follows: obtaining the lane line information data through the detection camera, screening out the lane line information on both sides of the lane where the current vehicle is located, calculating the offset of the vehicle from the center line of the lane based on the distances from the center line of the vehicle to the two lane lines, and multiplying this offset by the confidence level of the lane line data as feedback information to input into the fusion positioning module for positioning calibration.

[0013] The present invention also provides an autonomous driving multi-sensor fusion positioning method. Based on the above autonomous driving multi-sensor fusion positioning system, it includes: automatically switching to the positioning mode state suitable for the current operation scenario by comprehensively judging the positioning state of the combined inertial navigation module, the state of the lidar point cloud positioning module, the lane line data recognized by the front view camera module, the data of the odometer module, and the data of the map module; the positioning mode states include: State ①, pure combined inertial navigation positioning; State ②, pure lidar point cloud positioning; State ③, combined inertial navigation positioning as the main positioning system and lidar point cloud positioning as the calibration system; State ④, combined inertial navigation positioning and lane line calibration; State ⑤, lidar point cloud positioning and lane line calibration.

[0014] Specifically, the switching of the above positioning mode states is as follows: The system initialization defaults to state ①; when the duration of the inertial navigation non-fixed solution exceeds the threshold and the laser positioning confidence is good, it switches from state ① to state ②. When the laser positioning confidence is lower than the threshold and the inertial navigation is in a fixed solution, it switches from state ② to state ①; when the laser positioning confidence is good and the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ① to state ③. When the laser positioning confidence is lower than the threshold and there is no effective lane line information, it switches from state ③ to state ①; when there is effective lane line information, it switches from state ① to state ④. When there is no effective lane line information, it switches from state ④ to state ①; when the laser positioning confidence is good and the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ② to state ③. When the inertial navigation is in a non-fixed solution state and there is no effective lane line information, it switches from state ③ to state ②; when the laser positioning confidence is lower than the threshold and there is effective lane line information, it switches from state ③ to state ④. When the laser positioning confidence is good, it switches from state ④ to state ③; when there is effective lane line information, it switches from state ② to state ⑤. When there is no effective lane line information, it switches from state ⑤ to state ②; when the inertial navigation is in a non-fixed solution state and there is effective lane line information, it switches from state ③ to state ⑤. When the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ⑤ to state ③; when the duration of the inertial navigation non-fixed solution exceeds the threshold and the laser positioning confidence is good, it switches from state ④ to state ⑤. When the laser positioning confidence is lower than the threshold and the inertial navigation is in a fixed solution, it switches from state ⑤ to state ④.

[0015] As can be seen from the above description of the present invention, compared with the prior art, the present invention has the following advantages: 1. The multi-sensor fusion positioning for autonomous driving of the present invention is based on a variety of technologies such as integrated navigation positioning, lidar point cloud positioning, odometer calibration, and lane line calibration, and can achieve high-precision, high-stability, and high-self-adaptability positioning functions, and is effectively applicable to various autonomous driving scenarios such as ordinary urban roads, park tree-lined roads, elevated culvert roads, and other complex road conditions.

[0016] 2. High precision: The system of the present invention is based on a high-precision odometer (wheel speed). By fusing the integrated inertial navigation and lidar point cloud positioning data and superimposing the forward-looking camera lane line calibration, centimeter-level positioning accuracy under complex working conditions is achieved.

[0017] 3. High self-adaptability: The present invention can dynamically adjust the positioning strategy according to environmental changes. For example, in the case of elevated culvert road conditions, when the satellite signal is suddenly blocked and the positioning data confidence of the integrated inertial navigation positioning system is relatively low, the fusion system scheduling will automatically switch to the lidar point cloud positioning mode at this time, and combine the high-precision odometer and lane line detection data to dynamically calibrate the positioning data to ensure the safe passage of the autonomous driving vehicle through this section. Description of the Drawings

[0018] Figure 1This is the block diagram of the system components of the present invention.

[0019] Figure 2 This is the schematic diagram of the positioning mode switching state of the present invention. Detailed implementation manners

[0020] The following describes the detailed implementation manners of the present invention with reference to the accompanying drawings. To fully understand the present invention, many details are described below. However, for those skilled in the art, the present invention can be implemented without these details. For well-known components, methods, and processes, no further detailed description is given below.

[0021] This embodiment provides an autonomous driving multi-sensor fusion positioning system. Referring to Figure 1 , it includes a fusion positioning module 1, a lidar point cloud positioning module 2, an integrated inertial navigation positioning module 3, an odometer module 4, a front-view camera module 5, and a map module 6. Among them: The fusion positioning module 1 controls the normal operation of the positioning mode switching state machine, switches the positioning mode at a reasonable time, and ensures that the vehicle can smoothly and safely pass through the signal occlusion specific working condition section in the entire operation section; the fusion positioning module 1 performs data interaction and forwarding with the other modules.

[0022] The lidar point cloud positioning module 2 refers to a system that uses the map established by the lidar point cloud, searches for the position in the map that is closest to the real-time point cloud at the current position of the vehicle as its local coordinate, calculates the global coordinate according to the longitude and latitude coordinates stored in the map, and simultaneously calculates the pose information such as the vehicle heading, roll, and pitch. Some open-source algorithms require external calibration signals, such as the three-axis acceleration and angular velocity output by the inertial navigation system.

[0023] The main tasks of the lidar point cloud positioning module 2 mainly include: point cloud map establishment and real-time positioning based on the point cloud map. The lidar point cloud positioning module 2 collects lidar point cloud data during the mapping stage to make a point cloud map, and collects real-time point cloud data during the autonomous driving operation stage and compares it with the point cloud map to obtain the coordinate position of the current vehicle in the map.

[0024] The integrated inertial navigation positioning module 3 is a navigation system that combines a satellite navigation system and an inertial navigation system. Its core principle is to use the position, speed, and time information provided by the satellite navigation system (such as GPS, Beidou satellite navigation system, etc.), fuse it with the inertial navigation system (obtaining attitude and acceleration information through inertial sensors such as gyroscopes and accelerometers), and combine the wheel speed information output by the odometer system to achieve precise positioning and navigation of the vehicle through specific algorithms. The integrated inertial navigation positioning module 3 will give the current longitude and latitude coordinates and heading and other positioning data according to the current satellite signal and differential signal for the fusion positioning module to analyze.

[0025] The combined inertial navigation positioning module 3 collects the longitude and latitude coordinate data of the operation route during the mapping stage to create a map containing longitude and latitude coordinates. During the autonomous driving operation stage, it can obtain the real-time longitude and latitude coordinate position of the vehicle in real time, provide it to the control system, and then compare it with the longitude and latitude coordinate map to obtain positioning data and related control parameters.

[0026] The odometer module 4 mainly includes a controller and wheel speed sensors. The functions of the controller include: storing parameters such as the tire equivalent radius calibration table (the equivalent radius when the tire rolls under different tire pressures), obtaining the wheel speed (usually in revolutions per minute) or the original pwm rectangular wave data sent by the wheel speed sensors, calculating and publishing the equivalent speed of each wheel and the driving distance of the vehicle. The functions of the wheel speed sensors include: detecting how many revolutions the tire makes per minute through specific means or directly outputting the PWM rectangular wave, and sending the relevant information to the controller.

[0027] The odometer module 4 outputs high-precision wheel speed and mileage information for the combined inertial navigation positioning module 4 and the lidar point cloud positioning module 2 to call. The calibration parameter calculation and determination method of the high-precision odometer system based on wheel speed adopted in this embodiment can improve the wheel speed accuracy.

[0028] The calibration parameter calculation and determination method of the high-precision odometer system based on wheel speed is: obtain the tire equivalent rolling radius under all working conditions such as each tire pressure from the tire manufacturer, verify the measured rolling radius data under several tire pressure conditions through experiments, calculate a conversion coefficient through the above data, convert the manufacturer's rolling radius table into the rolling radius table during actual operation, and obtain the equivalent rolling radius under the current tire pressure condition by looking up the table during operation, and then calculate the current wheel speed.

[0029] The front view camera module 5 can monitor the road lane line information in real time, obtain the lane line confidence, and output the position information of the lane line relative to the vehicle center line, which is used as the basis for calibrating the vehicle's lateral coordinates under some working conditions.

[0030] The front view camera module 5 can detect targets such as lane lines, vehicles, pedestrians, and cyclists. Currently, the market maturity of the product is very high. The front view camera module 5 is preferably a detection camera installed at the front of the vehicle. The camera can specifically monitor the lane line data in front of the vehicle during driving, and re-calibrate the current positioning information through the lane line assisted calibration positioning method.

[0031] The lane line assisted calibration positioning method is: through the lane line information data obtained by the camera, screen out the lane line information on both sides of the lane where the current vehicle is located. Through the distances from the vehicle center line to the two lane lines, the offset of the vehicle from the lane center line can be calculated, and this offset is multiplied by the lane line data confidence and used as feedback information to input into the fusion positioning module for positioning calibration.

[0032] The map module 6 can record the point cloud data, longitude and latitude coordinate data, and marker data of the operation route, and inject them into the fusion positioning module when the system starts, serving as an objective basis for vehicle positioning and matching.

[0033] The map module 6 should at least include the point cloud information of the operation route and the longitude and latitude coordinate information, and can add the stop site location information, traffic light location information, speed limit information, road edge information, etc. as needed. The vehicle travels according to the map information for path planning. The map data is stored in a specific format, and the autonomous driving control program reads it through a specific program and stores it in the program structure variable, which can be called by relevant system applications.

[0034] This embodiment also provides an autonomous driving multi-sensor fusion positioning method. Based on the above autonomous driving multi-sensor fusion positioning system, it includes: automatically switching to the positioning mode state suitable for the current operation scenario by comprehensively judging the positioning status of the combined inertial navigation module, the lidar point cloud positioning module status, the front view camera module for identifying lane line data, the odometer module data, and the map module data. Refer to Figure 2 , the positioning mode states include: State ①, pure combined inertial navigation positioning; State ②, pure lidar point cloud positioning; State ③, combined inertial navigation positioning as the main positioning system and lidar point cloud positioning as the calibration system; State ④, combined inertial navigation positioning and lane line calibration; State ⑤, lidar point cloud positioning and lane line calibration.

[0035] The switching of the above positioning mode states is as follows: The system initialization defaults to state ①; when the duration of the inertial navigation non-fixed solution exceeds the threshold and the laser positioning confidence is good, it switches from state ① to state ②, and when the laser positioning confidence is lower than the threshold and the inertial navigation is in a fixed solution, it switches from state ② to state ①; when the laser positioning confidence is good and the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ① to state ③, and when the laser positioning confidence is lower than the threshold and there is no effective lane line information, it switches from state ③ to state ①; when there is effective lane line information, it switches from state ① to state ④, and when there is no effective lane line information, it switches from state ④ to state ①; when the laser positioning confidence is good and the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ② to state ③, and when the inertial navigation is in a non-fixed solution state and there is no effective lane line information, it switches from state ③ to state ②; when the laser positioning confidence is lower than the threshold and there is effective lane line information, it switches from state ③ to state ④, and when the laser positioning confidence is good, it switches from state ④ to state ③; when there is effective lane line information, it switches from state ② to state ⑤, and when there is no effective lane line information, it switches from state ⑤ to state ②; when the inertial navigation is in a non-fixed solution state and there is effective lane line information, it switches from state ③ to state ⑤, and when the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ⑤ to state ③; when the duration of the inertial navigation non-fixed solution exceeds the threshold and the laser positioning confidence is good, it switches from state ④ to state ⑤, and when the laser positioning confidence is lower than the threshold and the inertial navigation is in a fixed solution, it switches from state ⑤ to state ④.

[0036] The above is only the specific implementation manner of the present invention, but the design concept of the present invention is not limited thereto. Any non-substantive modification made to the present invention using this concept shall fall within the scope of infringement of the protection scope of the present invention.

Claims

1. An autonomous driving multi-sensor fusion positioning system, characterized by: Including fusion positioning module, lidar point cloud positioning module, combined inertial navigation positioning module, odometer module, front camera module, and map module; The fusion positioning module controls the normal operation of the positioning mode switching state machine, switches the positioning mode at a reasonable time, and ensures that the vehicle can smoothly and safely pass through the signal-blocked specific working condition section in the full operation section; the fusion positioning module interacts and forwards data with other modules; The laser radar point cloud positioning module collects laser radar point cloud data to produce a point cloud map during the mapping stage, and collects real-time point cloud data and compares the point cloud map to obtain the coordinate position of the current vehicle in the map during the automatic driving operation stage; The combined inertial navigation positioning module collects the longitude and latitude coordinate data of the operating route in the mapping stage to produce a map containing the longitude and latitude coordinates. In the automatic driving operation stage, the real-time longitude and latitude coordinate position of the vehicle can be obtained in real time and provided to the control system, and then the longitude and latitude coordinate map is compared to obtain the positioning data and related control parameters; The odometer module outputs high-precision wheel speed and mileage information for the combined inertial navigation positioning module and the laser radar point cloud positioning module to call; The front-view camera module can monitor the road lane line information in real time, obtain the lane line confidence, and output the position information of the lane line relative to the vehicle center line as the basis for calibrating the vehicle's lateral coordinates under certain working conditions; The map module can record the point cloud data, longitude and latitude coordinate data and markable object data of the operating route, and inject the fusion positioning module when the system is started as an objective basis for vehicle positioning matching.

2. The multi-sensor fusion positioning system for autonomous driving according to claim 1, characterized in that: The positioning modes include: pure combined inertial navigation positioning, pure lidar point cloud positioning, combined inertial navigation positioning as the main positioning system and lidar point cloud positioning as the calibration system, combined inertial navigation positioning and lane line calibration, and lidar point cloud positioning and lane line calibration.

3. The autonomous driving multi-sensor fusion positioning system according to claim 1, characterized in that: The combined inertial navigation positioning module utilizes the position, speed and time information provided by the satellite navigation system, integrates it with the inertial navigation system, and combines it with the wheel speed information output by the odometer module to achieve precise positioning and navigation of the vehicle through a specific algorithm.

4. The autonomous driving multi-sensor fusion positioning system according to claim 1, characterized in that: The odometer module outputs high-precision wheel speed, which is a high-precision odometer system calibration parameter calculation and measurement method based on wheel speed, specifically: obtaining the equivalent rolling radius of the tire under all working conditions such as tire pressure of the tire manufacturer; verifying the measured rolling radius data under several tire pressure conditions through experiments; calculating a conversion coefficient through the above data, and converting the manufacturer's rolling radius table into the rolling radius table in the actual operation process; During operation, the equivalent rolling radius under the current tire pressure condition is obtained by looking up the table, and then the current wheel speed is calculated.

5. The autonomous driving multi-sensor fusion positioning system according to claim 1, characterized in that: The odometer module includes a wheel speed sensor and a controller. The wheel speed sensor is used to detect the rotation speed of the tire or directly output a PWM rectangular wave, and send the relevant information to the controller; the controller stores the parameters of the tire equivalent radius calibration table, and is used to obtain the wheel speed or original PWM rectangular wave data sent by the wheel speed sensor, calculate and publish the equivalent speed of each wheel and the vehicle travel distance.

6. The autonomous driving multi-sensor fusion positioning system according to claim 1, characterized in that: The front-view camera module is a detection camera installed at the front of the vehicle. The detection camera can specifically monitor the lane line data ahead and recalibrate the current positioning information through the lane line auxiliary calibration positioning method.

7. The multi-sensor fusion positioning system for autonomous driving according to claim 6, characterized in that: The lane line auxiliary calibration and positioning method is specifically as follows: the lane line information data obtained by the detection camera is used to filter out the lane line information on both sides of the lane where the current vehicle is located, and the offset of the vehicle from the center line of the lane is calculated by the distance between the center line of the vehicle and the two lane lines. The offset is multiplied by the lane line data confidence level and input into the fusion positioning module as feedback information for positioning calibration.

8. An autonomous driving multi-sensor fusion positioning method, based on the autonomous driving multi-sensor fusion positioning system according to claim 1, characterized in that: include: By comprehensively judging the positioning status of the combined inertial navigation module, the status of the laser radar point cloud positioning module, the lane line data recognized by the front-view camera module, the odometer module data, and the map module data, it automatically switches to the positioning mode state that adapts to the current operating scenario; the positioning mode states include: state ①, pure combined inertial navigation positioning; state ②, pure laser radar point cloud positioning; state ③, combined inertial navigation positioning as the main positioning system and laser radar point cloud positioning as the calibration system; state ④, combined inertial navigation positioning and lane line calibration; state ⑤, laser radar point cloud positioning and lane line calibration.

9. The multi-sensor fusion positioning method for autonomous driving according to claim 8, characterized in that: The switching of the positioning mode state includes the following: the system is initialized to state ① by default; when the duration of the non-fixed solution of the inertial navigation exceeds the threshold and the laser positioning reliability is good, it switches from state ① to state ②, and when the laser positioning reliability is lower than the threshold and the inertial navigation is a fixed solution, it switches from state ② to state ①; when the laser positioning reliability is good and the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ① to state ③, and when the laser positioning reliability is lower than the threshold and there is no valid lane line information, it switches from state ③ to state ①; when there is valid lane line information, it switches from state ① to state ④, and when there is no valid lane line information, it switches from state ④ to state ①; when the laser positioning reliability is good and the duration of the inertial navigation fixed solution exceeds the threshold, it switches from state ② to state ③, and when the inertial navigation When the solution is not fixed and there is no valid lane line information, it switches from state ③ to state ②; when the laser positioning confidence is lower than the threshold and there is valid lane line information, it switches from state ③ to state ④, and when the laser positioning confidence is good, it switches from state ④ to state ③; when there is valid lane line information, it switches from state ② to state ⑤, and when there is no valid lane line information, it switches from state ⑤ to state ②; when the inertial navigation is not in a fixed solution state and there is valid lane line information, it switches from state ③ to state ⑤, and when the inertial navigation fixed solution duration exceeds the threshold, it switches from state ⑤ to state ③; when the inertial navigation non-fixed solution duration exceeds the threshold and the laser positioning confidence is good, it switches from state ④ to state ⑤, and when the laser positioning confidence is lower than the threshold and the inertial navigation is a fixed solution, it switches from state ⑤ to state ④.

Images