Multi-Sensor Fusion Localization Method and Device under Fog Interference

Abstract

The multi-sensor fusion localization method and device under fog interference provided by the present disclosure jointly input the clear scene LiDAR point cloud and the scene visibility into a fog point cloud generation model to obtain the fog point cloud of the current scene; use the fog point cloud to identify the current visibility of the scene and determine whether the LiDAR odometer is interfered by fog; for the case where the LiDAR odometer is interfered by fog, first use sequential filtering to calculate the predicted one-step residual and the standard deviation of each state in the LiDAR odometer measurement vector; then, according to the mismatch between the predicted one-step residual and the standard deviation, apply the 3σ criterion to divide these states into no mismatch, mild mismatch, and severe mismatch; finally, use the EKF to fuse the states with no mismatch, use the AEKF to fuse the states with mild mismatch, and isolate the states with severe mismatch. The present disclosure solves the expected functional safety problem of divergence of LiDAR-based multi-sensor fusion localization under fog interference conditions.

Description

Technical Field

[0001] The present disclosure relates to the technical field of multi-sensor fusion positioning for autonomous vehicles, and particularly to a multi-sensor fusion positioning method and device under fog interference. Background Art

[0002] As one of the core technologies of autonomous driving, multi-sensor fusion positioning technology can effectively solve the positioning problem in scenarios where a single sensor fails or has insufficient functions, and is the basis for vehicle autonomous navigation. Among them, the multi-sensor positioning technology based on LIDAR has the advantages of being less affected by changes in lighting conditions, being able to accurately perceive environmental features in 3D, and having high positioning accuracy, so it is widely used. However, in foggy conditions, the laser beam emitted by LIDAR will be absorbed, scattered, or refracted by fog droplets, causing the measurement information of the LIDAR odometer to be interfered, and further causing the fusion positioning to diverge. After the existing fault diagnosis and isolation methods identify that there is an abnormality in the measurement information of the LIDAR odometer, they immediately consider that the LIDAR has failed and directly isolate it, which will inevitably result in waste of the undisturbed part of the measurement information of the LIDAR odometer. Therefore, under fog interference conditions, it is a key problem to be solved to propose a functional improvement strategy that can accurately identify whether the measurement information of the LIDAR odometer is interfered and suppress the divergence of the LIDAR-based fusion positioning system. Summary of the Invention

[0003] In order to ensure the expected functional safety (SOTIF) of LIDAR-based multi-sensor fusion positioning under fog interference conditions, the present disclosure endeavors to propose a LIDAR-based multi-sensor fusion positioning method and device under fog interference conditions, aiming to identify whether the measurement information of the LIDAR odometer is interfered by fog and make full use of the undisturbed part of the measurement information of the LIDAR odometer on the premise of ensuring that the fusion positioning does not diverge.

[0004] To achieve the above object, the multi-sensor fusion positioning method under fog interference provided in the first aspect embodiment of the present disclosure includes:

[0005] Construct a fog point cloud generation model for generating a fog point cloud of the scene where the autonomous vehicle is located considering both fog attenuation and noise through numerical simulation according to the clear laser point cloud of the scene where the autonomous vehicle is located obtained by the LiDAR sensor and the visibility of the scene;

[0006] Use the fog point cloud generation model to generate a fog point cloud of the current scene considering both fog attenuation and noise, and obtain the one-step prediction state vector and its mean square error matrix of the reference sensor at the current moment, and the measurement vector and its mean square error matrix of the LIDAR odometer, where the reference sensor is a sensor mounted on the autonomous vehicle and not affected by fog;

[0007] The extinction coefficient inversion formula is used to calculate the extinction coefficient of each laser point in the fog point cloud of the current scene, and based on this, the visibility of each laser point in the fog point cloud is calculated. The visibility of the laser points with a detection distance greater than the set detection distance threshold is screened out, and the average is calculated as the visibility of the identified current scene;

[0008] Compare the visibility of the identified current scene with the set visibility threshold. If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using a variance mismatch degree hierarchical adaptive Kalman filter based on the 3σ criterion as the fused positioning result; if the visibility of the current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using an extended Kalman filter as the fused positioning result. :

[0009] The multi-sensor fusion positioning method under fog interference provided in the first aspect of the present disclosure has the following characteristics and beneficial effects:

[0010] The present invention proposes a multi-sensor fusion positioning method under fog interference. Compared with the existing method, the present invention converts the clear laser point cloud of the scene obtained by the LiDAR numerical model into a fog point cloud that considers fog noise and decay according to the set visibility; uses the LiDAR echo point cloud to identify the current visibility of the scene and judge whether the LiDAR odometer is interfered by fog; uses sequential filtering and the 3σ criterion to grade the variance mismatch degree of each state in the LiDAR odometer measurement vector, and adaptively fuses these states in different ways to retain as much of the undisturbed part of the measurement information of the LiDAR odometer as possible and suppress the divergence of the LiDAR-based fusion positioning system.

[0011] In some embodiments, the method of generating a fog point cloud of a current scene taking into account fog decay and noise by using a fog point cloud generation model comprises the following steps:

[0012] 21) The visibility V' and laser wavelength λ of the current scene are jointly input into the fog decay simulation module in the fog point cloud generation model to obtain the extinction coefficient α of the current scene. The calculation formula is:

[0013]

