用于自动驾驶场景的多传感器融合定位方法技术

本发明专利技术公开了一种用于自动驾驶场景的多传感器融合定位方法，采用低成本传感器结合矢量地图通过改进的粒子滤波算法实现车道级定位。本发明专利技术不仅具有明显价格优势，有利于自动驾驶技术普及，同时能保证定位精度，而且使用更加方便，还能输出频率可调的高频定位信息，其能为环境感知和车身控制提供基准数据。

Multi sensor fusion localization method for automatic driving scene

【技术实现步骤摘要】
用于自动驾驶场景的多传感器融合定位方法
本专利技术是2019111650580的分案申请，属于自动驾驶领域，特别涉及用于自动驾驶场景的多传感器融合定位方法。
技术介绍
高精度定位是自动驾驶领域的重要研究课题，目前常用高精度组合导航、多线激光雷达、摄像等传感器结合高精度地图实现车道级定位，主要采用卡尔曼滤波、粒子滤波、SLAM(即时定位与地图构建)等融合定位算法。其中常规的卡尔曼滤波算法需要价格昂贵的高精度RTK(实时动态测量技术)和IMU，在高架桥、隧道等GPS信号不稳定地方，其定位精度较低；现有的粒子滤波算法需要借助深度学习模块检测道路标志，将其与先验地图中的道路特征匹配，更新模型参数；SLAM算法在车辆快速运行的高速公路上鲁棒性较低，此外其需要高性能的计算平台。所以，在现有技术中没有一种适用于车辆自动驾驶的既经济又能够精准定位的方法。
技术实现思路
专利技术目的：本专利技术针对现有技术存在的问题，提出了一种不仅经济又能够对车辆进行准确定位的用于自动驾驶场景的多传感器融合定位方法。技术方案：为实现上述目的，本专利技术提供了一种用于自动驾驶场景的多传感器融合定位方法，包括以下步骤：步骤1：车载传感器实施采集车辆的行驶信息；其中，车辆的行驶信息包括，车辆所在的经纬度、车辆的速度信息、航向信息、车辆所在的车道和车辆与所在车道的中心线之间的距离；步骤2：在矢量地图上以步骤1采集到的车辆所在的经纬度为圆心，GPS定位偏差为半径做圆；并在圆内根据高斯分布设置粒子群；其中，矢量地图中含有车道线、车道宽度、车道航向角的信息；步骤3：将传感器采集的航向信息、速度信息和步骤2中设置的粒子群中每个粒子的的位置信息加入高斯噪声后输入到第一恒定转率和速度运行模型中，第一恒定转率和速度模型输出每个粒子的状态信息，其中每个粒子的状态值包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息；步骤4：将不在车辆所在车道内的粒子的权重值设置为0；再分别计算剩下粒子点的权重值；步骤5：将步骤3获得每个粒子的状态信息和步骤4获得每个粒子的权重值通过加权平均法求出车辆的位置信息。进一步，所述步骤1中采用的传感器有GPS、IMU、Camera和里程计。这里采用的传感器更加的经济实惠。进一步，所述步骤2中GPS定位偏差通过公式计算获得，其中，λ为GPS定位偏差，η表示GPS定位精度，θ为收到星数，h为水平精度因子，β的取值范围在0.55～0.65，σ为稳定系数，μ为水平精度系数。有效的保证了整个方法的鲁棒性。进一步，所述步骤4中粒子点的权重值获取方法，包括以下步骤：步骤401：根据公式：分别计算粒子群中每个粒子在车道中位置差值和航向差值；其中，Δdi表示第i个粒子的位置差值，dc表示camer输出当前车辆距离车道中心线的距离偏差，表示第i个粒子的距离车道中心线的距离偏差，Δyawi表示第i个粒子的航向角差值，yawc表示camer输出的当前车辆与车道的航向角的偏差，表示第i个粒子所在车道的道路的航向角，表示第i个粒子的航向角。步骤402：将步骤401获得的每个粒子在车道中的位置差值和航向差值代入概率密度函数，经过归一化后得到每个粒子点的权重值wi；其中，wi为第i个粒子的权重，σd表示camer检测车辆距离车道中心线的距离偏差的方差，ud表示camer检测车辆距离车道中心线的距离偏差的均值，σyaw表示camer检测车辆与车道的航向角的偏差的方差，uyaw表示camer检测车辆与车道的航向角的偏差的均值。进一步，还包括高频模块，将步骤5中获得的车辆的位置信息、实时的车辆速度信息和车辆的航向信息输入到高频模块中，高频模块输出车辆位置信息；其中高频模块通过恒定转率和速度模型对车辆的位置信息进行计算。能够输出频率可调的高频定位信息。进一步，所述高频模块工作包括以下步骤：步骤601：将步骤5中获得的车辆的位置信息和当前采集的车辆速度信息以及车辆的航向信息输入到第二恒定转率和速度模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，其中，第二恒定转率和速度模型为：xt＝xt-1+v′t×cos(yawt)×Δtyt＝yt-1+v′t×sin(yawt)×Δt式中，yawt表示t时刻车辆的航向角，yawt-1表示t-1时刻车辆的航向角，表示在t-1时刻IMU输出的车辆的航向角的角速度，xt表示t时刻车辆在UTM坐标系下的横坐标，xt-1表示t-1时刻车辆在UTM坐标系下的横坐标，v′t在t时刻车辆ODOM(里程计)输出的车辆的速度，yt表示t时刻车辆在UTM坐标系下的纵坐标，yt-1表示t-1时刻车辆在UTM坐标系下的纵坐标；步骤602：检测是否采集到新的车辆速度信息和车辆的航向信息；如果采集到新的车辆速度信息和车辆的航向信息，则执行步骤603，如果没有采集到新的车辆速度信息和车辆的航向信息则执行步骤602；步骤603：检测步骤5是否有输出新的车辆的位置信息，如果没有，则用步骤601获得车辆的位置信息{xt，yt，yawt}结合此时采集到的新的车辆速度信息以及车辆的航向信息作为输入数据输入到第二恒定转率和速度模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，然后重复步骤602；如果步骤5有输出新的车辆的位置信息，则重复步骤601～602。工作原理：本文提出一种针对自动驾驶场景的定位方案，采用低成本GPS、IMU、Camera等传感器结合矢量地图通过改进的粒子滤波算法实现车道级定位。该方案使用的硬件设备和矢量地图数据均是自动驾驶技术的常用资源，其无需高昂传感设备和计算平台即可实现精准的车道级定位。有益效果：与现有技术相比，本专利技术具有以下优点：1.本专利技术采用的GPS和IMU均为低成本传感设备，具有明显价格优势，有利于自动驾驶技术普及。2.本专利技术将自动驾驶车辆必备传感器GPS的绝对位置信息和Camera的道路感知信息进行融合定位，有效的保证了定位精度。3.本专利技术通过粒子群构建GPS绝对偏差和车道横向定位的约束关系，实现车道级定位，其无需深度学习等目标检测平台可实现同级定位精度，使用更加方便。同时，把高斯噪声加入到整个粒子群中，用于模拟真实情况，让算法的输入数据包含系统的不确定性，经过观测模块更新粒子权重后，粒子滤波算法的输出会更加精确。4.本专利技术将文中粒子滤波输出的定位信息和车辆预测模型结合，能输出频率可调的高频定位信息，其能为环境感知和车身控制提供基准数据。附图说明图1为本专利技术的系统框图；图2为矢量地图上粒子群设置范围示意图；图3为矢量地图上粒子群设置示意图；图4为根据视觉车道检测信息筛选粒子群后的示意图；图5为定位结果示意图。具体实施方式下面将结合本专利技术实例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实本文档来自技高网...

