阅读说明：本技术 激光雷达匹配定位方法及装置 (Laser radar matching positioning method and device ) 是由 梁宝华 黄友 张国龙 张放 李晓飞 张德兆 王肖 霍舒豪 于 2019-09-19 设计创作，主要内容包括：本发明提供了一种激光雷达匹配定位方法,包括：获取当前激光雷达点云信息；根据位置坐标,将地面点和地面以上点映射在预设的栅格地图上；计算栅格地图中的每个栅格的地面点的分布和地面以上点的分布；根据每个栅格的地面点的分布和地面以上点的分布,计算每个栅格中各点的概率；以栅格地图中的第一栅格为中心,确定预设搜索范围内的栅格集；当车辆位于栅格集中的任一栅格时,根据与车辆高度对应的点的概率,计算不同偏航角对应的匹配概率；根据栅格集中的每个栅格中不同偏航角对应的匹配概率,确定概率矩阵；根据概率矩阵,对定位结果进行评价,生成评价结果；根据评价结果,确定车辆的定位结果。由此,排除了场景的差别,能很好适应多场景。(The invention provides a laser radar matching positioning method, which comprises the following steps: acquiring current laser radar point cloud information; according to the position coordinates, mapping the ground points and the points above the ground on a preset grid map; calculating the distribution of the ground points of each grid in the grid map and the distribution of the points above the ground; calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground; determining a grid set in a preset search range by taking a first grid in a grid map as a center; when the vehicle is positioned in any grid in the grid set, calculating the matching probability corresponding to different yaw angles according to the probability of points corresponding to the height of the vehicle; determining a probability matrix according to the matching probability corresponding to different yaw angles in each grid in the grid set; evaluating the positioning result according to the probability matrix to generate an evaluation result; and determining the positioning result of the vehicle according to the evaluation result. Therefore, the method eliminates the difference of scenes and can be well adapted to multiple scenes.)

激光雷达匹配定位方法及装置

技术领域

本发明涉及自动驾驶领域，尤其涉及激光雷达匹配定位方法及装置。

背景技术

近年来无人驾驶关键技术的不断突破与发展，大大加快了无人驾驶技术的落地与普及。无人驾驶车辆的正常行驶离不开车辆实时高精度定位，高精度定位作为实现无人驾驶的关键技术，其方式主要包括高精度差分GNSS(Global Navigation Satellite System)定位、激光雷达匹配定位、视觉定位等。激光雷达匹配定位作为其中重要的定位手段，快速且稳定的定位以及准确的定位效果评价是其最关键的因素。

激光雷达匹配定位技术一般包括计算最优匹配定位结果和定位结果评价两部分。

目前大多数激光雷达匹配方法使用非线性优化技术，以最大化激光匹配概率为目标，计算最优的匹配结果。当评价激光匹配结果是否可用时，往往是基于最优结果的匹配概率与固定阈值比较，来评价结果的可用性，如果大于阈值认为匹配可用，小于阈值认为匹配失败。

基于优化匹配概率计算定位结果存在如下问题：该方法只提供最优解对应的概率值，不能提供搜索空间内的概率分布，损失了大量的信息，而利用这些信息却可以进行更有效的定位结果判定，甚至可以通过这些信息进行场景的识别。

用匹配概率作为匹配的评价指标存在如下问题：不同场景，评价匹配结果是否有效的概率阈值往往不同。比如，有些场景以树木植被为主，有些场景以楼宇为主，不同的场景往往有不同的匹配概率；相同场景，随着季节变化，匹配的概率也随之变化。比如多植被场景，冬季落叶，夏季繁茂，不同季节的匹配概率也相应变化。所以，匹配概率不能作为稳定、统一的手段作为判断定位是否可用的标准。

发明内容

本发明实施例的目的是提供一种激光雷达匹配定位方法及装置，以解决现有技术中的以匹配概率作为判断定位可用的标准所存在的信息损失量大及不能有效的进行定位结果的判定的问题。