[0014] 22) The extinction coefficient α of the current scene is compared with the detection distance x of each laser point p in a frame of clear laser point cloud. pThe echo energy simulation module in the input fog point cloud generation model obtains the echo energy of each laser point p in the clear laser point cloud The calculation formula is:

[0015]

[0016]

[0017] Among them, C L is an inherent property of the LiDAR sensor, P0 is the single-beam laser emission energy of the LiDAR sensor, A R is the effective area of the LiDAR receiver, η sr is the efficiency of the LiDAR receiver, η st is the efficiency of the LiDAR transmitter, ρ TAR is the reflectivity of the target detected by the LiDAR sensor;

[0018] 23) Input the echo energy of each laser point p in the clear laser point cloud into the fog noise simulation module in the fog point cloud generation model, and calculate the signal-to-noise ratio SNR of the echo signal of each laser point p , and the calculation formula is:

[0019]

[0020]

[0021]

[0022]

[0023] Among them, is the number of effective signal photons in the echo signal of each laser point ρ in the clear laser point cloud, τ is the pulse time of the LiDAR sensor, η is the quantum efficiency of the LiDAR receiver, e is the single-photon energy; F is the noise factor of the LiDAR detector; N B is the number of background light noise signal photons, P B is the sky background radiation brightness, θ is the field of view angle of the LiDAR receiver, Δλ is the filter bandwidth of the LiDAR sensor, A R is the effective area of the LiDAR receiver, ΔX is the spatial resolution of the LiDAR sensor, c is the speed of light, h is the Planck constant; N D is the dark count of the LiDAR receiver, C D is the dark count rate;

[0024] 24) According to the signal-to-noise ratio SNR of the echo signal of each laser point pDetermine whether the detection distance and echo energy of the laser point p are valid based on the magnitude of the set threshold SNR0, and obtain the effective fog decay laser point cloud after removing the invalid information.

[0025] 25) Add ranging noise to the effective fog decay laser point cloud to obtain a laser point cloud that simultaneously considers fog decay and noise, which serves as the fog point cloud of the current scene.

[0026] In some embodiments, the one-step prediction state vector and its mean square error matrix of the reference sensor are the one-step prediction state vector and its mean square error matrix of a single reference sensor, or the one-step prediction fusion vector and its mean square error matrix of multiple reference sensors.

[0027] In some embodiments, step 24) includes:

[0028] If SNR p < SNR0, then determine that the detection distance and echo energy of the laser point p are invalid, and remove this invalid laser point from the clear laser point cloud as an invalid laser point; if SNR p ≥ SNR0, then the detection distance and echo energy of the laser point p are valid, and it serves as a valid laser point; use all valid laser points to form an effective fog decay laser point cloud.

[0029] In some embodiments, step 25) includes:

[0030] According to the signal-to-noise ratio SNR j of the laser point j in the effective fog decay laser point cloud, calculate the uncertainty of the detection distance of the effective fog decay laser point cloud, which satisfies the following distribution:

[0031]

[0032] Add the uncertainty of the detection distance of the effective fog decay laser point cloud to the detection distance x j of the laser point j, and finally obtain a laser point cloud that simultaneously considers fog decay and noise, where the method of adding ranging noise is is the detection distance of the laser point j with noise, and rd is a random number with a standard normal distribution.

[0033] In some embodiments, the visibility of the identified current scene is obtained according to the following steps:

[0034] 31) For the fog point cloud that simultaneously considers fog decay and noise with n laser points in a frame at time k, use the extinction coefficient inversion formula to calculate the extinction coefficient α j of a certain laser point j among them:

[0035]

[0036]

[0037] Among them, x j and are the detection distance and echo energy of the laser point j respectively, C L is an inherent property of the LiDAR sensor, P0 is the single-beam laser emission energy of the LiDAR sensor, A R is the effective area of the LiDAR receiver, η sr is the efficiency of the LiDAR receiver, η st is the efficiency of the LiDAR transmitter, ρ TAR is the reflectivity of the target detected by the LiDAR sensor;

[0038] 32) Use the visibility inversion formula to calculate the visibility V identified by the laser point j according to the extinction coefficient α j of the laser point j: j :

[0039]

[0040] Among them, λ is the laser wavelength;

[0041] 33) Repeat steps 31) to 32) until all n laser points are traversed;

[0042] 34) Obtain the detection distance threshold dth that meets the accuracy requirements through experiments or simulation tests, and screen out the visibility V of the laser points with a detection distance greater than the detection distance threshold dth, and find its average value V as the visibility V of the currently identified scene. The calculation formula is as follows: q Among them, N is the number of laser points with a detection distance greater than the detection distance threshold dth in the fog point cloud considering both fog attenuation and noise.

[0043]

[0044] In some embodiments, when it is determined that the measurement vector of the LiDAR odometer is interfered by fog, the fused positioning result is obtained according to the following steps:

[0045]

[0046] 421) Use sequential filtering to calculate the one-step prediction residual k of each state in the measurement vector Z of the LiDAR odometer and the standard deviation

[0047] 422) According to the one-step prediction residual of the state The corresponding standard deviation The magnitude of is used to classify the variance mismatch degree of the state according to the 3σ criterion, specifically as follows: The variance mismatch degree of the state is classified as follows:

[0048] If Then it is determined that the state has no variance mismatch; if Then it is determined that the state has a mild variance mismatch; if Then it is determined that the state has a severe variance mismatch;

[0049] 423) According to the classification result of the state The state and the one-step prediction state vector X of the reference sensor k The corresponding state in are fused, specifically as follows:

[0050] If the state has no variance mismatch, the extended Kalman filter is used to fuse the state and the state If the state has a mild variance mismatch, the adaptive extended Kalman filter is used to fuse the state and the state If the state has a severe variance mismatch, then the state

[0051] 424) If all the states in the measurement vector Z of the LiDAR odometer k are traversed, the estimated state vector and its mean square error matrix are obtained as the fused positioning result.

[0052] In some embodiments, calculating the one-step prediction residual of each state k in the measurement vector Z of the LiDAR odometer using sequential filtering and the standard deviation of the residual Specifically includes: Specifically includes:

[0053] 4211) Using the measurement vector Z of the LiDAR odometer k a certain state and the one-step prediction state vector X of the reference sensor k the corresponding state in to calculate the one-step prediction residual of the state The calculation formula is: The calculation formula is:

[0054]

[0055] Among them, H i is the i-th row of the Jacobian matrix;

[0056] 422) Use the mean square error matrix P of the one-step predicted state vector of the reference sensor k of a certain diagonal element in and the mean square error matrix R of the measurement vector of the LiDAR odometer k of the corresponding diagonal element in to calculate the state of the one-step predicted residual corresponding standard deviation The calculation formula is:

[0057]

[0058] The multi-sensor fusion positioning device under fog interference provided by the second aspect embodiment of the present disclosure includes:

[0059] The first module is configured to construct a fog point cloud generation model for generating a fog point cloud of the scene where the autonomous vehicle is located considering both fog decay and noise by numerical simulation according to the clear laser point cloud of the scene where the autonomous vehicle is located obtained by the LiDAR sensor and the visibility of the scene;

[0060] The second module is configured to use the fog point cloud generation model to generate a fog point cloud of the current scene considering both fog decay and noise, and obtain the one-step predicted state vector and its mean square error matrix of the reference sensor at the current moment, the measurement vector and its mean square error matrix of the LiDAR odometer, and the reference sensor is a sensor mounted on the autonomous vehicle and not affected by fog;

[0061] The third module is configured to calculate the extinction coefficient identified by each laser point in the fog point cloud of the current scene using the extinction coefficient inversion formula, and calculate the visibility identified by each laser point in the fog point cloud based on this using the visibility inversion formula, screen out the visibility identified by the laser points with a detection distance greater than the set detection distance threshold, and find its average value as the visibility of the identified current scene;

[0062] The fourth module is configured to compare the visibility of the identified current scene with a set visibility threshold. If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using variance mismatch degree hierarchical adaptive Kalman filtering based on the 3σ criterion as the fused positioning result. If the visibility of the identified current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using extended Kalman filtering as the fused positioning result.

[0063] The computer-readable storage medium provided by the third aspect embodiment of the present disclosure stores computer instructions for causing the computer to execute the multi-sensor fusion positioning method according to any one of the embodiments in the first aspect of the present disclosure. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] The drawings are only for the purpose of illustrating specific embodiments and are not considered to be a limitation of the present application.

[0065] Figure 1 It is the overall flowchart of the fusion positioning method provided by the first aspect embodiment of the present disclosure.

[0066] Figure 2 It is the flowchart of generating fog point cloud in the fusion positioning method provided by the first aspect embodiment of the present disclosure.

[0067] Figure 3 It is the plane simulation diagram of the application scenario of the fusion positioning method provided by the first aspect embodiment of the present disclosure.

[0068] Figure 4 It is the 3D simulation diagram of the application scenario of the fusion positioning method provided by the first aspect embodiment of the present disclosure.

[0069] Figure 5 It is the structural schematic diagram of the electronic device provided by the third aspect embodiment of the present disclosure. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0070] In order to make the purpose, technical solutions and advantages of the present disclosure clearer, the present disclosure will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present disclosure and are not used to limit the present disclosure.

[0071] Rather, this disclosure covers any alternatives, modifications, equivalent methods, and schemes defined by the claims that are within the spirit and scope of this disclosure. Further, in order to enable the public to better understand this disclosure, in the following detailed description of this disclosure, some specific details are described in detail. Those skilled in the art can fully understand this disclosure without the description of these details.

[0072] See Figure 1 , which is the overall flowchart of the multi-sensor fusion positioning method under fog interference provided by the embodiment of the first aspect of this disclosure. In the autonomous vehicle of the embodiment of this disclosure, a LiDAR sensor affected by fog drying and a sensor not affected by fog interference are installed. The sensor not affected by fog interference is used as the reference sensor. The sensors not affected by fog interference include an inertial navigation system and a global satellite positioning and navigation system. The multi-sensor fusion positioning method provided by the embodiment of the first aspect of this disclosure includes:

[0073] 1) Construct a fog point cloud generation model for generating a fog point cloud of the scene where the autonomous vehicle is located that simultaneously considers fog attenuation and noise through numerical simulation according to the clear laser point cloud of the scene where the autonomous vehicle is located obtained by the LiDAR sensor and the visibility of the scene;

[0074] 2) Use the constructed fog point cloud generation model to generate a fog point cloud of the current scene that simultaneously considers fog attenuation and noise, and obtain the one-step prediction state vector and its mean square error matrix of the reference sensor on the autonomous vehicle at the current moment, the measurement vector of the LiDAR odometer, and its mean square error matrix;