【技术保护点】
1.一种用于自动驾驶场景的多传感器融合定位方法，其特征在于：包括以下步骤：/n步骤1：车载传感器实施采集车辆的行驶信息；其中，车辆的行驶信息包括，车辆所在的经纬度、车辆的速度信息、航向信息、车辆所在的车道和车辆与所在车道的中心线之间的距离；/n步骤2：在矢量地图上以步骤1采集到的车辆所在的经纬度为圆心，GPS定位偏差为半径做圆；并在圆内根据高斯分布设置粒子群；其中，矢量地图中含有车道线、车道宽度、车道航向角的信息；/n步骤3：将传感器采集的航向信息、速度信息和步骤2中设置的粒子群中每个粒子的的位置信息加入高斯噪声后输入到第一恒定转率和速度运行模型中，第一恒定转率和速度模型输出每个粒子的状态信息，其中每个粒子的状态值包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息；/n步骤4：将不在车辆所在车道内的粒子的权重值设置为0；再分别计算剩下粒子点的权重值；/n步骤5：由步骤4获得的粒子权重值作为步骤3中相应粒子状态信息的权重值，对步骤3中的粒子信息进行加权平均；/n所述步骤2中GPS定位偏差通过公式

【技术特征摘要】
1.一种用于自动驾驶场景的多传感器融合定位方法，其特征在于：包括以下步骤：
步骤1：车载传感器实施采集车辆的行驶信息；其中，车辆的行驶信息包括，车辆所在的经纬度、车辆的速度信息、航向信息、车辆所在的车道和车辆与所在车道的中心线之间的距离；
步骤2：在矢量地图上以步骤1采集到的车辆所在的经纬度为圆心，GPS定位偏差为半径做圆；并在圆内根据高斯分布设置粒子群；其中，矢量地图中含有车道线、车道宽度、车道航向角的信息；
步骤3：将传感器采集的航向信息、速度信息和步骤2中设置的粒子群中每个粒子的的位置信息加入高斯噪声后输入到第一恒定转率和速度运行模型中，第一恒定转率和速度模型输出每个粒子的状态信息，其中每个粒子的状态值包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息；
步骤4：将不在车辆所在车道内的粒子的权重值设置为0；再分别计算剩下粒子点的权重值；
步骤5：由步骤4获得的粒子权重值作为步骤3中相应粒子状态信息的权重值，对步骤3中的粒子信息进行加权平均；
所述步骤2中GPS定位偏差通过公式计算获得，其中，λ为GPS定位偏差，η表示GPS定位精度，θ为收到星数，h为水平精度因子，β的取值范围在0.55～0.65，σ为稳定系数，μ为水平精度系数；
所述步骤4中粒子点的权重值获取方法，包括以下步骤：
步骤401：根据公式：






分别计算粒子群中每个粒子在车道中位置差值和航向差值；其中，Δdi表示第i个粒子的位置差值，dc表示camer输出当前车辆距离车道中心线的距离偏差，表示第i个粒子的距离车道中心线的距离偏差，Δyawi表示第i个粒子的航向角差值，yawc表示camer输出的当前车辆与车道的航向角的偏差，表示第i个粒子所在车道的道路的航向角，表示第i个粒子的航向角；
步骤402：将步骤401获得的每个粒子在车道中的位置差值和航向差值代入概率密度函数，经过归一化后得到每个粒子点的权重值wi；



其中，wi为第i个粒子的权重，σd表示camer检测车辆距离车道中心线的距离偏差的方...

【专利技术属性】
技术研发人员：李赵，刘心刚，袁瑞廷，张旸，陈诚，刘洁，
申请(专利权)人：奥特酷智能科技南京有限公司，
类型：发明
国别省市：江苏;32