CN110887493B - 基于局部地图匹配的轨迹推算方法、介质、终端和装置 - Google Patents

Abstract

本发明公开一种基于局部地图匹配的轨迹推算方法、介质、终端和装置，通过相邻点云的匹配计算得到相对位姿，以此作为初始变换将当前点云变换至局部地图中，然后通过地图匹配算法校正得到绝对位姿。本发明只用到了激光一种传感器，因此免去了多传感器校正和融合的困难，实施起来较为简单，同时通过本发明方法得到的轨迹比之前根据惯性导航或者轮式里程计得到的轨迹提升了至少10倍的精度，且位姿误差可以控制在0.1％以内，同时单次计算绝对位姿的时间大幅减小，可以控制在20ms内，因此适合部署在机器人主体上进行实时估计。

Description

基于局部地图匹配的轨迹推算方法、介质、终端和装置

【技术领域】

本发明涉及导航定位领域，尤其涉及一种基于局部地图匹配的轨迹推算方法、介质、终端和装置。

【背景技术】

随着电子硬件的进步以及处理器算力的提升，能够实时响应的移动机器人正逐步走进人类的日常生活中。其中，定位能力是机器人实现自主移动的关键。定位是机器人通过感知自身和周围环境信息，经过一定的数据融合处理计算自身位姿的过程。目前机器人定位一般分为相对定位与绝对定位。常用的绝对定位包括全球定位***(GPS)、超声波定位***等。由于机器人的工作区域通常是确定且有限的，因为一般不考虑机器人在地球中的位置，而只需考虑其在工作区域的绝对位置即可。常用的相对定位是指惯性导航或者里程计算法。惯性导航主要使用惯性测量单元(IMU)提供的三轴加速度、三轴陀螺仪或者三轴磁强计。在静止状态下，通过磁强计与加速度计获取自身方向姿态。在运动状态下，通过加速度计获取位置，通过陀螺仪校正姿态。里程计算法主要使用轮上编码器，运动状态下，根据主动轮上编码器的旋转脉冲计数来计算机器人的相对位姿，根据起始位姿可以累积出机器人在工作区域的当前绝对位姿。近年来，相对定位有了可观的进展。通过里程计算法得到机器人的位姿已经不限于轮上编码器的使用。不同于根据自身信息推算自身的位姿，根据环境信息，机器人一样可以推算出自身相对位姿信息。比如，利用相邻激光点云或者视觉图像的相对位移反推出机器人的相对位移。相邻点云或者图像的相对位移信息可以通过匹配计算得到。根据起始位姿，通过累积计算机器人同样可以得到自身在工作区域的绝对位姿。

在绝对定位中，全局定位***(GPS)无法用在室内，并且更新频率较低；超声波定位***虽具有低成本、小型化和易于连接的特点，但其不能在长距离下使用，同时需要在场景中大面积布置相应装置，再加上信号干扰的问题使得其无法满足机器人的定位要求。在相对定位中，位姿的计算需要对惯性测量单元(IMU)的数据进行一次或二次积分，再加上惯性测量单元(IMU)本身的非线性温漂，其估计值缺少稳定性与准确性。而基于轮上编码器的里程计算法，受限于机器人自身震动或地面因素导致的打滑，并且其位姿推算具有累积误差，因此也不太准确。而基于点云或者图像匹配的里程计算法，其位姿估计的精度受限于匹配效果以及激光或者视觉对环境感知的固有偏差，并且匹配是有一定的重叠范围限制的，因此其估计结果也存在较大误差。

【发明内容】

本发明提供了一种基于局部地图匹配的轨迹推算方法、介质、终端和装置，解决了以上所述的技术问题。

本发明解决上述技术问题的技术方案如下：一种基于局部地图匹配的轨迹推算方法，包括以下步骤：

步骤1，将起始位置处获取的初始帧点云作为参考点云；

步骤2，并将所述参考点云变换为栅格地图；

步骤3，获取与参考点云相邻的第二帧点云，并作为当前点云；

步骤4，对当前点云与参考点云进行匹配，生成当前点云相对于参考点云的第一相对位姿；

步骤5，累积所述第一相对位姿生成第二帧点云的绝对位姿，并在第二帧点云的绝对位姿将所述第二帧点云添加到栅格地图；

步骤6，获取与第二帧点云相邻的第三帧点云，对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿，并在第二帧点云的绝对位姿将所述第三帧点云添加到当前栅格地图；