[0075] 3) Use the extinction coefficient inversion formula to calculate the extinction coefficient identified by each laser point in the fog point cloud of the current scene, and based on this, use the visibility inversion formula to calculate the visibility identified by each laser point in the fog point cloud. Screen out the visibility identified by the laser points with a detection distance greater than the set detection distance threshold, and find its mean value as the visibility of the identified current scene;

[0076] 4) Compare the visibility of the current scene with the set visibility threshold: If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using the variance mismatch degree hierarchical adaptive Kalman filter based on the 3σ criterion (3σ-SVGM-AEKF, 3σ-SVGM-AEKF) as the fused positioning result; If the visibility of the identified current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using the Extended Kalman Filter (EKF) as the fused positioning result.

[0077] In some embodiments, let the one-step prediction state vector and its mean square error matrix of the reference sensor on the autonomous vehicle at the current moment be X k and P k , and let the measurement vector and its mean square error matrix of the LiDAR odometer on the autonomous vehicle at the current moment be Z k and R k . Among them, the one-step prediction state vector X k and its mean square error matrix P k can be the one-step prediction state vector and its mean square error matrix of a single reference sensor, or can be the one-step prediction state fusion vector and its mean square error matrix of multiple reference sensors. The specific fusion method can adopt the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), Particle Filter (PF), etc.

[0078] In some embodiments, refer to Figure 2 , for the flowchart of generating the fog point cloud of the current scene by the fog point cloud generation model, Figure 3 , Figure 4 , for the plane simulation diagram and 3D simulation diagram of the scene where the autonomous vehicle is located. The points A and B in Figure 3 represent the starting point and the ending point of the driving path of the autonomous vehicle respectively, and the color blocks with different grayscales in Figure 3 are the distribution areas of the building groups around the scene where the autonomous vehicle is located. Generate the fog point cloud of the current scene considering both fog decay and noise using the constructed fog point cloud generation model, including the following steps:

[0079] (21) Jointly input the visibility V' (set by oneself) of the current scene and the laser wavelength λ into the fog attenuation simulation module in the fog point cloud generation model to obtain the extinction coefficient α of the current scene. The calculation formula of the fog attenuation simulation module is:

[0080]

[0081] where λ is the laser wavelength;

[0082] (22) Input the extinction coefficient α of the current scene and the detection distance x of each laser point p in a frame of clear laser point cloud p into the echo energy simulation module in the fog point cloud generation model to calculate the echo energy of each laser point p in the clear laser point cloud The calculation formula is:

[0083]

[0084]

[0085] where C L is an inherent property of the LiDAR sensor, P0 = 1.6 μJ is the single-beam laser emission energy of the LiDAR sensor, A R = 10 cm 2 is the effective area of the LiDAR receiver, η sr = 0.3 is the efficiency of the LiDAR receiver, η st = 0.8 is the efficiency of the LiDAR transmitter, ρ TAR is the reflectivity of the target detected by the LiDAR sensor, ρ TAR is related to the material of the target and takes values between 0.2 and 0.8;

[0086] (23) Input the echo energy of each laser point p in the clear laser point cloud into the fog noise simulation module in the fog point cloud generation model to calculate the signal-to-noise ratio SNR of the echo signal of each laser point p , and the calculation formula is:

[0087]

[0088]

[0089]

[0090]

[0091] where To clarify the number of effective signal photons in the echo signal of each laser point p in the laser point cloud, τ is the pulse time of the LiDAR sensor, η is the quantum efficiency of the LiDAR receiver, and e is the single-photon energy; F is the noise factor of the LiDAR detector, optionally, F = 3; N B is the number of background light noise signal photons, P B is the sky background radiation brightness, θ is the field of view angle of the LiDAR receiver, Δλ is the filter bandwidth of the LiDAR sensor, A R is the effective area of the LiDAR receiver, ΔX is the spatial resolution of the LiDAR sensor, c is the speed of light, and h is the Planck constant; N D is the dark count of the LiDAR receiver, C D = 300 is the dark count rate; each of the above parameters is set according to the LiDAR design manual and relevant standards;

[0092] 24) According to the signal-to-noise ratio SNR p of the echo signal of each laser point, judge whether the detection distance and echo energy of the laser point p are effective by comparing with the set threshold SNR0, and remove the invalid information to obtain the effective fog-decayed laser point cloud. Specifically:

[0093] If SNR p < SNR0, the detection distance and echo energy of this laser point are invalid, and it is regarded as an invalid laser point, and this invalid laser point is removed from the clear laser point cloud; if SNR p ≥ SNR0, the detection distance and echo energy of this laser point are effective, and it is regarded as an effective laser point; use all effective laser points to form an effective fog-decayed laser point cloud;

[0094] 25) Add ranging noise to the effective fog-decayed laser point cloud to obtain a laser point cloud that takes into account both fog decay and noise, as the fog point cloud of the current scene. Specifically:

[0095] According to the signal-to-noise ratio SNR j of the laser point j in the effective fog-decayed laser point cloud, calculate the uncertainty of the detection distance of the effective fog-decayed laser point cloud, which satisfies the following distribution:

[0096]

[0097] After that, add the uncertainty of the detection distance of the effective fog-decayed laser point cloud to the detection distance x j of the laser point j, and finally obtain a laser point cloud that takes into account both fog decay and noise. Among them, the way of adding ranging noise is is the detection distance of the laser point j with noise, and rd is a random number of the standard normal distribution.

[0098] Furthermore, the LiDAR numerical model provided by MATLAB can be used to obtain the clear laser point cloud of the current scene of the autonomous vehicle.

