一种基于三维点云与卫星图匹配的车辆定位方法与流程

本发明涉及自动控制技术领域，尤其涉及一种基于三维点云与卫星图匹配的自主车辆定位方法。

背景技术：

无人车作为一种移动机器人，是一种涉及了环境感知、定位与导航、任务规划、机器学习等方面的高度智能集合体。近年来，随着深度学习等技术的兴起，无人驾驶技术也取得了很大的进步，并引起人们的广泛关注。

定位是无人驾驶的关键技术之一。采用卫星定位系统可以得到车辆的位置，但由于城市峡谷效应，当卫星信号受到建筑物的遮挡或反射时，卫星定位系统会精度降低甚至定位失败。为了增加整个定位系统的精度和可靠性，可以利用实现构建好的地图进行定位作为卫星定位的补充。目前主流方法大多采用数辆地图采集车遍历待构图区域实现地图的构建，这种方法需要的时间及人力物力成本很高。

技术实现要素：

有鉴于此，本发明提供了一种基于三维点云与卫星图匹配的自主车辆定位方法，可以提供一种新的定位方法。

一种三维点云与卫星图匹配的自主车辆定位方法，包括如下步骤：

步骤1、预先训练神经网络，具体为：获取整个地图上同一区域对应的卫星图像块以及三维点云生成的栅格图，作为一对正样本，该对正样本的期望值为两者的匹配概率，取值为1；同时，获取整个地图上两个不同区域分别对应的卫星图像块和三维点云生成的栅格图，构成一对负样本，该对负样本的期望值为两者的匹配概率，取值为0；获得多对正样本和多对负样本，对神经网络进行训练，网络输出为卫星图像块与栅格图的匹配概率；

步骤2、对粒子滤波器所有粒子进行初始化，得到粒子滤波器的每一个粒子的位置和航向以及权重初值；

步骤3、基于步骤2获得的粒子的位置、航向及权重初值，根据车辆搭载的航位推算模块的数据更新粒子的位置、航向及权重值；

根据更新后的每个粒子的位置xⁱ,yⁱ，以每个粒子的位置为几何中心，在地图上截取一个与步骤1中所述卫星图像块大小一致的截图图像块该截取图像块的方向与地球坐标北向的夹角为该粒子的航向角ψⁱ；

步骤4、利用车载传感器得到的三维点云，构建俯视视角的栅格图；

步骤5、取步骤3截取的每一个截取图像块与步骤4获得的栅格图组成一对样本，输入到步骤1训练好的神经网络，得到两者的匹配概率；则每一个粒子对应一个匹配概率；

针对每一个粒子，利用其对应的匹配概率乘以上一时刻粒子的归一化权重得到当前时刻该粒子的权重；

步骤6、根据粒子在步骤3更新后的位置及航向，以及步骤5更新后的权重，对位置和航向分别利用加权求和再求均值的方式，得到当前时刻车辆位置及航向的估计值，完成车辆定位。

较佳的，所述步骤2中，对粒子滤波器初始化方式采用在整个地图范围内随机播洒粒子，获得各粒子在地图上的位置、航向及权重的初值；或利用卫星定位等方法确定的车辆初始位置附近随机播洒粒子，获得各粒子在地图上的位置、航向及权重初值。

较佳的，所述步骤3中，航位推算模块为轮式里程计、视觉里程计、激光里程计或惯性导航系统。

较佳的，当航位推算模块为轮式里程计为例，粒子的更新公式为：

其中，δs为里程计测量得到的上一时刻到当前时刻车辆行驶的距离增量，δψ为航向角增量。

较佳的，粒子更新后，根据里程计的噪声模型，对每个粒子增加随机噪声。

较佳的，在执行步骤5之后且在步骤6之前，计算有效粒子个数：

n表示粒子个数；若neff小于设定阈值t且wmax大于设定阈值p，则对粒子进行重采样，并基于重采样后的粒子执行步骤6；如果不满足neff小于设定阈值t且wmax大于设定阈值p的条件，则直接执行步骤6；wmax表示所有粒子权重的最大值。

较佳的，设定阈值t＝0.6n；设定阈值p＝0.8。

较佳的，粒子重采样方法采用stratifiedresampling方法。

本发明具有如下有益效果：

本发明提供了一种基于三维点云与卫星图匹配的自主车辆定位方法，可以基于三维点云与卫星图匹配的自主车辆定位，该方法利用车载传感器得到的环境三维点云信息，结合车辆航位推算方法的输出结果，利用粒子滤波器实现车辆在卫星图或航拍图上的定位，进而得到车辆的全局坐标及航向角，可给gps提供辅助定位信息，是一种新的定位方法；本方法对粒子滤波器的粒子权重通过神经网络比较点云生成的栅格图与卫星图像块的相似度，兼容多种传感器及多种航位推算方法，且环境适应性强。