步骤7，提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿；

步骤8，判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云，且继续获取点云数据并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。

在一个优选实施方式中，采用迭代最近点算法ICP生成当前点云相对于参考点云的第一相对位姿以及第三帧点云相对于第二帧点云的第二相对位姿，所述迭代最近点算法ICP的迭代次数设定为2-5次，初始变换设定为(0，0，0)。

在一个优选实施方式中，所述预设距离为10-20米。

在一个优选实施方式中，采用轮式差分驱动模型累积所述第一相对位姿生成所述第二帧点云的绝对位姿。

本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的基于局部地图匹配的轨迹推算方法。

本发明实施例的第三方面提供了一种基于局部地图匹配的轨迹推算终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述基于局部地图匹配的轨迹推算方法的步骤。

本发明实施例的第四方面提供了一种基于局部地图匹配的轨迹推算装置，包括点云获取模块、第一栅格地图变换模块、相对位姿生成模块、第一绝对位姿生成模块、第二栅格地图变换模块、第二绝对位姿生成模块和判断模块，

所述点云获取模块用于获取点云数据；

所述第一栅格地图变换模块用于将初始帧点云作为参考点云，并将所述参考点云变换为栅格地图；

所述相对位姿生成模块用于对当前点云与参考点云进行匹配生成当前点云相对于参考点云的第一相对位姿；以及用于对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿；

所述第一绝对位姿生成模块用于累积所述第一相对位姿生成所述第二帧点云的绝对位姿；

所述第二栅格地图变换模块用于在第二帧点云的绝对位姿将第二帧点云和第三帧点云添加到栅格地图；

所述第二绝对位姿生成模块用于提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿；

所述判断模块用于判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云，且继续获取点云数据并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。

在一个优选实施方式中，采用迭代最近点算法ICP生成当前点云相对于参考点云的第一相对位姿以及第三帧点云相对于第二帧点云的第二相对位姿，所述迭代最近点算法ICP的迭代次数设定为2-5次，初始变换设定为(0，0，0)。

在一个优选实施方式中，所述预设距离为10-20米。

在一个优选实施方式中，所述第一绝对位姿生成模块具体用于采用轮式差分驱动模型累积所述第一相对位姿生成所述第二帧点云的绝对位姿。

本发明提出了一种简单有效且稳定准确的计算相对位姿以及绝对位姿的方法，通过相邻点云的匹配计算得到相对位姿，以此作为初始变换将当前点云变换至局部地图中，然后通过地图匹配算法校正得到绝对位姿。本发明只用到了激光一种传感器，因此免去了多传感器校正和融合的困难，实施起来较为简单，同时通过本发明方法得到的轨迹比之前根据惯性导航或者轮式里程计得到的轨迹提升了至少10倍的精度，且位姿误差可以控制在0.1％以内，同时单次计算绝对位姿的时间大幅减小，可以控制在20ms内，因此适合部署在机器人主体上进行实时估计。

为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是实施例1提供的基于局部地图匹配的轨迹推算方法的流程示意图；

图2是实施例2提供的基于局部地图匹配的轨迹推算装置的结构示意图；

图3是实施例3提供的基于局部地图匹配的轨迹推算终端的结构示意图；

图4是移动机器人在90*60平方米场景中的移动轨迹；

图5是移动机器人在90*60平方米场景中所创建的栅格地图。

【具体实施方式】

为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。

图1是本发明实施例1提供的一种基于局部地图匹配的轨迹推算方法的流程示意图，如图1所示，方法包括以下步骤：

步骤1，将起始位置处获取的初始帧点云作为参考点云；

步骤2，获取此时点云的绝对位姿以及栅格地图的预设大小，将所述参考点云变换为栅格地图。

步骤3，获取与初始帧点云相邻的第二帧点云作为当前点云。

步骤4，使用迭代最近点算法ICP对当前点云与参考点云进行匹配，生成当前点云相对于参考点云的第一相对位姿，此时需要给定初始变换，本实施例给定的初始变换是(0，0，0)。同时考虑到所生成的第一相对位姿在之后的地图匹配算法中只作为初始变换使用，因此只需要该迭代最近点算法的一个比较粗略的结果即可，所以为了提升计算效率，这里设定迭代次数为2-5次，优选为3次。当然在其他实施例中也可以采用其他迭代算法生成当前点云相对于参考点云的第一相对位姿，具体计算过程在此不进行详细说明，但是均在本申请的保护范围以内。