[0099] According to the fog point cloud generation model constructed in the embodiments of the present disclosure, the fog point cloud of the current scene that simultaneously considers fog attenuation and noise has the following advantages: 1. The visibility value corresponding to the fog point cloud is adjustable; 2. In addition to considering the influence of fog attenuation, the influence of noise is also considered; 3. The calculation amount is small, and it can be conveniently integrated into different LiDAR numerical models.

[0100] In some embodiments, refer to Figure 1 , and the visibility of the recognized current scene is obtained according to the following steps:

[0101] 31) For the fog point cloud of a frame with n laser points at time k that simultaneously considers fog attenuation and noise, calculate the extinction coefficient α recognized by a certain laser point j among them j , and the calculation formula after inversion is:

[0102]

[0103] where x j and are the detection distance and echo energy of the laser point j respectively, C L is the inherent property of the LiDAR sensor, and ρ TAR is the reflectivity of the target detected by the LiDAR sensor;

[0104] 32) Calculate the visibility V recognized by the laser point j using the extinction coefficient α recognized by the laser point j j , and the calculation formula after inversion is: j , and the calculation formula after inversion is:

[0105]

[0106] where λ is the laser wavelength;

[0107] 33) Repeat the above process until all n laser points are traversed;

[0108] 34) The fog noise contained in the fog point cloud results in a lower visibility recognition accuracy for the laser points with a shorter detection distance and a higher visibility recognition accuracy for the laser points with a longer detection distance. Based on this, the detection distance threshold dth that meets the accuracy requirements is obtained through experiments or simulation tests; the visibility V of the laser points with a detection distance greater than the detection distance threshold dth qFilter them out and calculate their mean value V, which is used as the visibility V of the recognized current scene. The calculation formula is as follows:

[0109]

[0110] Among them, N is the number of laser points in the fog point cloud that simultaneously consider fog attenuation and noise and have a detection distance greater than the detection distance threshold dth.

[0111] According to the method for calculating the visibility of the recognized current scene provided by the embodiments of the present disclosure, it has the following advantages: 1. Small computational amount; 2. Not easily affected by noise, and high visibility recognition accuracy.

[0112] In some embodiments, referring to Figure 1 , step 4) specifically includes the following steps:

[0113] 41) Compare the visibility V of the recognized current scene with the set visibility threshold Th. If V ≤ Th, it is considered that the measurement vector Z of the LiDAR odometer k is interfered by fog, and step 42) is executed. Otherwise, it is considered that the measurement vector Z of the LiDAR odometer k is normal, and step 43) is executed;

[0114] 42) Use 3σ - SVGM - AEKF to fuse the measurement vector Z of the LiDAR odometer k with the one - step prediction state vector X of the reference sensor k to obtain the estimated state vector and its mean square error matrix as the fusion positioning result. Specifically:

[0115] 421) Use sequential filtering to calculate the one - step prediction residual k of each state in the measurement vector Z of the LiDAR odometer and the standard deviation of this residual. Specifically include:

[0116] 4211) Use a certain state k in the measurement vector Z of the LiDAR odometer and the corresponding state k in the one - step prediction state vector X of the reference sensor to calculate the one - step prediction residual of state The calculation formula is:

[0117]

[0118] Among them, H i is the i - th row of the Jacobian matrix;

[0119] 4212) Calculate the mean square error matrix P of the one-step predicted state vector using the reference sensor k for a certain diagonal element and the mean square error matrix R of the measurement vector of the LiDAR odometer k for the corresponding diagonal element Calculate the state for the one-step predicted residual for the corresponding standard deviation The calculation formula is:

[0120]

[0121] 422) According to the state for the one-step predicted residual and its corresponding standard deviation use the 3σ criterion to classify the variance mismatch degree of the state Specifically:

[0122] If then it is determined that the state has no variance mismatch; if then it is determined that the state has a mild variance mismatch; if then it is determined that the state has a severe variance mismatch;

[0123] 423) According to the classification result of the state fuse the state with Specifically:

[0124] If the state has no variance mismatch, use the Extended Kalman Filter (EKF) to fuse the state with the state If the state has a mild variance mismatch, use the Adaptive Extended Kalman Filter (AEKF) to fuse the state with the state If the state has a severe variance mismatch, then isolate the state

[0125] 424) If all the states in the measurement vector Z of the LiDAR odometer k are traversed, then output the estimated state vector and its mean square error matrix

[0126] 43) Use the EKF to fuse the measurement vector Z of the LiDAR odometer kThe one-step prediction state vector X of the reference sensor k , output the estimated state vector and its mean square error matrix

[0127] According to the fusion method provided by the embodiments of the present disclosure, it has the following advantages: 1. Sequential filtering is used to traverse all states, and the recognition accuracy of mismatched states is high; 2. The noise adaptive method is used to retain as much effective information as possible in the LiDAR measurement state; 3. The hierarchical method is used to isolate severely mismatched states to avoid the introduction of incorrect information.

[0128] The multi-sensor fusion positioning device under fog interference provided by the second aspect of the present disclosure includes:

[0129] The first module is configured to construct a fog point cloud generation model for generating a fog point cloud of the scene where the autonomous vehicle is located considering both fog attenuation and noise through numerical simulation according to the clear laser point cloud of the scene where the autonomous vehicle is located obtained by the LiDAR sensor and the visibility of the scene;

