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

权利要求书

1.一种雾干扰下的多传感器融合定位方法，其特征在于，包括：

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

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

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

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

2.根据权利要求1所述的多传感器融合定位方法，其特征在于，所述利用雾点云生成模型生成同时考虑雾衰退与噪声的当前场景的雾点云，包括以下步骤：

21)将当前场景的可见度V'和激光波长λ联合输入雾点云生成模型中的雾衰退模拟模块，得到当前场景的消光系数α；

22)将当前场景的消光系数α与一帧清晰激光点云中每一个激光点p的探测距离x_p输入雾点云生成模型中的回波能量模拟模块，获取清晰激光点云中每一个激光点p的回波能量

23)将清晰激光点云中每一个激光点p的回波能量输入雾点云生成模型中的雾噪声模拟模块，计算每一个激光点回波信号的信噪比SNR_p；

24)根据每一个激光点回波信号的信噪比SNR_p与设定的阈值SNR₀的大小判断激光点p的探测距离与回波能量是否有效，将无效信息剔除后得到有效雾衰退激光点云；

25)对有效雾衰退激光点云添加测距噪声，得到同时考虑雾衰退与噪声的激光点云，作为当前场景的雾点云。

3.根据权利要求1所述的多传感器融合定位方法，其特征在于，所述基准传感器的一步预测状态向量及其均方误差阵为单一基准传感器的一步预测状态向量及其均方误差阵，或者为多种基准传感器的一步预测状态融合向量及其均方误差阵。

4.根据权利要求2所述的多传感器融合定位方法，其特征在于，步骤24)包括：

若SNR_pSNR₀，则判断激光点p的探测距离与回波能量无效，作为无效激光点，从清晰激光点云中去除该无效激光点；若SNR_p≥SNR₀，则激光点p的探测距离与回波能量有效，作为有效激光点；利用所有有效激光点构成有效雾衰退激光点云。

5.根据权利要求2所述的多传感器融合定位方法，其特征在于，步骤25)包括：

根据有效雾衰退激光点云中激光点j的信噪比SNR_j，计算有效雾衰退激光点云探测距离的不确定度将有效雾衰退激光点云探测距离的不确定度添加至激光点j的探测距离x_j中，最终得到同时考虑雾衰退与噪声的激光点云，其中，测距噪声的添加方式为，为带有噪声的激光点j的探测距离，rd为标准正态分布随机数。