步骤5，采用轮式差分驱动模型累积所述第一相对位姿生成第二帧点云的绝对位姿，并在第二帧点云的绝对位姿将所述第二帧点云添加到栅格地图。在其他实施例中也可以采用其他机器人运动学模型累积所述第一相对位姿生成第二帧点云的绝对位姿。优选实施例中，根据栅格地图更新法则，比如栅格占用概率模型在当前的绝对位姿处将当前点云添加进栅格地图中。

步骤6，获取与第二帧点云相邻的第三帧点云，对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿，并在第二帧点云的绝对位姿将所述第三帧点云添加到当前栅格地图。

步骤7，提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿。在地图匹配算法中，主要是基于栅格化的点云数据与当前栅格地图进行占用栅格搜索并计算得分，根据得分逐步调整位姿。本实施例中，此处设定占用栅格搜索范围为当前栅格化点云周围5层栅格内，以保证计算效率和精度。

步骤8，判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云(即新的第二帧点云)且继续获取点云数据作为新的第三帧点云，然后并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。优选实施例中，所述预设距离为10-20米，比如设定为15米。如图4和图5所示，为上述优选实施例中，移动机器人在90*60平方米场景中的移动轨迹和所创建的栅格地图。

上述优选实施例提出了一种简单有效且稳定准确的计算相对位姿以及绝对位姿的方法，通过相邻点云的匹配计算得到相对位姿，以此作为初始变换将当前点云变换至局部地图中，然后通过地图匹配算法校正得到绝对位姿。本发明只用到了激光一种传感器，因此免去了多传感器校正和融合的困难，实施起来较为简单，同时通过本发明方法得到的轨迹比之前根据惯性导航或者轮式里程计得到的轨迹提升了至少10倍的精度，且位姿误差可以控制在0.1％以内，同时单次计算绝对位姿的时间大幅减小，可以控制在20ms内，因此适合部署在机器人主体上进行实时估计。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的基于局部地图匹配的轨迹推算方法。

图2是本发明实施例2提供的一种基于局部地图匹配的轨迹推算装置的结构示意图，如图2所示，包括点云获取模块100、第一栅格地图变换模块200、相对位姿生成模块300、第一绝对位姿生成模块400、第二栅格地图变换模块500、第二绝对位姿生成模块600和判断模块700，

所述点云获取模块100用于获取点云数据；

所述第一栅格地图变换模块200用于将初始帧点云作为参考点云，并将所述参考点云变换为栅格地图；

所述相对位姿生成模块300用于对当前点云与参考点云进行匹配生成当前点云相对于参考点云的第一相对位姿；以及用于对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿；

所述第一绝对位姿生成模块400用于累积所述第一相对位姿生成所述第二帧点云的绝对位姿；

所述第二栅格地图变换模块500用于在第二帧点云的绝对位姿将第二帧点云和第三帧点云添加到栅格地图；

所述第二绝对位姿生成模块600用于提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿；

所述判断模块700用于判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云，且继续获取点云数据并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。

在一个优选实施方式中，所述相对位姿生成模块300采用迭代最近点算法ICP生成当前点云相对于参考点云的第一相对位姿以及第三帧点云相对于第二帧点云的第二相对位姿，所述迭代最近点算法ICP的迭代次数设定为2-5次，初始变换设定为(0，0，0)。

在一个优选实施方式中，所述预设距离为10-20米。

在一个优选实施方式中，所述第一绝对位姿生成模块400具体用于采用轮式差分驱动模型累积所述第一相对位姿生成所述第二帧点云的绝对位姿。

本发明实施例还提供了一种基于局部地图匹配的轨迹推算终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述基于局部地图匹配的轨迹推算方法的步骤。图3是本发明实施例3提供的基于局部地图匹配的轨迹推算终端的结构示意图，如图3所示，该实施例的基于局部地图匹配的轨迹推算终端8包括：处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤，例如图1所示的步骤1至步骤8。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能，例如图2所示模块100至700的功能。

示例性的，所述计算机程序82可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述可读存储介质81中，并由所述处理器80执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述基于局部地图匹配的轨迹推算终端8中的执行过程。