[0130] The second module is configured to use the constructed fog point cloud generation model to generate a fog point cloud of the current scene considering both fog attenuation and noise, and obtain the one-step prediction state vector and its mean square error matrix of the reference sensor on the autonomous vehicle at the current moment, the measurement vector and its mean square error matrix of the LiDAR odometer, and the reference sensor is a sensor mounted on the autonomous vehicle and not affected by fog;

[0131] The third module is configured to calculate the extinction coefficient identified by each laser point in the fog point cloud of the current scene using the extinction coefficient inversion formula, and based on this, calculate the visibility identified by each laser point in the fog point cloud using the visibility inversion formula, screen out the visibility identified by the laser points with a detection distance greater than the set detection distance threshold, and calculate its mean value as the visibility of the identified current scene;

[0132] The fourth module is configured to compare the visibility of the identified current scene with the set visibility threshold. If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using variance mismatch degree hierarchical adaptive Kalman filtering based on the 3σ criterion as the fusion positioning result; if the visibility of the identified current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer is fused with the one-step prediction state vector of the reference sensor using extended Kalman filtering as the fusion positioning result.

[0133] To implement the above embodiments, an embodiment of the present disclosure also provides a computer-readable storage medium, on which a computer program is stored, and the program is executed by a processor to perform the multi-sensor fusion positioning method under fog interference in the above embodiments.

[0134] Reference is made below Figure 5 to FIG. [not shown], which shows a schematic structural diagram of an electronic device suitable for implementing the embodiments of the present disclosure. It should be noted that the electronic device in the embodiments of the present disclosure may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Tablet Computers), PMPs (Portable Multimedia Players), in-vehicle terminals (such as in-vehicle navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, servers, etc. Figure 5 The electronic device shown is merely an example and should not impose any limitations on the functions and usage scope of the embodiments of the present disclosure.

[0135] As Figure 5 shown, the electronic device may include a processing device (such as a central processing unit, a graphics processing unit, etc.) 101, which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 102 or the program loaded from the storage device 108 into the random access memory (RAM) 103. In the RAM 103, various programs and data required for the operation of the electronic device are also stored. The processing device 101, the ROM 102, and the RAM 103 are connected to each other through a bus 104. The input / output (I / O) interface 105 is also connected to the bus 104.

[0136] Generally, the following devices may be connected to the I / O interface 105: an input device 106 including, for example, a touch screen, a touchpad, a keyboard, a mouse, a camera, a microphone, etc.; an output device 107 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 108 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 109. The communication device 109 can allow the electronic device to communicate with other devices wirelessly or wiredly to exchange data. Although Figure 5 FIG. [not shown] shows an electronic device having various devices, it should be understood that it is not required to implement or include all the shown devices. Instead, more or fewer devices may be implemented or included.

[0137] In particular, according to an embodiment of the present disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, this embodiment includes a computer program product that includes a computer program carried on a computer-readable medium, and the computer program contains program code for performing the methods shown in the flowcharts. In such an embodiment, the computer program can be downloaded and installed from the network via the communication device 109, or installed from the storage device 108, or installed from the ROM 102. When the computer program is executed by the processing device 101, the above-mentioned functions defined in the methods of the embodiments of the present disclosure are performed.

[0138] It should be noted that the above-mentioned computer-readable medium in the present disclosure can be a computer-readable signal medium, a computer-readable storage medium, or any combination of the two. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples of the computer-readable storage medium can include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, the computer-readable storage medium can be any tangible medium that contains or stores a program, and the program can be used by or in conjunction with an instruction execution system, apparatus, or device. In the present disclosure, the computer-readable signal medium can include a data signal propagated in a baseband or as part of a carrier wave, which carries computer-readable program code. Such a propagated data signal can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, and the computer-readable signal medium can send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted by any suitable medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.

[0139] The above-mentioned computer-readable medium can be included in the above-mentioned electronic device; or it can exist separately and not be assembled into the electronic device.

[0140] The computer-readable medium carries one or more programs. When the one or more programs are executed by the electronic device, the electronic device: constructs a fog point cloud generation model, which is used to generate a fog point cloud of the scene where the autonomous driving car is located by a numerical simulation method based on the clear laser point cloud of the scene where the autonomous driving car is located obtained by the LiDAR sensor and the visibility of the scene, while taking into account fog decay and noise; uses the constructed fog point cloud generation model to generate a fog point cloud of the current scene while taking into account fog decay and noise, obtains the one-step predicted state vector and its mean square error matrix of the reference sensor on the autonomous driving car at the current moment, and the measurement vector and its mean square error matrix of the LiDAR odometer, wherein the reference sensor is a sensor mounted on the autonomous driving car and is not affected by fog; uses the extinction coefficient inversion formula to calculate the extinction coefficient of each laser point identified in the fog point cloud of the current scene, and uses the visibility inversion formula as a basis to calculate the extinction coefficient of each laser point identified in the fog point cloud of the current scene The visibility of each laser point in the fog point cloud is calculated by an algorithm, and the visibility of the laser points whose detection distance is greater than the set detection distance threshold is screened out, and the average is calculated as the visibility of the identified current scene; the visibility of the identified current scene is compared with the set visibility threshold. If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer is fused with the one-step predicted state vector of the reference sensor using a variance mismatch degree graded adaptive Kalman filter based on the 3σ criterion as the fused positioning result; if the visibility of the identified current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer is fused with the one-step predicted state vector of the reference sensor using an extended Kalman filter as the fused positioning result.

[0141] Computer program code for performing the operations of the present disclosure may be written in one or more programming languages, or a combination thereof, including object-oriented programming languages, such as Java, Smalltalk, C++, python, and conventional procedural programming languages, such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., through the Internet using an Internet service provider).

