CN109211236B - 导航定位方法、装置及机器人 - Google Patents

Abstract

本发明实施例公开了一种导航定位方法、装置及机器人。在第一周期中，在第一时刻扫描第一点云数据，将第一点云数据作为基础点云,在第二时刻扫描第二点云数据，并将第一点云数据和所述第二点云数据进行匹配；在第三时刻获得第一点云数据和第二点云数据的偏差值和第二时刻的位姿数据,获取第二时刻和第三时刻之间所记录的位移偏移量,根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据，将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据,在进入下一周期扫描点云数据时将新的第二点云数据作为基础点云进行匹配，利用位移偏移量补偿平台在进行匹配过程中的位移量，提高定位的准确度。

Description

导航定位方法、装置及机器人

技术领域

本发明涉及机器人即时定位与地图构建领域，特别涉及一种导航定位方法、装置及机器人。

背景技术

室内导航定位一般基于激光或者视觉SLAM(simultaneous localization andmapping)方式。SLAM原理为：获得场景信息，将场景信息以某种形式进行存储形成地图，再次获得信息时，将两组信息数据进行匹配，并把原场景不存在的信息增加到已有地图信息中。室内环境较为平整，采用二维激光数据或者视觉深度信息等进行地图构建，数据量相对较小，计算周期较短，同时考虑到室内平台速度较慢，所以采集到一组数据进行的匹配定位计算时间中，移动平台相对位移较小，可认为计算得到的位姿信息为当前时刻位姿。

室外导航也可采用SLAM的方式，但是由于环境的不确定性，二维数据的方式变得极不稳定。所以室外采用三维点云匹配的方式。(点云：将扫描的数据以点的形式存在，大量数据组成的信息成为点云。三维点云信息可通过(x,y,z)的方式进行存储。)目前点云匹配的方式包括NDT匹配，ICP匹配等，扫描两幅相邻点云信息，通过以上算法可计算出两幅点云的位姿偏移量。描述一个特征值充足的室外环境需要一定数据量的点云信息，进行匹配需要一段时间(十分之几秒到几秒之间)，室外平台运动速度较快，在两组点云进行匹配的时间间隔，平台可能已经移动了一段距离，所以匹配的结果是一段时间以前的位姿信息，无法准确表示平台当前时刻位姿信息。

引入里程计位姿信息，在平台轮系上安装码盘，可计算获得运动轮旋转圈数，从而可计算移动的距离，计算出码盘里程计。该里程计信息由于打滑等原因与实际位姿变化存在一定误差，随着时间积累越大，误差越大。

发明内容

有鉴于此，本发明实施例提供了一种导航定位方法、装置及机器人，在匹配的过程中，使用码盘统计位移偏移量进行辅助定位，获得匹配的结果去修正该时刻之间某个时刻的位姿，减少定位的误差。

第一方面，本发明提供一种导航定位方法，所述方法包括：

在第一周期中，在第一时刻扫描第一点云数据，将所述第一点云数据作为基础点云；

在第二时刻扫描第二点云数据，并将所述第一点云数据和所述第二点云数据进行匹配；

在第三时刻获得所述第一点云数据和所述第二点云数据的偏差值和第二时刻的位姿数据；

获取所述第二时刻和所述第三时刻之间所记录的位移偏移量；

根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据；

将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据；在进入下一周期扫描点云数据时将所述新的第二点云数据作为基础点云进行匹配，以完成对整个场景的位姿修正。

可选地，所述获取所述第二时刻和所述第三时刻之间所记录的位移偏移量，包括：

利用码盘获取所述第二时刻和所述第三时刻之间所记录的位移偏移量。

可选地，所述将所述第一点云数据和所述第二点云数据进行匹配，包括：

采用NDT匹配或ICP匹配对所述第一点云数据和所述第二点云数据进行匹配。

可选地，所述在第一时刻扫描第一点云数据，包括：

在第一时刻扫描第一点云数据，并通过(x,y,z)的方式进行存储。

可选地，所述在第二时刻扫描第二点云数据，包括：

在第二时刻扫描第二点云数据，并通过(x,y,z)的方式进行存储。

第二方面，本发明提供一种导航定位装置，所述装置包括：

扫描单元，用于在第一周期中，在第一时刻扫描第一点云数据，将所述第一点云数据作为基础点云；

所述扫描单元还用于在第二时刻扫描第二点云数据，并将所述第一点云数据和所述第二点云数据进行匹配；

获取单元，用于在第三时刻获得所述第一点云数据和所述第二点云数据的偏差值及第二时刻的位姿数据；

所述获取单元还用于获取所述第二时刻和所述第三时刻之间所记录的位移偏移量；

修正单元，用于根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据；

所述修正单元还用于将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据；

