智能车辆利用混合采样滤波进行精确定位的方法及系统与流程
本发明属于智能驾驶技术领域，尤其涉及一种智能车辆利用混合采样滤波进行精确定位的方法及系统。
背景技术：
智能驾驶技术涉及认知工程、车辆工程、电子电气工程、控制科学与工程、人工智能等诸多学科，智能车辆是衡量一个国家科研实力和工业水平的重要标志。无人驾驶车的出现，从根本上改变了传统的“人-车-路”闭环系统中的车辆驾驶方式，将驾驶员从疲劳驾驶中解放出来，利用先进的传感器以及信息技术控制车辆行驶，让驾驶活动中常规、持久、低级、重复的操作自动完成，能够极大地提高交通系统的效率和安全，提高人类移动生活的品质，具有广泛的社会应用价值。同时，智能驾驶技术的研究将极大地增强我国在汽车主动安全等方面的核心竞争力，对提升我国汽车电子产品和汽车产业自主创新能力具有重大的战略意义。
智能车辆在车辆动力学的基础上，还包括环境感知、精确定位、信息融合、智能决策、车辆控制等部分。其中，定位技术是自动驾驶领域中的一项非常关键的基础性工作。基于全球导航卫星系统(gnss)的定位精度只能达到10m左右，无法满足自动驾驶需求。基于载波相位差分(rtk)的卫星定位在信号良好的环境下可以保持厘米级的精度，但其信号极易受到周围高大物体的遮挡或产生多径效应，从而导致定位结果的波动和定位精度的下降。将rtk-gnss与惯性测量单元(imu)或里程计结合的组合定位技术可以在一定程度上改善该问题。但由于惯导和里程计均存在累积误差，因此在卫星定位长期不稳定或失效的环境下保持高精度的定位，则需要惯导有很高的精度，而这种成本是非常昂贵的。
为了能够在比较低的成本范围内实现高精度的定位，目前主流的方法通常是使用lidar或视觉，结合事先构建的高精度地图，通过将实时数据与地图数据匹配的方法将定位精度控制在可用范围内。目前，基于视觉的成本要低于lidar，但其测距精度低，且易受光线的影响。相对而言，lidar的精度和可靠性都要优于视觉。
激光雷达可以提供周围环境中的物体表面的三维位置信息和反射强度信息，二者均可以用来定位。基于反射率的定位使用路面上的标志和车道线等平面静态信息实现精确定位，这种方法的定位精度高，受车辆/行人等动态目标的影响小，但该方法应用场景比较有限。雨雪天气会导致路面标志变得模糊不清，从而影响定位精度；在非路口路段，路面缺乏纵向特征，纵向定位精度难以保证；而在乡村、越野等非结构化道路，路面可能完全没有道路标志信息，此时这种定位方法就会完全失效。
基于高度信息的定位应用场景更加广泛，受天气影响小，且在结构化和非结构化道路均适用。但面临的一个关键问题是克服环境中的运动目标(行驶的车辆和行人)，多变的静态目标(城市改造，停靠的车辆，植物枝叶的生长)以及由此产生的遮挡对lidar定位的影响。
百度采用了将两种信息进行融合的定位，根据定位结果的置信度，自适应的调整两者的权重。但当地面标志被污染而周围环境又存在大量动态目标时，上述方法将很难得到稳定、精确的定位结构。
lidar定位的核心问题就是实时数据与地图数据的配准问题，根据地图类型的不同可以分为点云地图的配准和栅格地图的配准。
基于点云地图的配准常用于场景重建以及slam中，比较有代表性的两类方法有迭代最近点icp和高斯分布变换ndt。很多研究人员对两类配准方法的发展做出了贡献，提出了一系列优秀的改进算法，如gicp，ml-ndt，iron等，提高了算法的鲁棒性。
基于这些方法的配准通常需要定义两帧点云配准的代价函数，并过计算函数梯度来迭代的最小化配准代价。由于梯度下降的性质，这些方法高度依赖于初始位置，并且受到局部极小值的影响。在初始误差较大，环境噪声较大的情况下容易陷入局部最优，往往无法达到理想的配准效果。因此，它们通常被作为局部的精确配准算法来使用，在此之前，通常需要进行粗配准来滤除噪点，并为精确配准提供比较理想的初始位置。
基于随机采样一致性(randomsampleconsensus，ransac)的方法是目前最为常用的粗配准方法之一，很多学者从特征提取和ransac算法本身进行了改进，四点匹配(4-pointcongruentset，4pcs)及其改进算法利用满足仿射不变约束的共面四点进行ransac搜索，即使两帧点云的重合区域较小，初始位置偏差较大，依然能够得到良好的配准结果，取得了比较广泛的应用。但在无人驾驶面临的城市环境中，经常存在大量的动态目标，要使用随机抽样的方式同时找到四个静态目标点是非常困难的，算法效率会大打折扣。ml-ransac算法通过建立的sample-iteration-sample的多层架构解决高动态场景下的点云配准问题。本发明对该算法进行扩展，将其融入本发明基于栅格地图的概率定位框架中。
在基于栅格地图的配准中，粒子滤波是移动机器人领域应用最为广泛的算法之一。该算法在处理先验分布非常复杂或未知的问题时具有非常优秀的性能，当粒子数充足且分布合理的情况下，即使搜索空间较大，依然能够有很大概率在多次迭代后找到最优解。随着迭代的进行，粒子会逐渐收敛，使得算法精度逐渐提升，但同时也损失了粒子的多样性，致使粒子贫化，算法鲁棒性下降，在大规模的高动态场景中，增大了陷入局部最优的风险。
考虑到无人驾驶的应用场景，卫星定位系统通常可以将定位误差限制在几米的范围内，谷歌提出了直方图滤波(histogramfilter,hf)算法。这种方法赋予了不必担心粒子在正确位置附近丢失的显著优点，也成为了近年来lidar定位的主流方法之一。直方图滤波和粒子滤波均存在搜索空间较大时效率低下的问题，在高维情况中，这种问题更加突出。针对该问题，百度首先采用lucas-kanade(lk)算法确定车辆的航向，然后用直方图滤波来进行水平位置两个维度的定位，可以提高在大搜索空间中的定位效率，但lk算法在航向角偏差过大时容易失效。
虽然在绝大多数情况下，卫星定位结果的误差都不会特别大，但依然存在一些比较极端的情况。例如，在隧道中，隧道的遮挡会导致卫星定位失效，而隧道纵向上的特征单一会导致lidar定位失效，只能通过航位推算进行相对定位，车辆在这种环境中行驶一定的距离和时间后，会产生较大的累积误差，此时如果车辆行驶到了一个隧道中的岔路口，就需要利用lidar和地图在较大的初始偏差下进行定位。此外，无人车也会遇到类似“绑架”的问题，比如在地下车库中开机，此时的初始位置误差通常是非常大的，有时可能会有上百米，而车辆的航向也是完成未知。这种情况下的定位也是自动驾驶需要解决的。
技术实现要素：
本发明提供一种智能车辆利用混合采样滤波进行精确定位的方法及系统，利用混合采样滤波，结合事先构建的高精度地图数据和激光雷达或相机采集的实时点云数据，实现车辆在无/弱卫星信号环境中的精确定位，可用于车辆的辅助驾驶和无人驾驶。
本发明为实现上述目的，采用以下技术方案：首先使用高斯混合模型构建概率栅格地图，然后采用一个新的概率定位框架，使用核密度估计的方法将基于随机采样一致性的方法和基于贝叶斯滤波的方法高效融合在一起，充分利用各自优势弥补对方的不足，进行更加鲁棒、精确、快速的定位。
将随机采样一致性方法中的所有采样结果而不仅仅是内点比例最大结果进行核密度估计，并以此作为先验分布进行贝叶斯滤波的更新，然后将贝叶斯滤波的预测分布作为下一时刻新一轮随机采样的先验。
具体步骤如下：
a.将事先采集的点云数据根据其对应的精确的世界坐标，投影到二维栅格中，每个栅格包含了高度信息和反射率信息，两种信息均由高斯混合模型表示，高斯混合模型中的分量个数不小于1，栅格为正方形，边长不小于5cm。
b.对采集的实时点云和地图数据分别进行去噪，分割和特征提取，然后对提取的特征点进行基于ransac的配准。
c.将ransac中每一次满足约束条件的采样得到的内点比例作为该位置的权重或置信度，使用核密度估计的方法来对本次ransac中的全部结果进行聚类和拟合，得到每个核的参数。其中内点距离阈值dinlier决定了粗配准的误差范围，而核密度估计的带宽hkde和精确定位中滤波算法的采样空间大小wspace均由该误差范围决定。因此通过该框架，我们就可以很自然的将这三个部分的关键参数：ransac中内点距离阈值，高斯核密度估计中的带宽，滤波定位中的初始采样空间统一在一起。
hkde＝k1·dinlier
wspace＝k2·dinlier
d.在分布的峰值附近使用基于贝叶斯的滤波方法如直方图滤波，得到若干精确的高概率位置，峰值个数超过1时，对每个峰值均使用直方图滤波，并依据核密度函数和本时刻的先验概率计算这些位置在当前时刻的后验概率。使用后验概率最大的位置作为本次定位的最终位置，依概率计算这些位置方差得到本次定位的置信度。
e.根据马尔可夫原理和贝叶斯法则，利用惯性测量单元或里程计输出的运动状态信息分别计算这些位置对应的下一时刻定位的先验分布。
其中，xt表示车辆在t时刻的位置，
相应地，还提供了一种智能车辆利用混合采样滤波进行精确定位的系统，包括混合采样滤波单元、事先构建的高精度地图单元以及雷达或相机采集的实时点云数据单元，
利用所述混合采样滤波单元，结合所述事先构建的高精度地图单元数据和雷达或相机采集的实时点云数据单元，实现车辆在无/弱卫星信号环境中的精确定位。
其中，所述混合采样滤波单元是一种采用核密度估计方法融合基于随机采样一致性方法和基于贝叶斯滤波方法的概率定位框架，将随机采样一致性方法中的所有采样结果而不仅仅是内点比例最大结果进行核密度估计，并以此作为先验分布进行贝叶斯滤波的更新，并将贝叶斯滤波的预测分布作为下一时刻新一轮随机采样的先验。
其中，所述事先构建的高精度地图单元是将点云数据投影到二维栅格中，每个栅格包含了高度信息和反射率信息，两种信息均由高斯混合模型表示，高斯混合模型中的分量个数不小于1，栅格为正方形，边长不小于5cm。
其中，所述雷达或相机采集的实时点云数据单元是包括了从传感器直接解析得到的原始点云和经过算法处理提取的特征点云。
与现有技术相比，本发明的有益效果为，摆脱了自动驾驶定位对昂贵的高精度惯性导航设备的需求，可在大幅降低成本的情况下实现满足自动驾驶需求的高精度实时定位；
具有更加广阔的适用范围和场景，不仅可以用于城市结构化道路，还可以用于乡村半结构化道路和越野非结构化道路；不仅可以用于静态或低动态场景，还可以用于车辆行人繁多、环境变化大的高动态场景；不仅可以在良好的天气条件下工作，还可以用于雨、雪、雾天气；
能够容忍的初始位姿偏差能够达到上百米，可以解决车辆地下车库启动、长期隧道行驶等极端场景中的精确定位，为l3、l4、l5级别的自动驾驶的实现和量产提供重要技术支撑。
附图说明
图1本申请较佳实例实验路段卫星图；
图2使用本发明方法定位前后的运动轨迹示意图；
图3三种不同方法的定位结果对比示意图；
图3中：(a)横向误差，(b)纵向误差，(c)航向角误差；
图4路口的定位结果示意图；
图4中：(a)本实例在高精度地图上定位的俯视图，(b)本实例在高精度地图上定位的3d视图，(c)定位场景的前视相机视图，(d)直方图滤波的定位分布(e)多层次随机采样一致性的定位分布，(f)本实例的定位分布；
图5直线道路的定位结果示意图；
图5中：(a)本实例在高精度地图上定位的俯视图，(b)本实例在高精度地图上定位的3d视图，(c)定位场景的前视相机视图，(d)直方图滤波的定位分布(e)多层次随机采样一致性的定位分布，(f)本实例的定位分布；
图6不同初始偏差下水平位置误差分布；
图6中：(a)散点图，(b)直方图；
图7为本申请方法步骤流程图。
具体实施方式
以下结合附图和具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
本实例使用velodynehdl-64e激光雷达进行点云数据采集，算法在主频为2.40ghz的4核工控机上运行，内存为8gb，系统采用ubuntu16.04。
本实例采用含有两个分量的高斯模型来对每一个栅格的反射率和高度分布分别进行建模。对于高度信息，使用两个高斯分量分别描述地面和障碍物的高度分布；对于反射率信息，使用两个分量分别描述地面上的高反射率数据和低反射率数据的分布。栅格大小为20cm。
本实例采用基于多元几何约束的区域生长算法来对激光雷达点云和栅格地图分别进行同步去噪和分割，并提取分割后的各目标的重心点作为配准用的特征点进行基于“sample-iteration-sample”的ml-ransac。
核密度估计是一种用于估计概率密度函数的非参数方法，x1,x2,...,xn为独立同分布的n个样本点，设其概率密度函数为f，核密度估计为以下：
其中，k(.)为核函数，本实例采用的是高斯核函数。h>0为平滑带宽(bandwidth)。
本实例对车辆的(x,y,θ)三个参数进行估计，在该模型中，只对水平位置进行建模和估计，即x＝(x,y),车辆的航向角使用的是每个高斯核中权重最大的位姿对应的航向角。
贝叶斯滤波是基于贝叶斯原理的一类滤波器，包括卡尔曼滤波，粒子滤波，直方图滤波等，本实例采用的是目前主流的直方图滤波。为了提高定位速度，采用分支定界的方法对直方图滤波进行了改进。
本实例选取了真实的城市道路场景进行测试，测试路段如图1所示。路段全长7.3km，路段包含了城市内部主干道，大型的交叉路口，城市快速路，高架桥闸道等。该路段高精度地图的采集时间和实验数据的采集时间间隔了1年以上，且一个为冬季的数据，一个为夏季的数据。这意味的很多地方的环境已经发生了一定程度的变化，图1中展示了两组发生变化的场景，一组为交叉路口处道路设施的变化，另一组是冬夏两季路两旁植被的变化，且冬季会给植被设置防冻围栏，而夏季则没有。这些静态场景的变化会增大lidar定位的难度。我们将本发明提出的算法与基于直方图滤波hf的定位方法(以下简称hf)以及基于ml-ransac的定位方法(以下简称ml-ransac)进行对比。
根据初始位置偏差的类型不同，实验分为2个部分，均在含有大量动态目标的道路场景中进行。
1连续变化的位置偏差
实验一测试的是在不使用gnss的情况下，仅使用一个精度较低的imu进行车辆位姿的推算，将惯导的推算结果作为lidar定位的输入，测试算法在有大量车辆和行人存在的高动态城市环境中的定位精度和稳定性。这种由纯推算产生的累积误差是连续缓慢变化的，不存在位置的跳变。
同时，采集了基于差分gps的车辆行驶轨迹，并在此基础上使用离线的lidarslam技术作为车辆位置的真实数据。实验包括ab和cd两段。图2分别显示了车辆行驶轨迹的真实数据，imu推算的轨迹以及基于本发明方法的lidar定位后的轨迹。
三种方法在实验中的定位误差如图3所示.可以看出，本发明方法无论是平均误差还是最大误差均在三者中最小。hf精度较高，平均误差与本发明方法误差接近，但算法的鲁棒性略差，在一些动态场景中，由于动态目标的遮挡和干扰，容易出现配准错误的情况，ml-ransac横向定位的稳定性优于hf，但在一些纵向特征较少的场景，因为没有结合滤波定位算法以及本发明的概率框架，缺乏合理的置信度评估，定位结果容易出现纵向的波动，且总体的定位精度较差。
图4和图5展示了两个不同场景的定位结果。在这两组场景中，hf和ml-ransac的定位结果均出先了不同程度的偏差。其中(a)(b)分别表示lidar在高精度地图中使用本发明方法的定位结果的俯视图，3d视图，其中，高精度地图用灰度图显示，红色显示的是用于定位的实时点云数据，蓝色表示通过配准得到的静态目标，黄线为lidar定位的轨迹。(c)为车载前视相机拍摄的场景图。(d)(e)(f)为三种方法在各场景中的定位结果拟合的概率分布，可以看出，在路口场景中由于动态目标的干扰，hf的定位分布的峰值明显偏离了正确的位置，而ml-ransac则更接近真实位置；在直线道路场景中由于环境纵向特征的匮乏，hf的定位分布在纵向上存在较大的方差，而ml-ransac的定位分布则在车辆纵向方向上存在多个峰值。通过本发明对两种方法的融合，在上述两个场景中均得到了峰值准确且方差较小的定位分布。
2离散随机跳变的位置偏差
实验二在实验一的基础上进一步提升难度，在存在累积误差的推算结果上再增加一个随机偏置作为lidar定位的输入，模拟gps因不完全遮挡和多径效应产生的定位突变情况，测试在有跳变误差存在的情况下，算法的精度和稳定性。实验分为三组，航向角偏移量在±5°内随机波动，水平位置偏移量分别在2.5m，5m，10m半径范围内随机波动。
图6显示了三种定位方法在该测试路段上的不同初始偏差下的定位结果水平距离误差分布散点图和误差分布直方图。为了更清晰的呈现定位误差的分布情况，图6(a)的纵坐标使用了对数坐标，可以看出，本发明方法的定位精度对初始偏差并不敏感，定位精度始终保持在较高的水平，而hf和ml-ransac的定位结果的总体误差和波动均比较较大。在该实验中，我们只统计了lidar定位的结果，剔除了两次lidar定位之间imu的推算结果。由于三种方法在不同的初始误差下完成一次定位的耗时不同，因此在同一段测试数据上，总时间相同，不同初始误差范围的定位次数是不同的，随着初始偏差的增大，三种方法的定位次数成效明显的递减趋势。
从图6(b)中可以看出，本发明算法在水平误差小于0.1m的次数远大于其他方法，且定位结果的误差集中分布在0.5m以内。
表1给出了三种方法在不同初始偏差下定位的统计结果，包括横向，纵向，航向角以及水平位置的方均根误差，水平误差小于0.1m和0.4m的百分比，算法耗时的均值和标准差。当初始偏差为10m时，hf在0.4m误差内的定位次数只占了其总定位次数的9.09％，而本发明方法仍然有93.18％。
表1不同初始位置偏差下定位统计结果对比
实验结果表明，无论是在连续累积的初始误差还是在离散随机跳动的初始误差情况下，算法均保持了较高的定位精度和较快的定位速度。相比于改进之前的直方图滤波算法和多层次随机采样一致性算法，在保证定位精度和速度的前提下，进一步提升了定位的鲁棒性，能够适应更复杂的定位场景；并且在初始位置存在较大偏差的情况下依然能够保持快速精确的定位，这是改进前的算法无法做到的。
以上所述仅是本发明的优选实施方式，应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
- 还没有人留言评论。精彩留言会获得点赞！