[0142] In the description of this specification, the descriptions referring to terms such as "one embodiment", "some embodiments", "examples", "specific examples", or "some examples", etc., mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of this application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0143] In addition, the terms "first" and "second" are used only for descriptive purposes and cannot be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, "a plurality of" means at least two, such as two, three, etc., unless otherwise specifically defined.

[0144] Any process or method description shown in a flowchart or described in other ways herein can be understood to represent a module, segment, or portion of code including one or more executable instructions for implementing a specific logical function or process, and the scope of the preferred embodiments of this application includes additional implementations, where the functions can be executed in a manner that is not in the order shown or discussed, including in a substantially simultaneous manner or in a reverse order according to the functions involved, which should be understood by those skilled in the art to which the embodiments of this application pertain.

[0145] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a definable sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch and execute instructions from the instruction execution system, apparatus, or device), or in conjunction with these instruction execution systems, apparatuses, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include the following: an electrical connection portion having one or more wirings (electronic device), a portable computer diskette (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium can even be paper or other suitable media on which a program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpretation, or other suitable processing as necessary, and then stored in a computer memory.

[0146] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, any one or a combination of the following techniques well known in the art can be used: discrete logic circuits having logic gate circuits for implementing logical functions on data signals, application specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field programmable gate arrays (FPGAs), etc.

[0147] Those of ordinary skill in the art of this technology can understand that all or part of the steps carried by the method of implementing the above embodiments can be completed by instructing relevant hardware through a program, and the developed program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiments.

[0148] In addition, each functional unit in various embodiments of the present application may be integrated into a processing module, or each unit may exist physically alone, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. When the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0149] The above-mentioned storage medium may be a read-only memory, a magnetic disk, an optical disc, etc. Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present application. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present application.

Claims

1. A multi-sensor fusion positioning method under fog interference, characterized in that, include: Construct a fog point cloud generation model to generate a fog point cloud of the scene where the autonomous driving car is located by taking into account fog decay and noise through a numerical simulation method based on the clear laser point cloud of the scene where the autonomous driving car is located obtained by the LiDAR sensor and the visibility of the scene; Generate a fog point cloud of the current scene taking into account fog decay and noise using a fog point cloud generation model, and obtain the one-step prediction state vector and its mean square error matrix of the reference sensor at the current moment, and the measurement vector and its mean square error matrix of the LiDAR odometer. The reference sensor is a sensor mounted on an autonomous driving vehicle and is not affected by fog. The extinction coefficient inversion formula is used to calculate the extinction coefficient of each laser point in the fog point cloud of the current scene, and based on this, the visibility of each laser point in the fog point cloud is calculated using the visibility inversion formula. The visibility of the laser points whose detection distance is greater than the set detection distance threshold is screened out, and their average is calculated as the visibility of the identified current scene; The visibility of the identified current scene is compared with the set visibility threshold. If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer is fused with the one-step predicted state vector of the reference sensor using a variance mismatch degree graded adaptive Kalman filter based on the 3σ criterion as the fused positioning result; if the visibility of the identified current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer is fused with the one-step predicted state vector of the reference sensor using an extended Kalman filter as the fused positioning result.

2. The multi-sensor fusion positioning method according to claim 1, wherein The method of using the fog point cloud generation model to generate the fog point cloud of the current scene taking into account fog decay and noise comprises the following steps: 21) The visibility V' and laser wavelength λ of the current scene are jointly input into the fog decay simulation module in the fog point cloud generation model to obtain the extinction coefficient α of the current scene; 22) The extinction coefficient α of the current scene and the detection distance x of each laser point p in a frame of clear lidar points p are input into the echo energy simulation module in the fog point cloud generation model to obtain the echo energy of each laser point p in the clear lidar points (23) The echo energy of each laser point p in the clear laser point cloud is input into the fog noise simulation module in the fog point cloud generation model to calculate the signal-to-noise ratio SNR of the echo signal of each laser point p ; 24) According to the signal-to-noise ratio SNR of each laser point echo signal p Determine whether the detection distance and echo energy of the laser point p are valid based on the comparison between the signal-to-noise ratio SNR and the set threshold SNR0, and obtain the effective fog decay laser point cloud after removing the invalid information; 25) Add ranging noise to the effective fog decay laser point cloud to obtain a laser point cloud that takes both fog decay and noise into consideration as the fog point cloud of the current scene.

3. The multi-sensor fusion positioning method according to claim 1, wherein The one-step prediction state vector and mean square error matrix of the reference sensor are the one-step prediction state vector and mean square error matrix of a single reference sensor, or the one-step prediction state fusion vector and mean square error matrix of multiple reference sensors.

4. The multi-sensor fusion positioning method according to claim 2, wherein Step 24) includes: If the SNR p < SNR0, it is determined that the detection distance and echo energy of the laser point p are invalid. As an invalid laser point, this invalid laser point is removed from the clear laser point cloud. If the SNR p ≥ SNR0, the detection distance and echo energy of the laser point p are valid, and it is used as a valid laser point; all valid laser points are used to form a valid fog-decayed laser point cloud.

