一种基于多元统计信息的大范围场景全局定位方法与流程

1.本发明涉及自动驾驶技术领域，特别是涉及一种基于多元统计信息的大范围场景全局定位方法。


背景技术：

2.在自动驾驶领域中，定位是其重要的基础设施，其一般采用高精度惯导对车辆位置进行全局的锚定，配合激光雷达或者摄像头，利用基于高精度地图的slam(simultaneous localization and mapping)技术，实现多传感器融合的全局定位。一方面它为无人车提供“在哪儿”信息；另一方面为感知环境提供辅助，进而帮助车辆进行决策规划。特别的，在硬件设备条件匮乏的情况下，高精度惯导设备无法配备，基于高精度地图的全局定位发挥着至关重要的作用。
3.现有的全局定位一般包括两种方案：一是基于全球定位系统或者北斗系统的全局定位；二是基于高精度地图的多传感器融合全局定位。基于高精度地图的多传感器融合全局定位需要在初始误差较大的情况下实现大范围场景的全局初始化定位。其通常使用车载传感器，例如激光雷达，对周围环境进行感知，然后与高精度地图进行配准，实现全局的高精度定位。常采用基于梯度下降的方法，如icp、gicp、ndt等；或者采用基于全局搜索方法，如csm等。
4.基于梯度下降方法的icp、gicp、ndt等方案需要提供一个与真值接近的初值才能配准成功，否则最终结果会陷入局部最小值；基于全局搜索的csm等方法通常不会陷入局部最小值问题，但是对环境的特征描述不够丰富，往往利用在2维场景或者2.5维场景，容易受到环境中噪声的干扰，导致鲁棒性不高，配准容易失败。


技术实现要素：

