一种基于点云地图的定位方法和装置与流程

1.本发明涉及计算机技术领域，尤其涉及一种基于点云地图的定位方法和装置。


背景技术：

2.在实现本发明过程中，发明人发现现有技术中至少存在如下问题：
3.目前，在自动驾驶的应用领域，对于点云地图中高度对称场景其事物识别精确度存在很大问题。例如，机器人处在道路中间位置，距离道路两侧距离接近，同时两侧建筑形状接近，此时将点云绕垂直方向旋转150
°
，得到的点云得分很可能接近正确位姿对应的得分，从而无法进行精确的点云匹配，即无法准确变换机器人的位姿。其中，在逆向工程中通过测量仪器得到的产品外观表面的点数据集合也称之为点云。


技术实现要素：

4.有鉴于此，本发明实施例提供一种基于点云地图的定位方法和装置，能够解决现有基于点云地图的定位精确度低的问题。
5.为实现上述目的，根据本发明实施例的一个方面，提供了一种基于点云地图的定位方法，包括获取当前点云数据，遍历当前点云数据中的每个点，计算每个点预设方向的对称点；确定与每个对称点距离最近的点，以基于对称点与最近点之间的距离，计算点云数据中每个点的权重；调用预设的匹配模型，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿。
6.可选地，遍历当前点云中每个点之前，包括：
7.获取当前点云数据，将当前点云数据存储至预设的kd-tree中；
8.确定与每个对称点距离最近的点，包括：
9.将每个对称点在kd-tree中进行检索，获得与每个对称点距离最近的点。
10.可选地，基于对称点与最近点之间的距离，计算点云数据中每个点的权重，包括：
11.如果对称点与最近点之间的距离小于或等于预设的距离阈值，则点云数据中对应点的权重为对称点与最近点之间的距离的一半；或者
12.如果对称点与最近点之间的距离大于预设的距离阈值，则点云数据中对应点的权重为预设的权重阈值。
13.可选地，计算每个点预设方向的对称点，包括：
14.计算每个点的前进方向和平移方向的对称点。
15.可选地，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿，包括：
16.根据当前点云数据以及每个点对应的权重，基于预设的相关性扫描匹配算法计算得到当前点云在地图中的相对位姿。
17.可选地，基于预设的相关性扫描匹配算法计算得到当前点云在地图中的相对位姿，包括：
18.根据预设分辨率对地图进行栅格化，以基于高斯分布算法对每个栅格中的点云进行建模，得到所述地图每个栅格对应的高斯分布模型；
19.将当前点云按照预设的角度分辨率和距离分辨率进行变换，得到不同位姿的候选点云，进而将每个候选点云投影到栅格化的地图中；
20.根据候选点云投影到的栅格中的高斯分布模型和候选点云中每个点的权重，得到所述候选点云的得分，进而将得分最高的候选点云作为当前点云在地图中变换后的相对位姿。
21.可选地，将当前点云按照预设的角度分辨率和距离分辨率进行变换，得到不同位姿的候选点云，包括：
22.根据预设的角度分辨率旋转所述当前点云，得到具有不同方向的点云，然后再根据预设的距离分辨率分别平移不同方向的点云，进而得到不同位姿的候选点云。
23.另外，本发明还提供了一种基于点云地图的定位装置，包括获取模块，用于获取当前点云数据，遍历当前点云数据中的每个点，计算每个点预设方向的对称点；处理模块，用于确定与每个对称点距离最近的点，以基于对称点与最近点之间的距离，计算点云数据中每个点的权重；调用预设的匹配模型，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿。
24.上述发明中的一个实施例具有如下优点或有益效果：本发明通过对点云数据的对称点计算，并且通过对称点得到相应的最近点，进而计算点云数据中每个点的权重，再根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿，从而提升在对称场景下的鲁棒性，实现了精准的点云匹配，达到准确识别事物，调整自动驾驶设备位姿的技术效果。
25.上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
26.附图用于更好地理解本发明，不构成对本发明的不当限定。其中：
27.图1是根据本发明第一实施例的基于点云地图的定位方法的主要流程的示意图；
28.图2是根据本发明第二实施例的基于点云地图的定位方法的主要流程的示意图；
29.图3是根据本发明实施例的基于点云地图的定位装置的主要模块的示意图；
30.图4是本发明实施例可以应用于其中的示例性系统架构图；
31.图5是适于用来实现本发明实施例的终端设备或服务器的计算机系统的结构示意图。
具体实施方式
32.以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
33.图1是根据本发明第一实施例的基于点云地图的定位方法的主要流程的示意图，
如图1所示，所述基于点云地图的定位方法包括：
34.步骤s101，获取当前点云数据，遍历当前点云数据中的每个点，计算每个点预设方向的对称点。
35.在一些实施例中，本发明为了能够更为快速和精准的确定点云数据中每个点的对称点，可以在遍历当前点云中每个点之前，将获取的当前点云数据存储至预设的kd-tree中，即将每个点依次存储至kd-tree的结构中。其中，kd-tree是k-dimensional树的简称，是一种分割k维数据空间的数据结构，可应用于多维空间关键数据的搜索。
36.作为较佳的实施例，计算每个点的前进方向和平移方向的对称点。具体地，点坐标为(x,y,z),前进方向的对称点为(x,-y,z),平移方向的对称点为(-x,y,z)。例如：点云数据中点(5，2，1)的前进方向对称点为(5，-2，1)以及平移方向对称点为(-5，2，1)。需要说明的是，由于机器人一般在平面移动，因此不考虑垂直方向的对称点。
37.步骤s102，确定与每个对称点距离最近的点，以基于对称点与最近点之间的距离，计算点云数据中每个点的权重。
38.在一些实施例中，为了快速和精准的确定与每个对称点距离最近的点，本发明将每个对称点在kd-tree中进行检索，获得与每个对称点距离最近的点。
39.在另一些实施例中，基于对称点与最近点之间的距离，计算点云数据中每个点的权重的时候，可以预先配置距离阈值，判断对称点与最近点之间的距离是否小于或等于该距离阈值，若是则点云数据中对应点的权重为对称点与最近点之间的距离的一半；若否则点云数据中对应点的权重为预设的权重阈值。
40.例如，判断对称点与最近点之间的距离是否小于或等于2，若是则点云数据中对应点的权重为对称点与最近点之间的距离的一半；若否则点云数据中对应点的权重为1。也就是说，计算点云数据中每个点的权重公式：
[0041][0042]
其中，weight为权重，x为对称点与最近点之间的距离。
[0043]
步骤s103，调用预设的匹配模型，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿。
[0044]
在一些实施例中，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿的时候，可以将当前点云数据以及每个点对应的权重输入至预设的相关性扫描匹配算法(correlation scan match，简称csm)中，计算得到当前点云在地图中的相对位姿。其中，所述的相关性扫描匹配算法(csm)是一种和初始值不敏感且不容易陷入局部最优解的匹配算法，通过分支界定法的加速可以达到较好的实时性以及匹配精度。
[0045]
进一步地，将当前点云数据以及每个点对应的权重输入至预设的相关性扫描匹配算法中，计算得到当前点云在地图中的相对位姿，包括：
[0046]
将已知的高精度点云地图按照预设的分辨率进行栅格化，进而将点云地图中的各个点投射到对应的栅格中以得到栅格地图。当所有点云地图中的点投射完之后，遍历各个栅格，根据各个栅格的点云分布情况，使用高斯分布算法对该栅格中的点云进行建模。
[0047]
较佳的，由于栅格是通过(x，y)进行的划分，因此只需要对z方向即高度进行高斯分布建模，具体地：计算栅格中点云z轴上的均值和协方差，以得到该栅格的一元高斯分布
模型。
[0048]
将当前通过多线激光扫描得到的点云按照预设的角度分辨率和距离分辨率进行变换，得到不同位姿的候选点云，进而将每个候选点云投影到建好的栅格地图中。较佳的，将当前通过多线激光扫描得到的点云按照预设的角度分辨率和距离分辨率进行变换可以为施加一个刚体变换，具体地：根据预设的角度分辨率旋转所述当前点云，得到具有不同方向的点云，然后再根据预设的距离分辨率分别平移不同方向的点云，进而得到不同位姿的候选点云。例如：角度分辨率是2
°
，那么就需要分别按照旋转2
°
，4
°
，6
°…
360
°
进行旋转，距离分辨率同理。
[0049]
根据候选点云投影到的栅格中的高斯分布模型和该候选点云中每个点的权重(其中，该候选点云中每个点的权重与当前点云中对应的点权重一致。)，得到该候选点云的得分。最后，选出得分最高的候选点云，作为当前点云在地图中变换后的相对位姿。较佳的，根据候选点云投影到的栅格中的高斯分布模型和该候选点云中每个点的权重计算候选点云得分时，可以将候选点云中每个点的高度分别输入至对应栅格的高斯分布模型中，以计算该候选点云中每个点的概率值，进而计算每个点概率值与权重的乘积再求和得到该候选点云的得分，获取得分最高的候选点云。
[0050]
图2是根据本发明第二实施例的基于点云地图的定位方法的主要流程的示意图，如图2所示，所述基于点云地图的定位方法包括：
[0051]
步骤s201，获取当前点云数据，将当前点云数据存储至预设的kd-tree中。
[0052]
步骤s202，计算每个点的前进方向和平移方向的对称点。
[0053]
步骤s203，将每个对称点在kd-tree中进行检索，获得与每个对称点距离最近的点。
[0054]
步骤s204，基于对称点与最近点之间的距离，计算点云数据中每个点的权重。
[0055]
在一些实施例中，如果对称点与最近点之间的距离小于或等于预设的距离阈值，则点云数据中对应点的权重为对称点与最近点之间的距离的一半。或者如果对称点与最近点之间的距离大于预设的距离阈值，则点云数据中对应点的权重为预设的权重阈值。
[0056]
步骤s205，根据当前点云数据以及每个点对应的权重，基于预设的相关性扫描匹配算法计算得到当前点云在地图中的相对位姿。
[0057]
图3是根据本发明实施例的基于点云地图的定位装置的主要模块的示意图，如图3所示，所述基于点云地图的定位装置300包括获取模块301和处理模块302。其中，获取模块301获取当前点云数据，遍历当前点云数据中的每个点，计算每个点预设方向的对称点；处理模块302确定与每个对称点距离最近的点，以基于对称点与最近点之间的距离，计算点云数据中每个点的权重；调用预设的匹配模型，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿。
[0058]
在一些实施例中，所述获取模块301遍历当前点云中每个点之前，包括：获取当前点云数据，将当前点云数据存储至预设的kd-tree中；
[0059]
所述处理模块302确定与每个对称点距离最近的点，包括：将每个对称点在kd-tree中进行检索，获得与每个对称点距离最近的点。
[0060]
在一些实施例中，所述处理模块302基于对称点与最近点之间的距离，计算点云数据中每个点的权重，包括：
[0061]
如果对称点与最近点之间的距离小于或等于预设的距离阈值，则点云数据中对应点的权重为对称点与最近点之间的距离的一半；或者
[0062]
如果对称点与最近点之间的距离大于预设的距离阈值，则点云数据中对应点的权重为预设的权重阈值。
[0063]
在一些实施例中，所述获取模块301计算每个点预设方向的对称点，包括：计算每个点的前进方向和平移方向的对称点。
[0064]
在一些实施例中，所述处理模块302根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿，包括：
[0065]
根据当前点云数据以及每个点对应的权重，基于预设的相关性扫描匹配算法计算得到当前点云在地图中的相对位姿。
[0066]
在一些实施例中，所述处理模块302基于预设的相关性扫描匹配算法计算得到当前点云在地图中的相对位姿，包括：
[0067]
根据预设分辨率对地图进行栅格化，以基于高斯分布算法对每个栅格中的点云进行建模，得到所述地图每个栅格对应的高斯分布模型；
[0068]
将当前点云按照预设的角度分辨率和距离分辨率进行变换，得到不同位姿的候选点云，进而将每个候选点云投影到栅格化的地图中；
[0069]
根据候选点云投影到的栅格中的高斯分布模型和候选点云中每个点的权重，得到所述候选点云的得分，进而将得分最高的候选点云作为当前点云在地图中变换后的相对位姿。
[0070]
在一些实施例中，所述处理模块302将当前点云按照预设的角度分辨率和距离分辨率进行变换，得到不同位姿的候选点云，包括：
[0071]
根据预设的角度分辨率旋转所述当前点云，得到具有不同方向的点云，然后再根据预设的距离分辨率分别平移不同方向的点云，进而得到不同位姿的候选点云。
[0072]
需要说明的是，在本发明所述基于点云地图的定位方法和所述基于点云地图的定位装置在具体实施内容上具有相应关系，故重复内容不再说明。
[0073]
图4示出了可以应用本发明实施例的基于点云地图的定位方法或基于点云地图的定位装置的示例性系统架构400。
[0074]
如图4所示，系统架构400可以包括终端设备401、402、403，网络404和服务器405。网络404用以在终端设备401、402、403和服务器405之间提供通信链路的介质。网络404可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
[0075]
用户可以使用终端设备401、402、403通过网络404与服务器405交互，以接收或发送消息等。终端设备401、402、403上可以安装有各种通讯客户端应用，例如购物类应用、网页浏览器应用、搜索类应用、即时通信工具、邮箱客户端、社交平台软件等(仅为示例)。
[0076]
终端设备401、402、403可以是具有基于点云地图的定位屏并且支持网页浏览的各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。
[0077]
需要说明的是，本发明实施例所提供的基于点云地图的定位方法一般由服务器405执行，相应地，计算装置一般设置于服务器405中。
[0078]
应该理解，图4中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。
[0079]
下面参考图5，其示出了适于用来实现本发明实施例的终端设备的计算机系统500的结构示意图。图5示出的终端设备仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
[0080]
如图5所示，计算机系统500包括中央处理单元(cpu)501，其可以根据存储在只读存储器(rom)502中的程序或者从存储部分408加载到随机访问存储器(ram)503中的程序而执行各种适当的动作和处理。在ram503中，还存储有计算机系统500操作所需的各种程序和数据。cpu501、rom502以及ram503通过总线504彼此相连。输入/输出(i/o)接口505也连接至总线504。
[0081]
以下部件连接至i/o接口505：包括键盘、鼠标等的输入部分506；包括诸如阴极射线管(crt)、液晶基于点云地图的定位器(lcd)等以及扬声器等的输出部分507；包括硬盘等的存储部分508；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分509。通信部分509经由诸如因特网的网络执行通信处理。驱动器510也根据需要连接至i/o接口505。可拆卸介质511，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器510上，以便于从其上读出的计算机程序根据需要被安装入存储部分508。
[0082]
特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分509从网络上被下载和安装，和/或从可拆卸介质511被安装。在该计算机程序被中央处理单元(cpu)501执行时，执行本发明的系统中限定的上述功能。
[0083]
需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、rf等等，或者上述的任意合适的组合。
[0084]
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所
标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0085]
描述于本发明实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中，例如，可以描述为：一种处理器包括获取模块和处理模块。其中，这些模块的名称在某种情况下并不构成对该模块本身的限定。
[0086]
作为另一方面，本发明还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该设备执行时，使得该设备包括获取当前点云数据，遍历当前点云数据中的每个点，计算每个点预设方向的对称点；确定与每个对称点距离最近的点，以基于对称点与最近点之间的距离，计算点云数据中每个点的权重；调用预设的匹配模型，根据当前点云数据以及每个点对应的权重计算得到当前点云在地图中的相对位姿。
[0087]
根据本发明实施例的技术方案，能够解决现有基于点云地图的定位精确度低的问题。
[0088]
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。