为解决上述问题，第一方面，本发明提供了一种激光雷达匹配定位方法，所述方法包括：

获取当前激光雷达点云信息；其中，所述激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标；

根据所述位置坐标，将所述地面点和所述地面以上点映射在预设的栅格地图上；

计算所述栅格地图中的每个栅格的地面点的分布和地面以上点的分布；

根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；

以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，所述第一栅格为定位结果预测值所在的栅格；

当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；

根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵；

根据所述概率矩阵，对定位结果进行评价，生成评价结果；

根据所述评价结果，确定车辆的定位结果。

在一种可能的实现方式中，所述获取当前激光雷达点云信息之前，所述方法还包括：

获取原始激光点云信息；

对所述原始激光点云信息进行坐标转换，得到世界坐标系下的当前激光点云信息。

在一种可能的实现方式中，所述根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率，具体包括：

根据公式计算每个栅格中各点的概率；

其中，(μ^g，σ^g)为地面点的分布，(μ^o，σ^o)为地面以上点的分布，每个栅格在栅格地图的位置为(i，j)，每个栅格在世界坐标系的位置为(x，y)，且(i，j)＝(x/r，y/r)，r为栅格地图的分辨率，p_z|i，j为(i，j)栅格内的点在高度为z时的概率，p_z|i，j＝p_(x，y，z)。

在一种可能的实现方式中，所述当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率，得到每个栅格的匹配概率集，具体包括：

根据公式计算不同偏航角对应的匹配概率；

其中，points为原始点云points_raw经过坐标变化后，在世界坐标系下的点云；在世界坐标系下，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方向的高斯分布参数与预设的阈值进行比较，如果每一个分布参数都小于预设的阈值中的参数时，评价结果为定位结果。

在一种可能的实现方式中，根据公式对概率矩阵进行归一化处理；其中，P_ij为概率矩阵，P0_ij为归一化后的概率矩阵，sum(P)为概率矩阵所有元素之和；

根据公式计算在x方向和y方向的高斯分布参数；其中，μ_x为x方向的均值，σ_x为x方向的方差，μ_y为y方向的均值，σ_y为y方向的方差；

如果则定位结果可用，其中，abs()为取绝对值函数，μ₀，σ₀分别为均值阈值和均方差阈值。

在一种可能的实现方式中，所述根据所述评价结果，确定车辆的定位结果，具体包括：

根据公式x＝μ_x*r，y＝σ_y*r得到车辆在世界坐标系的目标第一坐标和目标第二坐标；其中，r为栅格地图的分辨率；

根据概率矩阵中对应yaw_max，得到车辆在世界坐标系的目标偏航角；

根据车辆在世界坐标系的目标第一坐标和目标第二坐标，计算所述目标第一坐标、目标第二坐标对应的栅格的均值μ^g，得到车辆在世界坐标系的目标第三坐标；其中，所述目标第一、第二、第三坐标构成目标位置，所述目标位置、目标偏航角构成定位结果。

第二方面，本发明提供了一种激光雷达匹配定位装置，所述装置包括：

获取单元，所述获取单元用于获取当前激光雷达点云信息；其中，所述激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标；

映射单元，所述映射单元用于根据所述位置坐标，将所述地面点和所述地面以上点映射在预设的栅格地图上；

计算单元，所述计算单元用于计算所述栅格地图中的每个栅格的地面点的分布和地面以上点的分布；

所述计算单元还用于，根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率；

确定单元，所述确定单元用于以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，所述第一栅格为定位结果预测值所在的栅格；

所述计算单元还用于，当所述车辆位于所述栅格集中的任一栅格时，根据与所述车辆高度对应的点的概率，计算不同偏航角对应的匹配概率；

所述确定单元还用于，根据所述栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵；

评价单元，所述评价单元用于根据所述概率矩阵，对定位结果进行评价，生成评价结果；

