激光雷达匹配定位方法及装置制造方法及图纸

本发明专利技术提供了一种激光雷达匹配定位方法，包括：获取当前激光雷达点云信息；根据位置坐标，将地面点和地面以上点映射在预设的栅格地图上；计算栅格地图中的每个栅格的地面点的分布和地面以上点的分布；根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；当车辆位于栅格集中的任一栅格时，根据与车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；根据栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵；根据概率矩阵，对定位结果进行评价，生成评价结果；根据评价结果，确定车辆的定位结果。由此，排除了场景的差别，能很好适应多场景。

Laser radar matching positioning method and device

【技术实现步骤摘要】
激光雷达匹配定位方法及装置
本专利技术涉及自动驾驶领域，尤其涉及激光雷达匹配定位方法及装置。
技术介绍
近年来无人驾驶关键技术的不断突破与发展，大大加快了无人驾驶技术的落地与普及。无人驾驶车辆的正常行驶离不开车辆实时高精度定位，高精度定位作为实现无人驾驶的关键技术，其方式主要包括高精度差分GNSS(GlobalNavigationSatelliteSystem)定位、激光雷达匹配定位、视觉定位等。激光雷达匹配定位作为其中重要的定位手段，快速且稳定的定位以及准确的定位效果评价是其最关键的因素。激光雷达匹配定位技术一般包括计算最优匹配定位结果和定位结果评价两部分。目前大多数激光雷达匹配方法使用非线性优化技术，以最大化激光匹配概率为目标，计算最优的匹配结果。当评价激光匹配结果是否可用时，往往是基于最优结果的匹配概率与固定阈值比较，来评价结果的可用性，如果大于阈值认为匹配可用，小于阈值认为匹配失败。基于优化匹配概率计算定位结果存在如下问题：该方法只提供最优解对应的概率值，不能提供搜索空间内的概率分布，损失了大量的信息，而利用这些信息却可以进行更有效的定位结果判定，甚至可以通过这些信息进行场景的识别。用匹配概率作为匹配的评价指标存在如下问题：不同场景，评价匹配结果是否有效的概率阈值往往不同。比如，有些场景以树木植被为主，有些场景以楼宇为主，不同的场景往往有不同的匹配概率；相同场景，随着季节变化，匹配的概率也随之变化。比如多植被场景，冬季落叶，夏季繁茂，不同季节的匹配概率也相应变化。所以，匹配概率不能作为稳定、统一的手段作为判断定位是否可用的标准。
技术实现思路
本专利技术实施例的目的是提供一种激光雷达匹配定位方法及装置，以解决现有技术中的以匹配概率作为判断定位可用的标准所存在的信息损失量大及不能有效的进行定位结果的判定的问题。为解决上述问题，第一方面，本专利技术提供了一种激光雷达匹配定位方法，所述方法包括：获取当前激光雷达点云信息；其中，所述激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标；根据所述位置坐标，将所述地面点和所述地面以上点映射在预设的栅格地图上；计算所述栅格地图中的每个栅格的地面点的分布和地面以上点的分布；根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，所述第一栅格为定位结果预测值所在的栅格；当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵；根据所述概率矩阵，对定位结果进行评价，生成评价结果；根据所述评价结果，确定车辆的定位结果。在一种可能的实现方式中，所述获取当前激光雷达点云信息之前，所述方法还包括：获取原始激光点云信息；对所述原始激光点云信息进行坐标转换，得到世界坐标系下的当前激光点云信息。在一种可能的实现方式中，所述根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率，具体包括：根据公式计算每个栅格中各点的概率；其中，(μg，σg)为地面点的分布，(μo，σo)为地面以上点的分布，每个栅格在栅格地图的位置为(i，j)，每个栅格在世界坐标系的位置为(x，y)，且(i，j)＝(x/r，y/r)，r为栅格地图的分辨率，pz|i，j为(i，j)栅格内的点在高度为z时的概率，pz|i，j＝p(x，y，z)。在一种可能的实现方式中，所述当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率，得到每个栅格的匹配概率集，具体包括：根据公式计算不同偏航角对应的匹配概率；其中，points为原始点云pointsraw经过坐标变化后，在世界坐标系下的点云；在世界坐标系下，x，y方向的搜索栅格数为m，yaw方向的搜索范围为da，x方向的栅格的搜索范围为(r-m，r+m)，y方向的栅格搜索范围为(c-m，c+m)，yaw的搜索范围为(yaw-da，yaw+da)，yaw为偏航角。在一种可能的实现方式中，所述根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵，具体包括：根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定每个栅格中的最大的匹配概率；每个栅格中的最大的匹配概率，构成概率矩阵。在一种可能的实现方式中，所述根据所述概率矩阵，对定位结果进行评价，生成评价结果，具体包括：对所述概率矩阵进行归一化处理；根据归一化处理后的所述概率矩阵，分别计算在x和y方向的高斯分布参数；将所述在x和y方向的高斯分布参数与预设的阈值进行比较，如果每一个分布参数都小于预设的阈值中的参数时，评价结果为定位结果。在一种可能的实现方式中，根据公式对概率矩阵进行归一化处理；其中，Pij为概率矩阵，P0ij为归一化后的概率矩阵，sum(P)为概率矩阵所有元素之和；根据公式计算在x方向和y方向的高斯分布参数；其中，μx为x方向的均值，σx为x方向的方差，μy为y方向的均值，σy为y方向的方差；如果则定位结果可用，其中，abs()为取绝对值函数，μ0，σ0分别为均值阈值和均方差阈值。在一种可能的实现方式中，所述根据所述评价结果，确定车辆的定位结果，具体包括：根据公式x＝μx*r，y＝σy*r得到车辆在世界坐标系的目标第一坐标和目标第二坐标；其中，r为栅格地图的分辨率；根据概率矩阵中对应yaw_max，得到车辆在世界坐标系的目标偏航角；根据车辆在世界坐标系的目标第一坐标和目标第二坐标，计算所述目标第一坐标、目标第二坐标对应的栅格的均值μg，得到车辆在世界坐标系的目标第三坐标；其中，所述目标第一、第二、第三坐标构成目标位置，所述目标位置、目标偏航角构成定位结果。第二方面，本专利技术提供了一种激光雷达匹配定位装置，所述装置包括：获取单元，所述获取单元用于获取当前激光雷达点云信息；其中，所述激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标；映射单元，所述映射单元用于根据所述位置坐标，将所述地面点和所述地面以上点映射在预设的栅格地图上；计算单元，所述计算单元用于计算所述栅格地图中的每个栅格的地面点的分布和地面以上点的分布；所述计算单元还用于，根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；确定单元，所述确定单元用于以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，所述第一栅格为定位结果预测值所在的栅格；所述计算单元还用于，当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；所述确定单元还用于，根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定本文档来自技高网...