扫描单元还用于在进入下一周期扫描点云数据时将所述新的第二点云数据作为基础点云进行匹配，以完成对整个场景的位姿修正。

可选地，所述扫描单元还用于：

利用码盘获取所述第二时刻和所述第三时刻之间所记录的位移偏移量。

可选地，所述扫描单元还用于：

采用NDT匹配或ICP匹配对所述第一点云数据和所述第二点云数据进行匹配。

可选地，所述扫描单元还用于：

在第一时刻扫描第一点云数据，并通过(x,y,z)的方式进行存储；或

在第二时刻扫描第二点云数据，并通过(x,y,z)的方式进行存储。

第三方面，本发明提供一种机器人，应用于如上述的导航定位方法。

本发明实施例公开了一种导航定位方法、装置及机器人，在第一周期中，在第一时刻扫描第一点云数据，将第一点云数据作为基础点云,在第二时刻扫描第二点云数据，并将第一点云数据和所述第二点云数据进行匹配；在第三时刻获得第一点云数据和第二点云数据的偏差值，根据偏差值获得第二时刻的位姿数据,获取第二时刻和第三时刻之间所记录的位移偏移量,利用第二时刻的位姿数据和位移偏移量确定第三时刻的位姿数据,根据第三时刻的位姿数据修正第二点云数据得到对应第三时刻的位姿数据下的第三点云数据,在进入下一周期扫描点云数据时将第二点云数据作为基础点云进行匹配，利用位移偏移量补偿平台在进行匹配过程中的位移量，提高定位的准确度。

附图说明

图1是本发明实施例中提供的一种导航定位方法的流程图；

图2是本发明实施例中提供的一种导航定位方法的示意图；

图3是本发明实施例中提供的一种导航定位方法时间周期的示意图；

图4是本发明实施例中提供的一种导航定位装置的结构框图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本发明实施例提供了一种导航定位方法、装置及机器人，在匹配的过程中，使用码盘统计位移偏移量进行辅助定位，获得匹配的结果去修正该时刻之间某个时刻的位姿，减少定位的误差。

结合如1所示，本发明提供一种导航定位方法，所述方法包括：

S101、在第一周期中，在第一时刻扫描第一点云数据，将所述第一点云数据作为基础点云。

点云数据为在该时刻位姿下生成的世界坐标系下的点云数据，在第一时刻扫描第一点云数据，并通过(x,y,z)的方式进行存储。

S102、在第二时刻扫描第二点云数据，并将所述第一点云数据和所述第二点云数据进行匹配。

在第二时刻扫描第二点云数据，并通过(x,y,z)的方式进行存储。

采用NDT(Normal Distribution Transform)匹配或ICP(Iterative ClosestPoint，最近点搜索法)匹配对所述第一点云数据和所述第二点云数据进行匹配。

S103、在第三时刻获得所述第一点云数据和所述第二点云数据的偏差值和第二时刻的位姿数据；

S104、获取所述第二时刻和所述第三时刻之间所记录的位移偏移量。

S105、根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据。

S106、将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据。

S107、在进入下一周期扫描点云数据时将所述新的第二点云数据作为基础点云进行匹配，以完成对整个场景的位姿修正。

利用码盘作为里程计获取所述第二时刻和所述第三时刻之间所记录的位移偏移量，在进行匹配的过程中，平台也持续保持运动，当匹配完成计算之后平台已经移动一段距离，所以需要对计算出的位姿数据进行补偿，补偿量就是平台在匹配过程中的位移偏移量，从而可以减少定位的误差，提高定位的准确度。

结合图2所示，针对本发明提供的导航定位方法进行具体介绍：

S1、在0时刻，扫描一副点云数据A，作为基础点云。

S2、在时刻n，扫描一副点云数据B，此时机器人位姿为p，以点云数据A为基础点云，将两幅点云进行匹配。(匹配时间需要t)

S3、在时刻n+t获得点云数据A和点云数据B的偏差值，获得时刻n真实的位姿p1，并同时记录这段时间(t)内码盘的偏移量q，所以当前的位姿为p1+q。同时根据位姿信息修正相应点云信息，修正为实际位姿p1+q下的点云信息。

S4、在时刻n+t+m进入下一周期扫描。以点云数据B为基础点云，重复以上步骤，以完成第二次修正位姿时修正的位姿。

S5、直到最后一次创建地图时，平台不再运动，点云数据匹配计算过程中，平台位姿不会产生增量，即可完成对整个场景的位姿的修正。

该方案需要在每次获得点云信息及里程计信息时，增加时间戳记录。通过时间戳，可计算扫描点云信息到匹配结束后该段时间的位姿变化。其中采集码盘里程计精度较细致。

结合图3所示，采集码盘信息时间周期较短，达到毫秒级，所以根据点云匹配计算过程开始和结束的时间戳，通过时间戳比较确定码盘里程计增量的起始时刻和结束时刻，获得这一段时间内码盘里程计的位姿增量。