附图说明

图1为本发明的基于三维点云与卫星图匹配的自主车辆定位方法流程框图。

具体实施方式

基于现有技术的缺陷，由于卫星地图更容易获得且覆盖范围很广，因此本发明考虑如果可以利用车载传感器数据(例如激光雷达、相机等)与卫星地图进行匹配，则可以方便地得到车辆的位置，且无需事先构建地图。本发明提供了一种基于三维点云与卫星图或航拍图匹配的自主车辆定位方法，具体的过程如图1所示，包括：

步骤1、预先训练神经网络，具体为：获取整个地图上同一区域对应的卫星图像块以及三维点云生成的栅格图，作为一对正样本，该对正样本的期望值为两者的匹配概率，取值为1；获取多个区域的卫星图像块以及栅格图，得到多对正样本；同时，获取整个地图上两个不同区域分别对应的卫星图像块和三维点云生成的栅格图，构成一对负样本，该对负样本的期望值为两者的匹配概率，取值为0；按照上述方法获得多对正样本和多对负样本，对神经网络进行训练，网络输出为卫星图像块与栅格图的匹配概率。

步骤2、对粒子滤波器所有粒子进行初始化，具体为：粒子滤波器的每一个粒子保存了一组位置及航向以及权重初始化方式可以采用在整个地图范围内随机播洒粒子，获得各粒子在地图上的位置、航向及权重的初值；或利用卫星定位等方法确定的车辆初始位置附近随机播洒粒子，获得各粒子在地图上的位置、航向及权重初值；

步骤3、基于步骤2获得的粒子的位置、航向及权重初值，根据车辆搭载的航位推算模块的数据更新粒子的位置、航向及权重值；航位推算模块可以是轮式里程计、视觉里程计、激光里程计或惯性导航系统等多种航位推算模块。以轮式里程计为例，粒子的更新公式为：

其中，δs为里程计测量得到的上一时刻到当前时刻车辆行驶的距离增量，δψ为航向角增量。之然后根据里程计的噪声模型，对每个粒子增加随机噪声(例如高斯噪声)。

根据更新后的每个粒子的位置xⁱ,yⁱ，以每个粒子的位置为几何中心，在地图上截取一个与步骤1中所述卫星图像块大小一致的截图图像块该截取图像块的方向与地球坐标北向的夹角为该粒子的航向角ψⁱ；

步骤4、利用车载传感器得到的三维点云，构建俯视视角的栅格图gl＝{gh,gi}。gh表示每一个栅格储存的落入该栅格的所有点的最大高度值，gi表示该最高点对应的反射强度信息(如果传感器是激光雷达)或颜色信息(如果传感器是相机)。

步骤5、取步骤3截取的每一个截取图像块与步骤4获得的栅格图组成一对样本，输入到步骤1训练好的神经网络，得到两者的匹配概率则每一个粒子对应一个匹配概率；

针对每一个粒子，利用其对应的匹配概率更新该粒子的权重，更新公式为：

其中为上一时刻粒子的归一化权重；记录所有粒子权重的最大值wmax，然后对粒子权重进行归一化得到

步骤6、为防止粒子滤波器出现退化现象(经过若干次迭代后，除了少数粒子权重很大外，其余大量粒子的权重几乎为0)，需要根据粒子滤波器的粒子权重情况确定是否进行重采样。首先计算有效粒子个数：

n表示粒子个数；若neff小于设定阈值t且wmax大于设定阈值p，则说明当前时刻有效粒子(权重很大的粒子)的数量很少，且当前时刻存在权值足够高的粒子，需要对粒子进行重采样；如果不需要重采样，则直接执行步骤7；设定阈值t和设定阈值p根据经验设定，一般取t＝0.6n，p＝0.8。重采样方法可以采用stratifiedresampling方法，该方法根据粒子的权重在所有粒子权重总和所占比例决定其被重新取样的概率。

步骤7、确定重采样后粒子在步骤3更新后的位置及航向，以及步骤5更新后的权重，利用加权求和再求均值的方式(每个粒子的位置乘各自的权重后求和再求均值得到位置估计值；每个粒子的航向乘各自的权重后求和再求均值得到航向估计值)，得到当前时刻车辆位置及航向的估计值，完成车辆定位。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。