一种激光点云定位方法和系统与流程
本申请涉及自动控制领域，尤其涉及一种激光点云定位方法和系统。
背景技术：
自动驾驶车辆是一种不需要人工控制即可对其周围环境进行感知，对场景进行决策判断，对其进行控制的现代车辆。在自动驾驶车辆中，定位模块是自动驾驶系统中的核心基础模块，不仅是路径规划模块必不可少的输入，而且也可以简化感知模块的场景理解及分类算法。现有技术方案中一般都采用惯性导航系统加激光点云定位系统的组合，目前，采用安装有高端imu的惯性导航系统，以及激光点云定位系统的定位方案已在谷歌、百度自动驾驶车辆上取得了应用。
其中，惯性导航系统提供自动驾驶车辆的初始姿态，包含有位置信息(平面位置x,y以及高度z)以及姿态角(滚转角roll、俯仰角pitch和偏航角yaw)。采用激光点云定位算法对位置信息及偏航角yaw进行优化。
现有技术在对平面位置x,y进行匹配搜索时，一般都使用反射值、高度值等度量，或单独使用反射值，或单独使用高度值，或简单将二者叠加使用。
单独考虑一种度量方式的缺点是当匹配分布较差，可能会得到错误的平面位置x,y；
将二者简单叠加的方式在一定程度能克服一种度量失败的情况，但也存在失败的情况。例如，对于一段重新铺设的路面，反射值匹配的分布会非常差，而高度值匹配的分布则比较好，简单叠加二者的最终结果也会比较差。
因此，单独考虑某一种匹配或简单叠加都不是一个好的解决方法。
技术实现要素：
本申请的多个方面提供一种激光点云定位方法和系统，用以提高定位精度，增强鲁棒性。
本申请的一方面，提供一种激光点云定位方法，包括：
将与自动驾驶车辆当前位置匹配的激光点云反射值数据和高度值数据转化为地平面内的激光点云投影数据；
为激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率分配权重，确定激光点云投影数据与激光点云二维网格地图的匹配概率；
基于所述激光点云投影数据与激光点云二维网格地图的匹配概率确定所述自动驾驶车辆在激光点云二维网格地图中的位置。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述激光点云反射值数据包括各激光点的坐标以及各激光点对应的激光反射强度值；
所述激光点云高度值数据包括各激光点的坐标以及各激光点对应的高度值。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述将激光点云反射值数据和激光点云高度值数据转化为地平面内的激光点云投影数据包括：
将三维空间内各个坐标位置的激光点云反射值投影至地平面，得到地平面内各个坐标位置的激光点云反射值；
将三维空间内各个坐标位置的激光点云高度值投影至地平面，得到地平面内各个坐标位置的激光点云高度值。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述激光点云二维网格地图通过如下方式生成：
将地图栅格化，得到地图网格；
采集与各地图网格的定位位置对应的激光点云反射值数据和激光点云高度值数据；
在各地图网格中存储与之对应的地图数据；
所述地图数据包括与该地图网格对应的定位位置内的各激光点的激光反射强度值的均值、与该地图网格对应的定位位置内的各激光点的激光反射强度值的方差、与该地图网格对应的定位位置内的各激光点的高度值的均值以及与该地图网格对应的定位位置内的各激光点的数量。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述为激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率分配权重，确定激光点云投影数据与激光点云二维网格地图的匹配概率包括：
计算激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率；
根据所述反射值匹配概率的分布的方差以及所述高度值匹配概率的分布的方差，计算所述反射值匹配概率以及所述高度值匹配概率的自适应权重；
根据所述反射值匹配概率、高度值匹配概率以及计算得到的自适应权重，计算激光点云投影数据与激光点云二维网格地图的匹配概率。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述反射值匹配概率的自适应权重γ由以下公式进行计算，
所述高度值匹配概率的自适应权重为1-γ。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，基于所述匹配概率确定所述自动驾驶车辆在激光点云二维网格地图中的位置包括：
基于上一定位位置对所述激光点云投影数据与激光点云二维网格地图的匹配概率进行更新，计算激光点云投影数据在激光点云二维网格地图的预定范围内的后验概率；
基于所述后验概率的分布确定自动驾驶车辆在激光点云二维网格地图中的位置。
根据本发明的另一方面，提供一种激光点云定位系统，包括：
转化模块，用于将与自动驾驶车辆当前位置匹配的激光点云反射值数据和高度值数据转化为地平面内的激光点云投影数据；
匹配概率确定模块，用于为激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率分配权重，确定激光点云投影数据与激光点云二维网格地图的匹配概率；
位置确定模块，用于基于所述激光点云投影数据与激光点云二维网格地图的匹配概率确定所述自动驾驶车辆在激光点云二维网格地图中的位置。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述激光点云反射值数据包括各激光点的坐标以及各激光点对应的激光反射强度值；
所述激光点云高度值数据包括各激光点的坐标以及各激光点对应的高度值。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述转化模块，具体用于：
将三维空间内各个坐标位置的激光点云反射值投影至地平面，得到地平面内各个坐标位置的激光点云反射值；
将三维空间内各个坐标位置的激光点云高度值投影至地平面，得到地平面内各个坐标位置的激光点云高度值。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述激光点云二维网格地图通过如下方式生成：
将地图栅格化，得到地图网格；
采集与各地图网格的定位位置对应的激光点云反射值数据和激光点云高度值数据；
在各地图网格中存储与之对应的地图数据；
所述地图数据包括与该地图网格对应的定位位置内的各激光点的激光反射强度值的均值、与该地图网格对应的定位位置内的各激光点的激光反射强度值的方差、与该地图网格对应的定位位置内的各激光点的高度值的均值以及与该地图网格对应的定位位置内的各激光点的数量。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述匹配概率确定模块具体用于：
计算激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率；
根据所述反射值匹配概率的分布的方差以及所述高度值匹配概率的分布的方差，计算所述反射值匹配概率以及所述高度值匹配概率的自适应权重；
根据所述反射值匹配概率、高度值匹配概率以及计算得到的自适应权重，计算激光点云投影数据与激光点云二维网格地图的匹配概率。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述反射值匹配概率的自适应权重γ由以下公式进行计算，
所述高度值匹配概率的自适应权重为1-γ。
如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述位置确定模块具体用于：
基于上一定位位置对所述激光点云投影数据与激光点云二维网格地图的匹配概率进行更新，计算激光点云投影数据在激光点云二维网格地图的预定范围内的后验概率；
基于所述后验概率的分布确定自动驾驶车辆在激光点云二维网格地图中的位置。
本发明的另一方面，提供一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如以上所述的方法。
本发明的另一方面，提供一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现如以上所述的方法。
由所述技术方案可知，本申请实施例，提高了激光点云定位的精度，增强了鲁棒性。
【附图说明】
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请一实施例提供的激光点云定位方法的流程示意图；
图2为本申请一实施例提供的激光点云定位系统的结构示意图；
图3示出了适于用来实现本发明实施方式的示例性计算机系统/服务器012的框图。
【具体实施方式】
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的全部其他实施例，都属于本申请保护的范围。
可应用本申请的激光点云定位方法或激光点云定位系统的实施例的示例性系统架构可以包括自动驾驶车辆、网络和服务器。
网络用于在自动驾驶车辆和服务器之间提供通信链路的介质，可以包括各种连接类型，例如有线、无线通信链路等。
自动驾驶车辆可以通过网络与服务器交互，以接收发送消息等。自动驾驶车辆上可以安装有lidar激光雷达、惯性导航系统、通信装置等。
服务器可以是提供各种服务的服务器，例如对自动驾驶车辆采集到的激光点云反射值数据和高度值数据进行处理，并将处理结果(例如自动驾驶车辆的定位信息)反馈给自动驾驶车辆的服务器。
需要说明的是，本申请实施例所提供的激光点云定位方法可以由自动驾驶车辆执行，或者由服务器执行，或者一部分步骤由自动驾驶车辆执行且另一部分步骤由服务器执行。相应地，激光点云定位系统可以设置于服务器中，或者设置于自动驾驶车辆中，或者一部分模块设置在服务器中且另一部分设置在自动驾驶车辆中。
图1为本申请一实施例提供的激光点云定位方法的流程示意图，如图1所示，包括以下步骤：
步骤s11、获取与自动驾驶车辆当前位置匹配的第一激光点云反射值数据和高度值数据；
步骤s12、将第一激光点云反射值数据和第一激光点云高度值数据转化为地平面内的激光点云投影数据；
步骤s13、分别计算激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率，为反射值匹配概率与高度值匹配概率分配权重，确定激光点云投影数据与激光点云二维网格地图的匹配概率；
步骤s14、基于上一定位位置对所述激光点云投影数据与激光点云二维网格地图的匹配概率进行更新，计算激光点云投影数据在激光点云二维网格地图的预定范围内的后验概率；
步骤s15、基于所述后验概率的分布确定自动驾驶车辆在激光点云二维网格地图中的位置。
在步骤s11的一种优选实现方式中，
第一激光点云反射值数据包括各激光点的第一坐标以及第一激光点云反射值数据中的各激光点对应的激光反射强度值；第一激光点云高度值数据包括各激光点的第一坐标以及第一激光点云高度值数据中的各激光点对应的高度值。
在一些可选的实现方式中，所述激光点的第一坐标可以是各激光点在自动驾驶车辆的车辆坐标系下的坐标。
激光点云的反射值是激光点照射物体后的反射强度，其数值范围例如可以是0～255。
假设激光照射到物体后反射的激光点的坐标是(x,y,z)，可将z作为激光点的高度值。
可以由自动驾驶车辆的激光点云反射值采集装置来采集自动驾驶车辆当前位置的第一激光点云反射值数据，由设置于自动驾驶车辆上的激光点云高度值采集装置来采集自动驾驶车辆当前位置的第一激光点云高度值数据；再通过有线或无线连接方式上传至服务器中，以实现对第一激光点云反射值数据和第一激光点云高度值数据的获取。
需要指出的是，上述无线连接方式可以包括但不限于3g/4g连接、wifi连接、蓝牙连接、wimax连接、zigbee连接、uwb(ultrawideband)连接、以及其他现在已知或将来开发的无线连接方式。
在步骤s12的一种优选实现方式中，
通过将第一激光点云反射值数据投影至地平面，可以将在步骤s21中获取到的三维空间内各个坐标位置的激光点云反射值转换为地平面内各个坐标位置的激光点云反射值。
通过将第一激光点云高度值数据至投影地平面，可以将在步骤s21中获取到的三维空间内各个坐标位置的激光点云高度值转换为地平面内各个坐标位置的激光点云高度值。
在一些可选的实现方式中，所述将第一激光点云反射值数据和第一激光点云高度值数据转化为地平面内的激光点云投影数据可以进一步包括：
步骤s121、将第一激光点云反射值数据和第一激光点云高度值数据转换为第三激光点云反射值数据和第三激光点云高度值数据。
其中，第三激光点云反射值数据例如可以包括各激光点的第三坐标以及与第一激光点云反射值数据中的各激光点对应的激光反射强度值，其中第三坐标为第一激光点云反射值数据中的各激光点在世界坐标系下的坐标；第三激光点云高度值数据例如可以包括各激光点的第三坐标以及与第一激光点云高度值数据中的各激光点对应的高度值，其中第三坐标为第一激光点云高度值数据中的各激光点在世界坐标系下的坐标。
步骤s122、将第三激光点云反射值数据和第三激光点云高度值数据投影至地平面，生成激光点云投影数据。
将世界坐标系下各个坐标位置的激光点云反射值投影至地平面，得到地平面内各个坐标位置的激光点云反射值；将世界坐标系下各个坐标位置的激光点云高度值投影至地平面，得到地平面内各个坐标位置的激光点云高度值。
其中，所述激光点云投影数据可以包括各激光点的投影坐标、各投影网格内的各激光点的激光反射强度值的均值、各投影网格内的各激光点的激光反射强度值的方差、各投影网格内的各激光点的高度值的均值、各投影网格内的各激光点的高度值的方差以及各投影网格内的各激光点的数量。
各投影网格可以与地图网格具有相同的大小和形状，例如，可以为全等的矩形。
在步骤s13的一种优选实现方式中，
在一些可选的实现方式中，所述激光点云二维网格地图可以通过以下流程来生成：
具体而言，步骤s21，将地球表面在世界坐标系的地平面内划分为m×n个地图区域，其中，各地图区域例如可以具有相同的大小和形状。
在一些可选的实现方式中，世界坐标系可以采用utm坐标系统(universaltransversemercatorsystem)。
步骤s22，将各地图区域进一步划分成m×n个地图网格，其中，各地图网格具有相同的大小和形状。
由于步骤s21中划分出的地图区域可能覆盖较大的区域范围，在生成激光点云二维网格地图的过程中，各地图区域中的激光点数量可能具有相当大的数量级，导致定位处理的计算量较大。此外，当地图区域可能覆盖较大的区域范围时，基于该地图得到的定位结果的精度较低。因此，可以对各地图区域进行进一步地细分，从而减小定位处理的计算量，提高定位结果的定位精度。
步骤s23，采集与各地图网格的定位位置对应的第二激光点云反射值数据和第二激光点云高度值数据，其中，第二激光点云反射值数据包括各激光点在世界坐标系下的第二坐标以及与第二激光点云反射值数据中的各激光点对应的激光反射强度值；第二激光点云高度值数据包括各激光点在世界坐标系下的第二坐标以及与第二激光点云高度值数据中的各激光点对应的高度值。
步骤s24，在各地图网格中存储与之对应的地图数据。其中，地图数据包括与该地图网格对应的定位位置内的各激光点的激光反射强度值的均值、与该地图网格对应的定位位置内的各激光点的激光反射强度值的方差、与该地图网格对应的定位位置内的各激光点的高度值的均值以及与该地图网格对应的定位位置内的各激光点的数量。
在本实施例中，所述激光点云二维网格地图为预先建立好的包含有反射值均值、反射值方差、高度值均值以及高度值方差的二维网格地图m。
步骤s13可以包括以下子步骤：
步骤s131、分别计算激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率；
其中，反射值匹配使用ssd(sumofsquareddifferences)来度量激光点云投影数据z与地图m之间的相似性，计算公式如下：
rz和rm分别表示激光点云投影数据和激光点云二维网格地图的反射强度值的均值，σz和σm分别表示激光点云投影数据和激光点云二维网格地图的反射强度值的方差。
高度值匹配也使用ssd来度量激光点云投影数据z与地图m之间的相似性，计算公式如下：
其中，az和am分别表示激光点云投影数据和激光点云二维网格地图的高度值的均值。
反射值匹配概率p(zr|x,y,m)和高度值匹配概率p(za|x,y,m)由以下公式计算：
其中，η为归一化常量，当α＝e时，以上为高斯分布的函数，α用来调整分布的平滑性，nz为地图网格对应的定位位置内的各激光点的数量。
步骤s132、根据所述反射值匹配概率的分布的方差以及所述高度值匹配概率的分布的方差，计算所述反射值匹配概率的自适应权重γ以及高度值匹配概率的自适应权重1-γ；
其中，所述自适应权重由反射值匹配概率p(zr|x,y,m)和高度值匹配概率p(za|x,y,m)的分布[p(zr|x,y,m)]xy和[p(za|x,y,m)]xy计算，首先分别计算这两个分布在x和y两个方向上的方差，公式如下：
β为一预设常数参数，zr和za分别表示激光点云投影数据中的反射值和高度值信息；
自适应权重γ由以下公式进行计算：
其中，
步骤s133、根据所述反射值匹配概率、高度值匹配概率以及对应的权重，计算激光点云投影数据与激光点云二维网格地图的匹配概率；
其中，激光点云投影数据与激光点云二维网格地图的匹配概率p(z|x,y,m)＝η·p(zr|x,y,m)γ·p(za|x,y,m)1-γ(6)。
在步骤s14的一种优选实现方式中，
使用二维直方图滤波器基于上一定位位置对所述匹配概率进行更新，计算激光点云投影数据在激光点云二维网格地图的预定范围内的后验概率。
所述后验概率
其中，z是由激光点云投影数据生成的在线局部地图，m激光点云二维网格地图，k是∑xyp(z|x,y,m)和
直方图滤波器的每个状态(x,y)的预测概率计算公式如下：
其中，p(i,j)是sins惯导系统，例如惯性导航系统或多传感器融合定位系统预测输出的概率，σ用来描述两帧之间的移动。
在步骤s25的一种优选实现方式中，
选择直方图滤波器中的一个小区域求其期望，所选择的小区域围绕着后验概率p(x,y|z,m)最大值或次大值，若次大值达到最大值一定比例且其距离滤波器中心更近的话，则选择次大值的位置作为小区域的中心；否则，选择最大值的位置作为小区域的中心。
假设小区域为z，那么最优平面位置
图2为本申请一实施例提供的激光点云定位系统的结构示意图，如图2所示，包括：
获取模块21，用于获取与自动驾驶车辆当前位置匹配的第一激光点云反射值数据和高度值数据；
转换模块22，用于将第一激光点云反射值数据和第一激光点云高度值数据转化为地平面内的激光点云投影数据；
匹配概率确定模块23，用于分别计算激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率，为反射值匹配概率与高度值匹配概率分配权重，确定激光点云投影数据与激光点云二维网格地图的匹配概率；
后验概率计算模块24，用于基于上一定位位置对所述激光点云投影数据与激光点云二维网格地图的匹配概率进行更新，计算激光点云投影数据在激光点云二维网格地图的预定范围内的后验概率；
位置确定模块25，用于基于所述后验概率的分布确定自动驾驶车辆在激光点云二维网格地图中的位置。
在获取模块21的一种优选实现方式中，
第一激光点云反射值数据包括各激光点的第一坐标以及第一激光点云反射值数据中的各激光点对应的激光反射强度值；第一激光点云高度值数据包括各激光点的第一坐标以及第一激光点云高度值数据中的各激光点对应的高度值。
在一些可选的实现方式中，所述激光点的第一坐标可以是各激光点在自动驾驶车辆的车辆坐标系下的坐标。
激光点云的反射值是激光点照射物体后的反射强度，其数值范围例如可以是0～255。
假设激光照射到物体后反射的激光点的坐标是(x,y,z)，可将z作为激光点的高度值。
可以由自动驾驶车辆的激光点云反射值采集装置来采集自动驾驶车辆当前位置的第一激光点云反射值数据，由设置于自动驾驶车辆上的激光点云高度值采集装置来采集自动驾驶车辆当前位置的第一激光点云高度值数据；再通过有线或无线连接方式上传至服务器中，以实现对第一激光点云反射值数据和第一激光点云高度值数据的获取。
需要指出的是，上述无线连接方式可以包括但不限于3g/4g连接、wifi连接、蓝牙连接、wimax连接、zigbee连接、uwb(ultrawideband)连接、以及其他现在已知或将来开发的无线连接方式。
在步骤转换模块22的一种优选实现方式中，
通过将第一激光点云反射值数据投影至地平面，可以将在获取模块21获取到的三维空间内各个坐标位置的激光点云反射值转换为地平面内各个坐标位置的激光点云反射值。
通过将第一激光点云高度值数据至投影地平面，可以将获取模块21获取到的三维空间内各个坐标位置的激光点云高度值转换为地平面内各个坐标位置的激光点云高度值。
在一些可选的实现方式中，转换模块22将第一激光点云反射值数据和第一激光点云高度值数据转化为地平面内的激光点云投影数据可以进一步包括：
将第一激光点云反射值数据和第一激光点云高度值数据转换为第三激光点云反射值数据和第三激光点云高度值数据。
其中，第三激光点云反射值数据例如可以包括各激光点的第三坐标以及与第一激光点云反射值数据中的各激光点对应的激光反射强度值，其中第三坐标为第一激光点云反射值数据中的各激光点在世界坐标系下的坐标；第三激光点云高度值数据例如可以包括各激光点的第三坐标以及与第一激光点云高度值数据中的各激光点对应的高度值，其中第三坐标为第一激光点云高度值数据中的各激光点在世界坐标系下的坐标。
将第三激光点云反射值数据和第三激光点云高度值数据投影至地平面，生成激光点云投影数据。
将世界坐标系下各个坐标位置的激光点云反射值投影至地平面，得到地平面内各个坐标位置的激光点云反射值；将世界坐标系下各个坐标位置的激光点云高度值投影至地平面，得到地平面内各个坐标位置的激光点云高度值。
其中，所述激光点云投影数据可以包括各激光点的投影坐标、各投影网格内的各激光点的激光反射强度值的均值、各投影网格内的各激光点的激光反射强度值的方差、各投影网格内的各激光点的高度值的均值、各投影网格内的各激光点的高度值的方差以及各投影网格内的各激光点的数量。
各投影网格可以与地图网格具有相同的大小和形状，例如，可以为全等的矩形。
在匹配概率确定模块23的一种优选实现方式中，
在一些可选的实现方式中，所述激光点云二维网格地图可以通过以下流程来生成：
将地球表面在世界坐标系的地平面内划分为m×n个地图区域，其中，各地图区域例如可以具有相同的大小和形状。
在一些可选的实现方式中，世界坐标系可以采用utm坐标系统(universaltransversemercatorsystem)。
将各地图区域进一步划分成m×n个地图网格，其中，各地图网格具有相同的大小和形状。
由于划分出的地图区域可能覆盖较大的区域范围，在生成激光点云二维网格地图的过程中，各地图区域中的激光点数量可能具有相当大的数量级，导致定位处理的计算量较大。此外，当地图区域可能覆盖较大的区域范围时，基于该地图得到的定位结果的精度较低。因此，可以对各地图区域进行进一步地细分，从而减小定位处理的计算量，提高定位结果的定位精度。
采集与各地图网格的定位位置对应的第二激光点云反射值数据和第二激光点云高度值数据，其中，第二激光点云反射值数据包括各激光点在世界坐标系下的第二坐标以及与第二激光点云反射值数据中的各激光点对应的激光反射强度值；第二激光点云高度值数据包括各激光点在世界坐标系下的第二坐标以及与第二激光点云高度值数据中的各激光点对应的高度值。
在各地图网格中存储与之对应的地图数据。其中，地图数据包括与该地图网格对应的定位位置内的各激光点的激光反射强度值的均值、与该地图网格对应的定位位置内的各激光点的激光反射强度值的方差、与该地图网格对应的定位位置内的各激光点的高度值的均值以及与该地图网格对应的定位位置内的各激光点的数量。
在本实施例中，所述激光点云二维网格地图为预先建立好的包含有反射值均值、反射值方差、高度值均值以及高度值方差的二维网格地图m。
匹配概率确定模块23具体用于：
分别计算激光点云投影数据与激光点云二维网格地图的反射值匹配概率以及高度值匹配概率；
其中，反射值匹配使用ssd(sumofsquareddifferences)来度量激光点云投影数据z与地图m之间的相似性，计算公式如下：
rz和rm分别表示激光点云投影数据和激光点云二维网格地图的反射强度值的均值，σz和σm分别表示激光点云投影数据和激光点云二维网格地图的反射强度值的方差。
高度值匹配也使用ssd来度量激光点云投影数据z与地图m之间的相似性，计算公式如下：
其中，az和am分别表示激光点云投影数据和激光点云二维网格地图的高度值的均值。
反射值匹配概率p(zr|x,y,m)和高度值匹配概率p(za|x,y,m)由以下公式计算：
其中，η为归一化常量，当α＝e时，以上为高斯分布的函数，α用来调整分布的平滑性，nz为地图网格对应的定位位置内的各激光点的数量。
根据所述反射值匹配概率的分布的方差以及所述高度值匹配概率的分布的方差，计算所述反射值匹配概率的自适应权重γ以及高度值匹配概率的自适应权重1-γ；
其中，所述自适应权重由反射值匹配概率p(zr|x,y,m)和高度值匹配概率p(za|x,y,m)的分布[p(zr|x,y,m)]xy和[p(za|x,y,m)]xy计算，首先分别计算这两个分布在x和y两个方向上的方差，公式如下：
β为一预设常数参数，zr和za分别表示激光点云投影数据中的反射值和高度值信息；
自适应权重γ由以下公式进行计算：
其中，
根据所述反射值匹配概率、高度值匹配概率以及对应的权重，计算激光点云投影数据与激光点云二维网格地图的匹配概率；
其中，激光点云投影数据与激光点云二维网格地图的匹配概率p(z|x,y,m)＝η·p(zr|x,y,m)γ·p(za|x,y,m)1-γ(6)。
在后验概率计算模块24的一种优选实现方式中，
所述后验概率计算模块24使用二维直方图滤波器基于上一定位位置对所述匹配概率进行更新，计算激光点云投影数据在激光点云二维网格地图的预定范围内的后验概率。
所述后验概率
其中，z是由激光点云投影数据生成的在线局部地图，m激光点云二维网格地图，k是∑xyp(z|x,y,m)和
直方图滤波器的每个状态(x,y)的预测概率计算公式如下：
其中，p(i,j)是sins惯导系统，例如惯性导航系统或多传感器融合定位系统预测输出的概率，σ用来描述两帧之间的移动。
在位置确定模块25的一种优选实现方式中，
所述位置确定模块25选择直方图滤波器中的一个小区域求其期望，所选择的小区域围绕着后验概率p(x,y|z,m)最大值或次大值，若次大值达到最大值一定比例且其距离滤波器中心更近的话，则选择次大值的位置作为小区域的中心；否则，选择最大值的位置作为小区域的中心。
假设小区域为z，那么最优平面位置
采用上述实施例提供的技术方案，能够根据反射值和高度值分别匹配的情况自适应分配器权重，避免了两者叠加的尺度不一致性，提高了定位精度，增强了定位模块的鲁棒性。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本申请所必须的。
以上是关于方法实施例的介绍，以下通过装置实施例，对本发明所述方案进行进一步说明。
在所述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。所述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
图3示出了适于用来实现本发明实施方式的示例性计算机系统/服务器012的框图。图3显示的计算机系统/服务器012仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图3所示，计算机系统/服务器012以通用计算设备的形式表现。计算机系统/服务器012的组件可以包括但不限于：一个或者多个处理器或者处理单元016，系统存储器028，连接不同系统组件(包括系统存储器028和处理单元016)的总线018。
总线018表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(isa)总线，微通道体系结构(mac)总线，增强型isa总线、视频电子标准协会(vesa)局域总线以及外围组件互连(pci)总线。
计算机系统/服务器012典型地包括多种计算机系统可读介质。这些介质可以是任何能够被计算机系统/服务器012访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。
系统存储器028可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(ram)030和/或高速缓存存储器032。计算机系统/服务器012可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统034可以用于读写不可移动的、非易失性磁介质(图3未显示，通常称为“硬盘驱动器”)。尽管图3中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如cd-rom,dvd-rom或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线018相连。存储器028可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。
具有一组(至少一个)程序模块042的程序/实用工具040，可以存储在例如存储器028中，这样的程序模块042包括——但不限于——操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块042通常执行本发明所描述的实施例中的功能和/或方法。
计算机系统/服务器012也可以与一个或多个外部设备014(例如键盘、指向设备、显示器024等)通信，在本发明中，计算机系统/服务器012与外部雷达设备进行通信，还可与一个或者多个使得用户能与该计算机系统/服务器012交互的设备通信，和/或与使得该计算机系统/服务器012能与一个或多个其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(i/o)接口022进行。并且，计算机系统/服务器012还可以通过网络适配器020与一个或者多个网络(例如局域网(lan)，广域网(wan)和/或公共网络，例如因特网)通信。如图3所示，网络适配器020通过总线018与计算机系统/服务器012的其它模块通信。应当明白，尽管图3中未示出，可以结合计算机系统/服务器012使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、raid系统、磁带驱动器以及数据备份存储系统等。
处理单元016通过运行存储在系统存储器028中的程序，从而执行本发明所描述的实施例中的功能和/或方法。
上述的计算机程序可以设置于计算机存储介质中，即该计算机存储介质被编码有计算机程序，该程序在被一个或多个计算机执行时，使得一个或多个计算机执行本发明上述实施例中所示的方法流程和/或装置操作。
随着时间、技术的发展，介质含义越来越广泛，计算机程序的传播途径不再受限于有形介质，还可以直接从网络下载等。可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括——但不限于——电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括——但不限于——无线、电线、光缆、rf等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如java、smalltalk、c++，还包括常规的过程式程序设计语言—诸如“c”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(lan)或广域网(wan)连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。
- 还没有人留言评论。精彩留言会获得点赞！