结合图4所示，本发明提供一种导航定位装置，所述装置包括：

扫描单元401，用于在第一周期中，在第一时刻扫描第一点云数据，将所述第一点云数据作为基础点云；

所述扫描单元401还用于在第二时刻扫描第二点云数据，并将所述第一点云数据和所述第二点云数据进行匹配；

获取单元402，用于在第三时刻获得所述第一点云数据和所述第二点云数据的偏差值和第二时刻的位姿数据；

所述获取单元402还用于获取所述第二时刻和所述第三时刻之间所记录的位移偏移量；

修正单元403，用于根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据；

所述修正单元403还用于将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据；

扫描单元401还用于在进入下一周期扫描点云数据时将所述新的第二点云数据作为基础点云进行匹配，以完成对整个场景的位姿修正。

可选地，所述扫描单元401还用于：

利用码盘获取所述第二时刻和所述第三时刻之间所记录的位移偏移量。

可选地，所述扫描单元401还用于：

采用NDT匹配或ICP匹配对所述第一点云数据和所述第二点云数据进行匹配。

可选地，所述扫描单元401还用于：

在第一时刻扫描第一点云数据，并通过(x,y,z)的方式进行存储；或

在第二时刻扫描第二点云数据，并通过(x,y,z)的方式进行存储。

对应地，本发明提供一种机器人，应用于如上述的导航定位方法。

通过采用上面的导航定位方法，通过分时的方式，完成了室外三维点云方式的定位和创建地图，对位姿信息进行了精确的修正，符合了实际应用的要求，可以使得机器人在定位过程中可以获得更好的实现自主定位的准确性。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。

以上对本发明所提供的一种导航定位方法、装置及机器人进行了详细介绍，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种导航定位方法，其特征在于，所述方法包括：

在第一周期中，在第一时刻扫描第一点云数据，将所述第一点云数据作为基础点云；

在第二时刻扫描第二点云数据，并将所述第一点云数据和所述第二点云数据进行匹配；

在第三时刻获得所述第一点云数据和所述第二点云数据的偏差值及第二时刻的位姿数据；

获取所述第二时刻和所述第三时刻之间所记录的位移偏移量；

根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据；

将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据；

在进入下一周期扫描点云数据时将所述新的第二点云数据作为基础点云进行匹配，以完成对整个场景的位姿修正；

其中，所述将所述第一点云数据和所述第二点云数据进行匹配，包括：

采用NDT匹配或ICP匹配对所述第一点云数据和所述第二点云数据进行匹配。

2.根据权利要求1所述的方法，其特征在于，所述获取所述第二时刻和所述第三时刻之间所记录的位移偏移量，包括：

利用码盘获取所述第二时刻和所述第三时刻之间所记录的位移偏移量。

3.根据权利要求1所述的方法，其特征在于，所述在第一时刻扫描第一点云数据，包括：

在第一时刻扫描第一点云数据，并通过(x,y,z)的方式进行存储。

4.根据权利要求1所述的方法，其特征在于，所述在第二时刻扫描第二点云数据，包括：

在第二时刻扫描第二点云数据，并通过(x,y,z)的方式进行存储。

5.一种导航定位装置，其特征在于，所述装置包括：

扫描单元，用于在第一周期中，在第一时刻扫描第一点云数据，将所述第一点云数据作为基础点云；

所述扫描单元还用于在第二时刻扫描第二点云数据，并将所述第一点云数据和所述第二点云数据进行匹配；

获取单元，用于在第三时刻获得所述第一点云数据和所述第二点云数据的偏差值及第二时刻的位姿数据；

所述获取单元还用于获取所述第二时刻和所述第三时刻之间所记录的位移偏移量；

修正单元，用于根据所述位移偏移量和所述第二时刻的位姿数据确定位姿修正数据；

所述修正单元还用于将所述位姿修正数据对应的点云数据作为第二时刻下新的第二点云数据；

扫描单元还用于在进入下一周期扫描点云数据时将所述新的第二点云数据作为基础点云进行匹配，以完成对整个场景的位姿修正；

其中，所述扫描单元还用于：采用NDT匹配或ICP匹配对所述第一点云数据和所述第二点云数据进行匹配。

6.根据权利要求5所述的装置，其特征在于，所述扫描单元还用于：

利用码盘获取所述第二时刻和所述第三时刻之间所记录的位移偏移量。

7.根据权利要求5所述的装置，其特征在于，所述扫描单元还用于：

在第一时刻扫描第一点云数据，并通过(x,y,z)的方式进行存储；或

在第二时刻扫描第二点云数据，并通过(x,y,z)的方式进行存储。

8.一种机器人，其特征在于，应用于如权利要求1至4中任一项所述的导航定位方法。

Images