所述确定单元还用于，根据所述评价结果，确定车辆的定位结果。

第三方面，本发明提供了一种设备，包括存储器和处理器，所述存储器用于存储程序，所述处理器用于执行第一方面任一所述的方法。

第四方面，本发明提供了一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如第一方面任一所述的方法。

第五方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面任一所述的方法。

通过本发明实施例提供的激光雷达匹配定位方法及装置，将实时点云分为地面部分和地面以上部分，实现车辆位置姿态六自由度的解耦，加快了搜索速度，得到的概率矩阵可为定位结果评价提供更多可参考信息。将归一化的概率矩阵拟合成概率的高斯分布，通过判断高斯分布的均值和方差，判断定位结果可用性，均值代表了匹配结果与预测值的偏差，此指标与场景无关，只与预测值的精度关联。由于对概率矩阵进行归一化处理，排除了场景的差别，所以，能很好适应多场景。

附图说明

图1为本发明实施例一提供的激光雷达匹配定位方法流程示意图；

图2为本发明实施例一提供的概率矩阵示意图；

图3为本发明实施例二提供的激光雷达匹配定位装置结构示意图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

图1为本发明实施例一提供的激光雷达匹配定位方法流程示意图，该方法应用在安装有激光雷达的终端上，比如安装有激光雷达的无人驾驶车辆或机器人，本申请的执行主体为具有计算功能的终端、服务器或者处理器。本申请以将该方法应用在无人驾驶车辆为例进行说明，当将该方法应用在无人驾驶车辆时，该方法的执行主体为自动驾驶车辆控制单元(Automated Vehicle Control Unit，AVCU)，即无人驾驶车辆的中央处理器相当于无人驾驶车辆的“大脑”。如图1所示，本申请包括以下步骤：

步骤101，获取当前激光雷达点云信息；激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标。

具体的，激光雷达具有垂直排布的激光探头，激光探头采用飞行时间(time offly，ToF)技术测量距离信息，激光雷达在运行过程中绕垂直轴高速旋转，激光探头高频测量环境信息。激光雷达旋转一圈，即可采集一圈周围的环境数据，周围的环境信息以离散稀疏的三维空间坐标的形式提供，称为原始激光点云信息。

其中，周围的环境信息包括但不限于以树木植被为主的场景、以楼宇为主的场景，本申请对此并不限定，本申请可以适用多种场景。

在该步骤中，原始激光点云信息可以是基于激光坐标系的，也可以根据车辆与激光雷达的安装位置，得到车辆坐标系下的点云信息，在进行坐标转换后，得到世界坐标系下的当前激光点云信息，简称激光点云信息。

步骤102，根据位置坐标，将地面点和地面以上点映射在预设的栅格地图上。

具体的，在无人驾驶车辆未正式上路之前，可以进行测试，在测试阶段，可以进行激光点云地图的构建，具体的构建方法，可以是采用3D激光匹配算法进行点云帧间匹配，基于激光传感器的同时定位与建图(Simulttaneously Localization and Mapping，SLAM)方法构建激光点云地图，本申请对此并不限定。

对于构建好的激光点云地图，可以进行划分，得到栅格地图。栅格地图则是把周围环境信息划分成一系列栅格，可以看做以一定的分辨率r离散化的XY栅格平面。每个栅格用整数对(i，j)表示其在栅格地图中的位置，此处的位置指的是栅格中左上角的位置。那么实际位置，即在世界坐标系的位置(x，y)对应的栅格地图位置(i，j)＝(x/r，y/r)。

步骤103，计算栅格地图中的每个栅格的地面点的分布和地面以上点的分布。

具体的，将激光点云信息映射到栅格地图中，一个栅格中存在多个点，包括地面点和地面以上点。根据栅格中地面点的位置坐标，计算地面点的高斯分布(μ^g，σ^g)，其中，μ^g为地面点高斯分布的均值，σ^g为地面点高斯分布的方差。

