[发明专利]雾干扰下的多传感器融合定位方法和装置

说明书

本公开提供的雾干扰下的多传感器融合定位方法和装置，将场景清晰激光点云与场景可见度联合输入至雾点云生成模型，得到当前场景的雾点云；利用该雾点云对场景当前的可见度进行识别，判断LiDAR里程计是否被雾干扰；针对LiDAR里程计被雾干扰情况，首先利用序贯滤波计算LiDAR里程计量测向量中每一个状态的预测一步残差与该残差的标准差；然后根据预测一步残差相对于标准差的失配情况，运用3σ准则，将这些状态分为无失配、轻度失配与重度失配；最后使用EKF融合无失配状态，使用AEKF融合轻度失配状态，隔离重度失配状态。本公开解决了雾干扰条件下基于LiDAR的多传感器融合定位发散的预期功能安全问题。

技术领域

本公开涉及自动驾驶车辆的多传感器融合定位技术领域，具体涉及一种雾干扰下的多传感器融合定位方法和装置。

背景技术

多传感器融合定位技术作为自动驾驶核心技术之一，可有效解决单一传感器失效或功能不足场景下的定位问题，是车辆自主导航的基础。其中，基于LIDAR的多传感器定位技术，具有不易受光照条件变化影响、能精确3D感知环境特征、定位精度高的优点，因此被广泛采用。然而，在雾天条件下LIDAR发出的激光束会被雾滴吸收、散射或折射，使得LIDAR里程计的量测信息被干扰，进而使得融合定位发散。现有的故障诊断与隔离方法在识别到LIDAR里程计的量测信息存在异常之后，即认为LIDAR出现了故障，进而直接将其隔离，这势必会导致LIDAR里程计的量测信息中未被干扰的部分被浪费。因此，雾干扰条件下，提出一种能精确识别LIDAR里程计的量测信息是否受到干扰，并抑制基于LIDAR的融合定位系统发散的功能改进策略，是有待解决的关键问题。

发明内容

为了保证雾干扰条件下基于LIDAR的多传感器融合定位的预期功能安全(SOTIF)，本公开着力于提出一种雾干扰条件下基于LiDAR的多传感器融合定位方法和装置，其目的在于识别LIDAR里程计的量测信息是否被雾干扰，并在保证融合定位不发散的前提下充分利用LIDAR里程计量测信息中未被干扰的部分。

为了达到上述目的，本公开第一方面实施例提供的雾干扰下的多传感器融合定位方法，包括：

构建雾点云生成模型，用于根据LiDAR传感器获取的自动驾驶汽车所在场景的清晰激光点云及该场景的可见度通过数值模拟方法生成同时考虑雾衰退与噪声的自动驾驶汽车所在场景的雾点云；

利用雾点云生成模型生成同时考虑雾衰退与噪声的当前场景的雾点云，获取当前时刻基准传感器的一步预测状态向量及其均方误差阵、LiDAR里程计的量测向量及其均方误差阵，所述基准传感器为搭载于自动驾驶汽车上且不受雾干扰的传感器；

利用消光系数反演公式计算当前场景的雾点云中各激光点识别出的消光系数，并以此为依据计算雾点云中各激光点识别出的可见度，将探测距离大于设定的探测距离阈值的激光点识别出的可见度筛选出，并求其均值，作为识别出的当前场景的可见度；

将识别出的当前场景的可见度与设定的可见度阈值进行比较，若识别出的当前场景的可见度小于或者等于可见度阈值，则判定LiDAR里程计的量测向量被雾干扰，使用基于3σ准则的方差失配程度分级自适应卡尔曼滤波将LiDAR里程计的量测向量与基准传感器的一步预测状态向量融合，作为融合定位结果；若当前场景的可见度大于可见度阈值，则判定LiDAR里程计的量测向量正常，使用扩展卡尔曼滤波将LiDAR里程计的量测向量与基准传感器的一步预测状态向量融合，作为融合定位结果。：

本公开第一方面提供的雾干扰下的多传感器融合定位方法，具有以下特点及有益效果为：