【技术保护点】
1.一种激光雷达匹配定位方法，其特征在于，所述方法包括：/n获取当前激光雷达点云信息；其中，所述激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标；/n根据所述位置坐标，将所述地面点和所述地面以上点映射在预设的栅格地图上；/n计算所述栅格地图中的每个栅格的地面点的分布和地面以上点的分布；/n根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；/n以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，所述第一栅格为定位结果预测值所在的栅格；/n当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；/n根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵；/n根据所述概率矩阵，对定位结果进行评价，生成评价结果；/n根据所述评价结果，确定车辆的定位结果。/n

【技术特征摘要】
1.一种激光雷达匹配定位方法，其特征在于，所述方法包括：
获取当前激光雷达点云信息；其中，所述激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标；
根据所述位置坐标，将所述地面点和所述地面以上点映射在预设的栅格地图上；
计算所述栅格地图中的每个栅格的地面点的分布和地面以上点的分布；
根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；
以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，所述第一栅格为定位结果预测值所在的栅格；
当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；
根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵；
根据所述概率矩阵，对定位结果进行评价，生成评价结果；
根据所述评价结果，确定车辆的定位结果。


2.根据权利要求1所述的方法，其特征在于，所述获取当前激光雷达点云信息之前，所述方法还包括：
获取原始激光点云信息；
对所述原始激光点云信息进行坐标转换，得到世界坐标系下的当前激光点云信息。


3.根据权利要求1所述的方法，其特征在于，所述根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率，具体包括：
根据公式计算每个栅格中各点的概率；
其中，(μg，σg)为地面点的分布，(μo，σo)为地面以上点的分布，每个栅格在栅格地图的位置为(i，j)，每个栅格在世界坐标系的位置为(x，y)，且(i，j)＝(x/r，y/r)，r为栅格地图的分辨率，pz|i，j为(i,j)栅格内的点在高度为z时的概率，pz|i，j＝p(x，y，z)。


4.根据权利要求1所述的方法，其特征在于，所述当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率，得到每个栅格的匹配概率集，具体包括：
根据公式计算不同偏航角对应的匹配概率；
其中，points为原始点云pointsraw经过坐标变化后，在世界坐标系下的点云；在世界坐标系下，x,y方向的搜索栅格数为m，yaw方向的搜索范围为da，x方向的栅格的搜索范围为(r-m,r+m),y方向的栅格搜索范围为(c-m,c+m),yaw的搜索范围为(yaw-da,yaw+da)，yaw为偏航角。


5.根据权利要求1所述的方法，其特征在于，所述根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵，具体包括：
根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定每个栅格中的最大的匹配概率；
每个栅格中的最大的匹配概率，构成概率矩阵。


6.根据权利要求1所述的方法，其特征在于，所述根据所...

【专利技术属性】
技术研发人员：梁宝华，黄友，张国龙，张放，李晓飞，张德兆，王肖，霍舒豪，
申请(专利权)人：北京智行者科技有限公司，
类型：发明
国别省市：北京;11