5. The multi-sensor fusion positioning method according to claim 2, wherein Step 25) includes: According to the signal-to-noise ratio SNR of the laser point j in the effective fog-decayed laser point cloud j , calculate the uncertainty of the detection distance of the effective fog-decayed laser point cloud Add the uncertainty of the detection distance of the effective fog-decayed laser point cloud to the detection distance x of the laser point j j to finally obtain the laser point cloud considering both fog decay and noise. Among them, the addition method of the ranging noise is is the detection distance of the laser point j with noise, and rd is a random number of the standard normal distribution.

6. The multi-sensor fusion positioning method according to claim 1, wherein Follow these steps to get the visibility of the identified current scene: 31) For a frame of fog point cloud with n laser points at time k, considering both fog attenuation and noise, the extinction coefficient α identified by a certain laser point j is calculated using the extinction coefficient inversion formula. j : where x j and are the detection distance and echo energy of the laser point j respectively, C L is an inherent property of the LiDAR sensor, P0 is the single-beam laser emission energy of the LiDAR sensor, A R is the effective area of the LiDAR receiver, η sr is the efficiency of the LiDAR receiver, η st is the efficiency of the LiDAR transmitter, ρ TAR is the reflectivity of the target detected by the LiDAR sensor; 32) Calculate the visibility V of laser point j based on the extinction coefficient α identified by the visibility inversion formula j Calculate the visibility V identified by laser point j j : Where λ is the laser wavelength; 33) Repeat steps 31) to 32) repeatedly until all n laser points are traversed; 34) Obtain the detection distance threshold dth that meets the accuracy requirements through experiments or simulation tests, and screen out the visibility V of the laser points with a detection distance greater than the detection distance threshold dth, and calculate its mean value V as the visibility V of the identified current scene. The calculation formula is as follows: q and use it as the visibility V of the identified current scene. The calculation formula is as follows: Wherein, N is the number of laser points in the fog point cloud whose detection distance is greater than the detection distance threshold dth considering both fog decay and noise.

7. The multi-sensor fusion positioning method according to claim 1, wherein When it is determined that the measurement vector of the LiDAR odometer is interfered by fog, the fusion positioning result is obtained by following the steps below: 421) Calculate the measurement vector Z of the LiDAR odometer using sequential filtering k for each state in the one-step prediction residual and the standard deviation of this residual 422) According to the state of the one-step prediction residual and its corresponding standard deviation the magnitude of which is used to classify the variance mismatch degree of the state by using the 3σ criterion, specifically as follows: If then the determination status is no variance mismatch; If then the determination status is mild variance mismatch; If then the determination status is severe variance mismatch; 423) According to the classification result of the state fuse the state corresponding to the one-step predicted state vector X of the state and the reference sensor k as follows: Specifically: If the state has no variance mismatch, use the extended Kalman filter to fuse the state with the state If the state has a mild variance mismatch, use the adaptive extended Kalman filter to fuse the state with the state If the state has a severe variance mismatch, isolate the state 424) If all the states in the measurement vector Z of the LiDAR odometer k are traversed, the estimated state vector and its mean square error matrix are used as the fusion positioning result.

8. The multi-sensor fusion positioning method according to claim 7, wherein, The measurement vector Z for calculating the LiDAR odometer using sequential filtering k for each state the one-step prediction residual and the standard deviation of the residual Specifically, it includes: 4211) Use the measurement vector Z of the LiDAR odometer k for a certain state and the one-step predicted state vector X of the reference sensor k to calculate the corresponding state and compute the one-step prediction residual of the state The mean square error matrix P of the one-step predicted state vector using the reference sensor k for a certain diagonal element and the mean square error matrix R of the measurement vector of the LiDAR odometer k for the corresponding diagonal element Calculate the state for the one-step prediction residual for the corresponding standard deviation 9. A multi-sensor fusion positioning device under fog interference, characterized in that, include: The first module is configured to construct a fog point cloud generation model for generating a fog point cloud of the scene where the autonomous vehicle is located considering both fog decay and noise through numerical simulation based on the clear lidar point cloud of the scene where the autonomous vehicle is located obtained by the LiDAR sensor and the visibility of the scene; The second module is configured to use the fog point cloud generation model to generate a fog point cloud of the current scene considering both fog decay and noise, and obtain the one-step prediction state vector and its mean square error matrix of the reference sensor at the current moment, and the measurement vector and its mean square error matrix of the LiDAR odometer. The reference sensor is a sensor mounted on the autonomous vehicle and not interfered by fog; The third module is configured to calculate the extinction coefficient identified by each laser point in the fog point cloud of the current scene using the extinction coefficient inversion formula, and based on this, calculate the visibility identified by each laser point in the fog point cloud using the visibility inversion formula. The visibilities identified by the laser points with a detection distance greater than the set detection distance threshold are screened out, and their mean value is calculated as the visibility of the identified current scene; The fourth module is configured to compare the visibility of the identified current scene with the set visibility threshold. If the visibility of the identified current scene is less than or equal to the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is interfered by fog, and the measurement vector of the LiDAR odometer and the one-step prediction state vector of the reference sensor are fused using variance mismatch degree hierarchical adaptive Kalman filtering based on the 3σ criterion as the fusion positioning result; if the visibility of the identified current scene is greater than the visibility threshold, it is determined that the measurement vector of the LiDAR odometer is normal, and the measurement vector of the LiDAR odometer and the one-step prediction state vector of the reference sensor are fused using extended Kalman filtering as the fusion positioning result.

Images