5.本发明所要解决的技术问题是：为了克服现有技术中的不足，本发明提供一种基于多元统计信息的大范围场景全局定位方法，解决大范围三维场景下的全局配准问题。
6.本发明解决其技术问题所要采用的技术方案是：一种基于多元统计信息的大范围场景全局定位方法，利用多线激光雷达，实现了在具有较大初始误差下的全局定位。采用多元统计信息对三维场景特征进行描述，将场景划分为不同分辨率大小的栅格，采用全局搜索的方法实现全局的定位。
7.具体包括以下步骤：
8.step1：对场景的三维点云地图在x、y、z方向上分别进行栅格划分，获得三维栅格地图；
9.step2：统计三维栅格地图中落入每个三维栅格中的点云，获得每个三维栅格的点云集合vi，则vi＝{p1,p2,
…
,p
ni
}，并统计所有点云集合中的点云最大频数max
p
，则max
p
＝max{n1,n2,
…
,nk}；同时，对每个三维栅格在z方向上进行高斯拟合，计算出每个三维栅格的均值和方差；其中，vi表示第i个三维栅格的点云集合，ni表示集合vi中的点的数量，不同的集
合，点的数量不尽相同，i表示三维栅格的序号，i＝1,2,
…
k，k表示三维点云地图划分的栅格数量；
10.其中，频数、均值和方差均表示三维栅格地图的特征，其中，频数是分布特征，表示的是多线激光雷达扫描的点落到三维栅格地图中的概率；均值和方差是统计特征，表示的是多线激光雷达扫描的点落到三维栅格地图中，该点与三维栅格中点的相似度。
11.step3：将每个三维栅格在z方向上以一定的分辨率z
res
细划分，且z
res
《rz，按照下面公式计算点击中细分栅格后的得分：
[0012][0013]
其中，下标i表示三维栅格的序号，i＝1,2,
…
k；j表示在第i个栅格中按照z方向细划分后的栅格序号，α表示常数系数；z表示高度，即通过zres划分后的每个栅格的中心高度；rz表示三维点云地图进行栅格划分时z方向上的分辨率；z
res
表示每个三维栅格细化分时z方向上的分辨率；μi表示序号为i的三维栅格的均值；σi表示序号为i的三维栅格的方差。
[0014]
其中，细化分的意思就是将rz以z
res
的大小再进行细分，然后通过rz统计出高斯模型和概率模型计算出对应栅格的每个z
res
的得分。
[0015]
即为待匹配点云中单个点击中目标栅格后的得分，对划分的栅格进行一元高斯拟合及点云密度信息统计，实现的多元信息融合特征表达。
[0016]
其中，step1是进行的第一次栅格划分，step3在第一次栅格划分基础上，再对z方向上进行细化分，即进行第二次栅格划分，经过这个步骤后，可以把三维空间高精度点云地图划分为一系列三维栅格，并计算出了待匹配点云击中目标栅格后的得分。
[0017]
step4：将待配准的点云按照csm的全局搜索方法进行搜索，选择总得分最高的变换作为最优结果，即最终定位结果。本步骤目的是做点云到地图的匹配。在设置的解空间内搜索出点云到地图的最佳变换，该最佳变换包括平移和旋转。也就是把激光雷达平移和旋转后使得当前的点云和地图能够准确的重合，寻找平移和旋转的过程就是搜索。从开始到找到最重合的地方所经历的平移和旋转就是最佳变换，此处应为三个自由度(x，y，yaw)。
[0018]
本发明采用一种多尺度的分布特征及统计特征来刻画场景的栅格地图，采用多分辨多层次的栅格地图，一方面可以对点云地图进行数据压缩，其次与单一层次的栅格地图相比，对环境特征表达更加合理，算法的鲁棒性更高；与语义特征相比，采用语义特征需要对场景进行语义分割，该过程比较消耗算力，其次分割后的语义信息还是需要进行数据降维，因此，计算速度相对本方案来说较慢。
[0019]
csm的全局搜索方法是已经有的技术方案，就是采用多分辨形式的分支界定算法来实现的，不是本发明要求保护的创新点所在，因此，此处不作详细赘述。
[0020]
进一步，step1中栅格划分的具体过程为：
[0021]
step1.1：分别确定x、y、z方向上的分辨率r
x
、ry、rz；其中，分辨率r
x
、ry、rz构成一个三维栅格，各个方向上划分的分辨率不一定相同；
[0022]
step1.2：根据分辨率和三维点云地图的大小，确定划分的栅格数量k；
[0023]
step1.3：根据分辨率和栅格数量k，对场景中的三维点云地图进行栅格划分，获得三维栅格地图。
[0024]
进一步，在step1中，采用多线激光雷达获得场景的三维点云，并根据三维点云构建场景的三维点云地图。作为优选，三维点云地图是提前构建好的，可以将构建好的场景的三维点云地图直接应用在本发明的定位方法中。
[0025]
进一步，在step3中提前计算点击中每个细分栅格的得分，并组合成一个得分表，采用这种方式，把三维点云地图预划分为三维得分查找表，在后续的计算中可以直接采用查表的方式，加速计算。本发明的栅格存储的是对栅格里面特征的一种概率分布描述(单高斯拟合，其实也可以多核高斯拟合)和一种统计特征。对于计算得分：精细化栅格后，利用每一个栅格中心的高度值来代替可能落到这个栅格的所有点，通过栅格中心的高度值提前计算好得分，在接下来的搜索的过程中无需通过复杂算法来计算激光雷达每个点落到对应栅格的真实得分。提前对每个精细化栅格计算一个得分，能够加速计算，提高定位的速度。
[0026]
技术术语：
[0027]
csm是correlative scan matching的缩写，相关性扫描匹配。
[0028]
slam是simultaneous localization and mapping的缩写。
[0029]
icp是iterative closest point的缩写，迭代最近点。
[0030]
gicp是generalized icp的缩写，泛化迭代最近点。
[0031]
ndt是normal distributions transform的缩写，正态分布变换。
[0032]
本发明的有益效果是：
[0033]
(1)三维点云是由一系列点组成的，点是三维的，占用的存储很大，划分为栅格地图后，只需要存储栅格的得表分，首先栅格数量比点少几个数量级，其次存储的是每个栅格的一系列分数，只是一维的，因此，本定位方法对三维点云地图进行压缩，可以缩小数据的存储。
[0034]
(2)相对于二维栅格划分来说，本发明将三维点云数据在x、y、z三个方向上进行栅格划分，保留了更多的三维特征信息，使三维特征更加完整，有效提高了定位准确度和成功率。
[0035]
(3)利用高斯模型对每个栅格进行拟合，提取其特征信息，同时也考虑到每个栅格中的点的密度信息，利用频数、高斯拟合等多元的统计信息来表达点云特征，鲁棒性更强。
[0036]
(4)通过这个方案能够实现在20米范围内的全局定位，耗时在毫秒级别。
附图说明
[0037]
下面结合附图和实施例对本发明作进一步说明。
[0038]
图1是本发明全局定位方法的流程图。
[0039]
图2是三维点云地图栅格划分示意图。
[0040]
图3是基于多远统计信息的全局定位配准结果示意图。
具体实施方式
[0041]
现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
[0042]
如图1所示，本发明的一种基于多元统计信息的大范围场景全局定位方法，包括以下步骤：
[0043]
step1：栅格粗划分
[0044]
采用多线激光雷达获得场景的三维点云，并根据三维点云构建场景的三维点云地图。对场景的三维点云地图在x、y、z方向上分别进行栅格划分，获得三维栅格地图；其中，如图2所示，栅格划分的具体过程为：
[0045]
step1.1：分别确定x、y、z方向上的分辨率r
x
、ry、rz；其中分辨率r
x
、ry、rz构成一个三维栅格，各个方向上划分的分辨率不一定相同；
[0046]
在确定在xoy平面分辨率时，x方向、y方向和z方向上的分辨率可以相同也可以不同，若三个维度上的分辨率保持不同，则相对来说更加灵活。
[0047]
step1.2：根据分辨率和三维点云地图的大小，确定划分的栅格数量k；
[0048]
step1.3：根据分辨率和栅格数量k，对场景中的三维点云地图进行栅格划分，获得三维栅格地图。
[0049]
本实施例中的三维点云地图可以是提前构建好的，可以将构建好的场景的三维点云地图直接应用在本发明的定位方法中，也可以是使用时根据具体场景进行构建。
[0050]
step2：特征计算
[0051]
统计三维栅格地图中落入每个三维栅格中的点云，获得每个三维栅格的点云集合vi，则并统计所有点云集合中的点云最大频数max
p
，则max
p
＝max{n1,n2,
…
,nk}；同时，对每个三维栅格在z方向上进行高斯拟合，计算出每个三维栅格的均值和方差；其中，vi表示第i个三维栅格的点云集合，ni表示集合vi中的点的数量，不同的集合，点的数量不尽相同，i表示三维栅格的序号，i＝1,2,
…
k，k表示三维点云地图划分的栅格数量；
[0052]
其中，频数、均值和方差均表示三维栅格地图的特征，其中，频数是分布特征，表示的是多线激光雷达扫描的点落到三维栅格地图中的概率；均值和方差是统计特征，表示的是多线激光雷达扫描的点落到三维栅格地图中，该点与三维栅格中点的相似度。
[0053]
step3：z方向上栅格细化分
[0054]
如图2所示，将每个三维栅格在z方向上以一定的分辨率z
res
细划分，且z
res
《rz，按照下面公式计算点击中细分栅格后的得分：
[0055][0056]
其中，下标i表示三维栅格的序号，i＝1,2,
…
k，j表示在第i个栅格中按照z方向细划分后的栅格序号，α表示常数系数，z表示高度，rz表示三维点云地图进行栅格划分时z方向上的分辨率，z
res
表示每个三维栅格细化分时z方向上的分辨率，μi表示序号为i的三维栅格的均值，σi表示序号为i的三维栅格的方差；
[0057]
即为待匹配点云中单个点击中目标栅格后的得分，对划分的栅格进行一元高斯拟合及点云密度信息统计，实现的多元信息融合特征表达。
[0058]
其中，step1是进行的第一次栅格划分，step3在第一次栅格划分基础上，再对z方
向上进行细化分，即进行第二次栅格划分，经过这个步骤后，可以把三维空间高精度点云地图划分为一系列三维栅格，并计算出了待匹配点云击中目标栅格后的得分。
[0059]
可以提前计算点击中每个细分栅格的得分，并组合成一个得分表，采用这种方式，把三维点云地图预划分为三维的得分查找表，在后续的计算中可以直接采用查表的方式，加速计算。
[0060]
step4：点云配准定位
[0061]
将待配准的点云按照csm的全局搜索方法进行搜索，选择总得分最高的变换作为最优结果，即最终定位结果。
[0062]
csm的全局搜索方法是已经有的技术方案，就是采用多分辨形式的分支界定算法来实现的，不是本发明要求保护的创新点所在，因此，此处不作详细赘述。
[0063]
采用本发明基于多元统计信息的全局定位方法对点云地图进行处理，实现激光雷达点云与地图的配准，配准结果如图3所示，其中，白色点云为高精度矢量地图，黄色点云为初始待匹配点云，红色点云为配准后点云，其中，红色点云能够和地图完全匹配，黄色没有匹配，只是为了表征在巨大旋转和位移误差下面，该方案也能有效实现点云和地图的匹配。
[0064]
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。