根据栅格中地面点的位置坐标，计算地面点的高斯分布(μ^o，σ^o)，其中，μ^o为地面点高斯分布的均值，σ^o为地面点高斯分布的方差。

步骤104，根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率。

具体的，对于落在(i，j)栅格的点，该点对应的概率就是该点的z坐标对应的概率p_z|i，j。

根据公式计算每个栅格中各点的概率；

其中，(μ^g，σ^g)为地面点的分布，(μ^o，σ^o)为地面以上点的分布，每个栅格在栅格地图的位置为(i，j)，每个栅格在世界坐标系的位置为(x，y)，且(i，j)＝(x/r，y/r)，r为栅格地图的分辨率，p_z|i，j为(i，j)栅格内的点在高度为z时的概率。

因此，一个点(x，y，z)对应的概率可以表示为

步骤105，以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集。

其中，激光雷达匹配定位需要确定车辆在空间中的位置坐标(x，y，z)和姿态(roll，pitch，yaw)，(x，y，yaw)三个自由度是车辆最重要的自由度，也是需要重点计算的自由度。其中翻滚角roll，俯仰角pitch可以使用车载的惯性测量单元(Inertialmeasurement unit，IMU)计算得到。由于车辆在地面上行驶，当计算得到车辆的车辆(x，y)坐标后，车辆z为(x，y)对应栅格的地面高斯参数μ^g。因此，下述重点对(x，y，yaw)这三个自由度的计算进行说明。

其中，第一栅格为预测值所在的栅格。可以以x，y方向的搜索栅格数为m，偏航角yaw方向的搜索范围为da，x方向的栅格的搜索范围为(r-m，r+m)，y方向的栅格搜索范围为(c-m，c+m)，yaw的搜索范围为(yaw-da，yaw+da)作为预设的搜索范围。在上述范围内的栅格以及第一栅格，一起构成栅格集。

步骤106，当车辆位于栅格集中的任一栅格时，根据与车辆高度对应的点的概率，计算不同偏航角对应的匹配概率。

具体的，当车辆位于栅格(i，j)＝(xi/r，yi/r)且yaw＝yawi时，可得到车辆高度zi为(i，j)栅格内地面高斯参数μ^g，那么车辆匹配概率P_i，j，yawi可表示为：

其中，points为原始点云points_raw经过坐标变化后，在世界坐标系下的点云。

步骤107，根据栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵。

具体的，当车辆位于栅格(i，j)时，会计算不同偏航角对应的匹配概率，取不同偏航角对应的匹配概率中的最大概率作为栅格(i，j)对应的匹配概率P_ij，其中的最大概率对应的yawi记做yaw_max，即

在预设搜索范围内，所有的P_ij组成该次匹配的概率矩阵。参见图2，图2为栅格集，栅格集中的每一栅格对应一个小格，在预设的搜索范围内，得到栅格集中每个栅格的最大匹配概率，从而构成概率矩阵。

步骤108，根据概率矩阵，对定位结果进行评价，生成评价结果。

其中，步骤108包括以下内容：

首先，对概率矩阵进行归一化处理；然后，根据归一化处理后的概率矩阵，分别计算在x和y方向的高斯分布参数；最后，将在x和y方向的高斯分布参数与预设的阈值进行比较，如果每一个分布参数都小于预设的阈值中的参数时，评价结果为定位结果。

具体的，根据公式对概率矩阵进行归一化处理；其中，P_ij为概率矩阵，P0_ij为归一化后的概率矩阵，sum(P)为概率矩阵所有元素之和；归一化处理的目的是排除了环境场景的差别对定位结果的影响。

根据公式计算在x方向和y方向的高斯分布参数；其中，μ_x为x方向的均值，σ_x为x方向的方差，μ_y为y方向的均值，σ_y为y方向的方差，m为所有元素的个数；

