[发明专利]用于自动驾驶场景的多传感器融合定位方法

说明书

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

技术领域

本发明是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：根据公式：

分别计算粒子群中每个粒子在车道中位置差值和航向差值；其中，Δdⁱ表示第i个粒子的位置差值，d_c表示camer输出当前车辆距离车道中心线的距离偏差，表示第i个粒子的距离车道中心线的距离偏差，Δyawⁱ表示第i个粒子的航向角差值，yaw_c表示camer输出的当前车辆与车道的航向角的偏差，表示第i个粒子所在车道的道路的航向角，表示第i个粒子的航向角。