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

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

Track calculation method, medium, terminal and device based on local map matching

【技术实现步骤摘要】
基于局部地图匹配的轨迹推算方法、介质、终端和装置
本专利技术涉及导航定位领域，尤其涉及一种基于局部地图匹配的轨迹推算方法、介质、终端和装置。
技术介绍
随着电子硬件的进步以及处理器算力的提升，能够实时响应的移动机器人正逐步走进人类的日常生活中。其中，定位能力是机器人实现自主移动的关键。定位是机器人通过感知自身和周围环境信息，经过一定的数据融合处理计算自身位姿的过程。目前机器人定位一般分为相对定位与绝对定位。常用的绝对定位包括全球定位系统(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％以内，同时单次计算绝对位姿的时间大幅减本文档来自技高网...

【技术保护点】
1.一种基于局部地图匹配的轨迹推算方法，其特征在于，包括以下步骤：/n步骤1，将起始位置处获取的初始帧点云作为参考点云；/n步骤2，并将所述参考点云变换为栅格地图；/n步骤3，获取与参考点云相邻的第二帧点云，并作为当前点云；/n步骤4，对当前点云与参考点云进行匹配，生成当前点云相对于参考点云的第一相对位姿；/n步骤5，累积所述第一相对位姿生成第二帧点云的绝对位姿，并在第二帧点云的绝对位姿将所述第二帧点云添加到栅格地图；/n步骤6，获取与第二帧点云相邻的第三帧点云，对第三帧点云与第二帧点云进行匹配生成第三帧点云相对于第二帧点云的第二相对位姿，并在第二帧点云的绝对位姿将所述第三帧点云添加到当前栅格地图；/n步骤7，提取所述第二相对位姿的角度，并基于地图匹配算法和当前栅格地图生成第三帧点云的绝对位姿；/n步骤8，判断第三帧点云的绝对位姿与起始位置的距离是否大于预设距离，若否，则将第三帧点云更新为当前点云，且继续获取点云数据并重复步骤4-步骤8推算机器人轨迹；若是，则删除当前栅格地图，并将第三帧点云的绝对位姿作为起始位置，将第三帧点云作为参考点云，重复步骤2-步骤8，重新建立栅格地图并推算机器人轨迹。/n...

【技术特征摘要】
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所述的计算机可读存储介质和处理器，所述处理器...

【专利技术属性】
技术研发人员：蔡龙生，李国飞，
申请(专利权)人：上海有个机器人有限公司，
类型：发明
国别省市：上海;31