所述基于局部地图匹配的轨迹推算终端8可包括，但不仅限于，处理器80、可读存储介质81。本领域技术人员可以理解，图3仅仅是基于局部地图匹配的轨迹推算终端8的示例，并不构成对基于局部地图匹配的轨迹推算终端8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述基于局部地图匹配的轨迹推算终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。

所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述可读存储介质81可以是所述基于局部地图匹配的轨迹推算终端8的内部存储单元，例如基于局部地图匹配的轨迹推算终端8的硬盘或内存。所述可读存储介质81也可以是所述基于局部地图匹配的轨迹推算终端8的外部存储设备，例如所述基于局部地图匹配的轨迹推算终端8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质81还可以既包括所述基于局部地图匹配的轨迹推算终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述基于局部地图匹配的轨迹推算终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述***中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

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

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

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

本发明并不仅仅限于说明书和实施方式中所描述，因此对于熟悉领域的人员而言可容易地实现另外的优点和修改，故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下，本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。

Claims (10)

1.一种基于局部地图匹配的轨迹推算方法，其特征在于，包括以下步骤：

步骤1，将起始位置处获取的初始帧点云作为参考点云；

步骤2，并将所述参考点云变换为栅格地图；

步骤3，获取与参考点云相邻的第二帧点云，并作为当前点云；

步骤4，对当前点云与参考点云进行匹配，生成当前点云相对于参考点云的第一相对位姿；

步骤5，累积所述第一相对位姿生成第二帧点云的绝对位姿，并在第二帧点云的绝对位姿将所述第二帧点云添加到栅格地图；

步骤6，获取与第二帧点云相邻的第三帧点云，对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿，并在第二帧点云的绝对位姿将所述第三帧点云添加到当前栅格地图；

步骤7，提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿；

步骤8，判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云，且继续获取点云数据并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。

2.根据权利要求1所述的基于局部地图匹配的轨迹推算方法，其特征在于，采用迭代最近点算法ICP生成当前点云相对于参考点云的第一相对位姿以及第三帧点云相对于第二帧点云的第二相对位姿，所述迭代最近点算法ICP的迭代次数设定为2-5次，初始变换设定为(0，0，0)。

3.根据权利要求1或2所述的基于局部地图匹配的轨迹推算方法，其特征在于，所述预设距离为10-20米。

4.根据权利要求3所述的基于局部地图匹配的轨迹推算方法，其特征在于，采用轮式差分驱动模型累积所述第一相对位姿生成所述第二帧点云的绝对位姿。

5.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1-4任一项所述的基于局部地图匹配的轨迹推算方法。

6.一种基于局部地图匹配的轨迹推算终端，其特征在于，包括权利要求5所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-4任一项所述基于局部地图匹配的轨迹推算方法的步骤。

7.一种基于局部地图匹配的轨迹推算装置，实现权利要求1—4任一所述的机器人轨迹推算方法，其特征在于，包括点云获取模块、第一栅格地图变换模块、相对位姿生成模块、第一绝对位姿生成模块、第二栅格地图变换模块、第二绝对位姿生成模块和判断模块，

所述点云获取模块用于获取点云数据；

所述第一栅格地图变换模块用于将初始帧点云作为参考点云，并将所述参考点云变换为栅格地图；

所述相对位姿生成模块用于对当前点云与参考点云进行匹配生成当前点云相对于参考点云的第一相对位姿；以及用于对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿；

所述第一绝对位姿生成模块用于累积所述第一相对位姿生成所述第二帧点云的绝对位姿；

所述第二栅格地图变换模块用于在第二帧点云的绝对位姿将第二帧点云和第三帧点云添加到栅格地图；

所述第二绝对位姿生成模块用于提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿；

所述判断模块用于判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云，且继续获取点云数据并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。

8.根据权利要求7所述的基于局部地图匹配的轨迹推算装置，其特征在于，采用迭代最近点算法ICP生成当前点云相对于参考点云的第一相对位姿以及第三帧点云相对于第二帧点云的第二相对位姿，所述迭代最近点算法ICP的迭代次数设定为2-5次，初始变换设定为(0，0，0)。

9.根据权利要求7或8所述的基于局部地图匹配的轨迹推算装置，其特征在于，所述预设距离为10-20米。

10.根据权利要求9所述的基于局部地图匹配的轨迹推算装置，其特征在于，所述第一绝对位姿生成模块具体用于采用轮式差分驱动模型累积所述第一相对位姿生成所述第二帧点云的绝对位姿。

Images