如果则定位结果可用，其中，abs()为取绝对值函数，μ₀，σ₀分别为均值阈值和均方差阈值。

步骤109，根据评价结果，确定车辆的定位结果。

具体的，可以根据公式x＝μ_x*r，y＝σ_y*r得到车辆在世界坐标系的目标第一坐标和目标第二坐标。

根据概率矩阵中对应yaw_max，得到车辆在世界坐标系的目标偏航角。

根据车辆在世界坐标系的目标第一坐标和目标第二坐标，计算目标第一坐标、目标第二坐标对应的栅格的均值μ^g，得到车辆在世界坐标系的目标第三坐标；其中，目标第一、第二、第三坐标构成目标位置，目标位置、目标偏航角构成定位结果。

通过本发明实施例提供的激光雷达匹配定位方法，将实时点云分为地面部分和地面以上部分，实现车辆位置姿态六自由度的解耦，加快了搜索速度，得到的概率矩阵可为定位结果评价提供更多可参考信息。将归一化的概率矩阵拟合成概率的高斯分布，通过判断高斯分布的均值和方差，判断定位结果可用性，均值代表了匹配结果与预测值的偏差，此指标与场景无关，只与预测值的精度关联。由于对概率矩阵进行归一化处理，排除了场景的差别，所以，此方法能很好适应多场景。

图3为本发明实施例二提供的激光雷达匹配定位装置结构示意图，该激光雷达匹配定位装置应用在实施例一中的激光雷达匹配定位方法中，如图3所示，激光雷达匹配定位装置包括：获取单元301、映射单元302、计算单元303、确定单元304和评价单元305。

获取单元301用于获取当前激光雷达点云信息；其中，激光雷达点云信息包括地面点和地面以上点，每个点具有位置坐标。

映射单元302用于根据位置坐标，将地面点和地面以上点映射在预设的栅格地图上。

计算单元303用于计算栅格地图中的每个栅格的地面点的分布和地面以上点的分布。

计算单元303还用于，根据每个栅格的地面点的分布和地面以上点的分布，计算每个栅格中各点的概率。

确定单元304用于以栅格地图中的第一栅格为中心，确定预设搜索范围内的栅格集；其中，第一栅格为定位结果预测值所在的栅格。

计算单元303还用于，当车辆位于栅格集中的任一栅格时，根据与车辆高度对应的点的概率，计算不同偏航角对应的匹配概率。

确定单元304还用于，根据栅格集中的每个栅格中不同偏航角对应的匹配概率，确定概率矩阵。

评价单元305用于根据概率矩阵，对定位结果进行评价，生成评价结果。

确定单元304还用于，根据评价结果，确定车辆的定位结果。

每个单元的具体作用与上述实施例一中的方法相对应，此处不再赘述。

通过本发明实施例提供的激光雷达匹配定位装置，将实时点云分为地面部分和地面以上部分，实现车辆位置姿态六自由度的解耦，加快了搜索速度，得到的概率矩阵可为定位结果评价提供更多可参考信息。将归一化的概率矩阵拟合成概率的高斯分布，通过判断高斯分布的均值和方差，判断定位结果可用性，均值代表了匹配结果与预测值的偏差，此指标与场景无关，只与预测值的精度关联。由于对概率矩阵进行归一化处理，排除了场景的差别，所以，能很好适应多场景。

发明实施例三提供了一种设备，包括存储器和处理器，存储器用于存储程序，存储器可通过总线与处理器连接。存储器可以是非易失存储器，例如硬盘驱动器和闪存，存储器中存储有软件程序和设备驱动程序。软件程序能够执行本发明实施例提供的上述方法的各种功能；设备驱动程序可以是网络和接口驱动程序。处理器用于执行软件程序，该软件程序被执行时，能够实现本发明实施例一提供的方法。

本发明实施例四提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行本发明实施例一提供的方法。

本发明实施例五提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现本发明实施例一提供的方法。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。