一种机器人的扫图方法、装置、存储介质和机器人

技术领域

本发明涉及机器人领域，尤其涉及一种机器人的扫图方法、装置、存储介质和机器人。

背景技术

随着机器人行业的快速发展，各种服务机器人层出不穷，机器人在我们的生活、工作中也应用得越来越广泛。目前，机器人构建地图的方法有很多，SLAM(SimultaneousLocalization And Mapping，即时定位与地图构建)为最常用的方法，其通常是指在机器人上，通过对各种传感器数据进行采集和计算，生成对其自身位置姿态的定位和场景地图信息的系统。SLAM本质是个系统状态估计问题，即：在给定系统输入的条件下，估计出机器人的位姿和地图点的坐标，因此延伸出基于贝叶斯滤波以及基于图优化的两类SLAM算法。

基于贝叶斯滤波的方法是一种贝叶斯迭代状态估计理论，即先对机器人运动进行建模，构造出贴合物理场景的运动方程和观测方程，然后进行状态预测和测量更新。由于滤波类算法是基于递归计算的，下一时刻的估计值依赖于上一时刻的估计，因此在大尺度场景下，由于系统参数和传感器观测数据的不准确性，会造成误差慢慢累积，一旦当前时刻的估计出现偏差，无法修正该误差，最终将无法获得一致性的地图。而基于图优化的SLAM分为前端和后端两个模块：前端先从里程计获得初始位姿，然后通过激光雷达数据连续帧匹配，求得激光约束后的位姿；后端采用回环检测，构造闭环约束，通过最小化观测和估计残差求得优化后的位姿。在前端中会产生由估计所引入的误差，随着运动范围的扩大，该误差会逐渐累积，导致最后产生错误的结果。为解决该问题，在后端使用闭环检测进行全局优化，通过判断机器人是否回到历史中的某一点，添加约束较强的闭环约束，从而消除误差累积。

现有技术的图优化方法包括Cartographer方法，其主要采用UKF滤波器对激光雷达(LIDAR)、惯性导航测量单元(IMU)和里程计(Odometry)等传感器数据进行融合，然后形成子地图并采用回环检测方法生成分辨率为5cm的栅格地图，虽然累计误差相对较小，但是计算速度和计算精度均难以满足机器人，尤其是在楼宇间行使递送等业务的服务机器人对扫图定位精度和速度的需求。

发明内容

本发明提供了一种机器人的扫图方法、装置、存储介质和机器人，解决了现有技术的地图创建方法难以满足精度和速度需求的技术问题。

本发明解决上述技术问题的技术方案如下：一种机器人的扫图方法，基于cartographer算法，包括以下步骤：

步骤1，获取机器人的当前里程计数据、当前惯性测量单元数据、第一实时激光扫描数据以及前端扫描匹配器输出的局部优化位姿；

步骤2，对所述当前里程计数据、所述当前惯性测量单元数据以及所述局部优化位姿进行融合，生成融合结果，以根据所述融合结果生成机器人在任意时刻的预测位姿和预测运动速度；

步骤3，根据对应时刻的目标预测位姿和目标预测运动速度对所述第一实时激光扫描数据的运动畸变进行校正，生成第二实时激光扫描数据；

步骤4，将所述融合结果作为所述前端扫描匹配器的初始位姿，并基于所述第二实时激光扫描数据构建子地图；

步骤5，获取机器人的当前激光扫描数据，采用分支定界方法对所述子地图进行回环检测，并根据回环检测结果对全部子地图进行优化；

步骤6，将优化后的子地图拼接为大地图。

在一个优选实施方式中，采用扩展卡尔曼滤波方法对所述当前里程计数据、所述当前惯性测量单元数据以及前端扫描匹配器输出的所述局部优化位姿进行融合，具体包括：

采用惯性滤波器对所述当前里程计数据和所述当前惯性测量单元数据滤波后生成机器人的初始位置、初始速度和初始姿态；

将所述局部优化位姿与所述初始位置、所述初始姿态进行融合，并根据融合结果提取控制点的初始坐标；

将所述控制点的预设实际坐标与所述初始坐标求差，生成坐标残差；

将所述坐标残差通过扩展卡尔曼滤波计算，生成惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量以及初始姿态的误差修正量，并将所述惯性测量单元的误差反馈到所述惯性滤波器中；

根据所述初始位置的误差修正量、所述初始速度的误差修正量和所述初始姿态的误差修正量，对所述初始位置、所述初始速度和所述初始姿态进行修正，生成对应的融合结果。

在一个优选实施方式中，所述基于第二实时激光扫描数据构建子地图包括：

利用相关性扫描匹配方法将所述第二实时激光扫描数据在当前子地图上进行扫描匹配；

在扫描匹配过程中，采用高斯牛顿迭代方法生成所述第二实时激光扫描数据在所述子地图的目标位置，以使所述第二实时激光扫描数据中所有激光点在当前子地图的概率之和最大；

将所述第二实时激光扫描数据插入到所述目标位置，并对所述子地图进行更新。

在一个优选实施方式中，所述根据对应时刻的目标预测位姿和目标预测运动速度对第一实时激光扫描数据的运动畸变进行校正包括：

接收第一实时激光扫描数据；

根据对应时刻的目标预测位姿和目标预测运动速度，并基于预设插值方法求解所述第一实时激光扫描数据中每个激光点对应的机器人位姿；

根据所述每个激光点对应的机器人位姿将所述第一实时激光扫描数据中的所有激光点转换到同一坐标系下，并重新封装形成运动畸变校正后的第二实时激光扫描数据。

本发明实施例的第二方面提供了一种机器人的扫图装置，包括获取模块、预测模块、校正模块、子地图构建模块、回环模块和拼接模块，

所述获取模块用于获取机器人的当前里程计数据、当前惯性测量单元数据、第一实时激光扫描数据以及前端扫描匹配器输出的局部优化位姿；

所述预测模块用于对所述当前里程计数据、所述当前惯性测量单元数据以及所述局部优化位姿进行融合，生成融合结果，以根据所述融合结果生成机器人在任意时刻的预测位姿和预测运动速度；

所述校正模块用于根据对应时刻的目标预测位姿和目标预测运动速度对所述第一实时激光扫描数据的运动畸变进行校正，生成第二实时激光扫描数据；

所述子地图构建模块用于将所述融合结果作为所述前端扫描匹配器的初始位姿，并基于所述第二实时激光扫描数据构建子地图；

所述回环模块用于获取机器人的当前激光扫描数据，采用分支定界方法对所述子地图进行回环检测，并根据回环检测结果对全部子地图进行优化；

所述拼接模块用于将优化后的子地图拼接为大地图。

在一个优选实施方式中，所述预测模块包括融合单元，所述融合单元具体包括：

滤波单元，用于采用惯性滤波器对所述当前里程计数据和所述当前惯性测量单元数据滤波后生成机器人的初始位置、初始速度和初始姿态；

坐标提取单元，用于将所述局部优化位姿与所述初始位置、所述初始姿态进行融合，并根据融合结果提取控制点的初始坐标；

第一计算单元，用于将所述控制点的预设实际坐标与所述初始坐标求差，得到坐标残差；

第二计算单元，用于将所述坐标残差通过扩展卡尔曼滤波计算，生成惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量以及初始姿态的误差修正量，并将所述惯性测量单元的误差反馈到所述惯性滤波器中；

修正单元，用于根据所述初始位置的误差修正量、所述初始速度的误差修正量和所述初始姿态的误差修正量，对所述初始位置、所述初始速度和所述初始姿态进行修正，生成对应的融合结果。

在一个优选实施方式中，所述子地图构建模块具体包括：

扫描匹配单元，用于利用相关性扫描匹配方法将所述第二实时激光扫描数据在当前子地图上进行扫描匹配；

解算单元，用于在扫描匹配过程中，采用高斯牛顿迭代方法生成所述第二实时激光扫描数据在所述子地图的目标位置，以使所述第二实时激光扫描数据中所有激光点在当前子地图的概率之和最大；

更新单元，用于将所述第二实时激光扫描数据插入到所述目标位置，并对所述子地图进行更新。

在一个优选实施方式中，所述校正模块具体包括：

接收单元，用于接收第一实时激光扫描数据；

插值单元，用于根据对应时刻的目标预测位姿和目标预测运动速度，并基于预设插值方法求解所述第一实时激光扫描数据中每个激光点对应的机器人位姿；

转换单元，用于根据所述每个激光点对应的机器人位姿将所述第一实时激光扫描数据中的所有激光点转换到同一坐标系下，并重新封装形成运动畸变校正后的第二实时激光扫描数据。

本发明实施例的第三方面提供了一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以上所述机器人的扫图方法的步骤。

本发明实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现以上所述机器人的扫图方法的步骤。

本发明提供了一种机器人的扫图方法、装置、存储介质和机器人，不仅采用扩展卡尔曼滤波方法对里程计数据、惯性测量单元数据以及激光扫描数据进行融合，而且采用插值方法消除了激光扫描数据中的运动畸变，同时对用于更新子地图的高斯牛顿迭代过程进行优化，从而大幅提高了服务机器人创建环境地图的效率和精度，提高了机器人的服务质量和用户满意度。

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

附图说明

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

图1是实施例1提供的机器人的扫图方法的流程示意图；

图2是实施例2提供的机器人的扫图装置的结构示意图；

图3是实施例3提供的一种控制器的电路结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，如果不冲突，本发明实施例中的各个特征可以相互结合，均在本发明的保护范围之内。另外，虽然在装置示意图中进行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于装置中的模块划分，或流程图中的顺序执行所示出或描述的步骤。再者，本发明所采用的“第一”、“第二”、“第三”等字样并不对数据和执行次序进行限定，仅是对功能和作用基本相同的相同项或相似项进行区分。

本发明实施例的机器人可以被构造成任何合适形状，以便实现特定业务功能操作，例如，本发明实施例机器人可以为递送机器人、搬运机器人、看护机器人等等。

所述机器人一般包括壳体、传感器单元、驱动轮部件、存储部件及控制器。壳体的外形大体上呈圆形，在一些实施例中，壳体的外形可以大体上呈椭圆形、三角形、D形、柱形或其他形状。

传感器单元用于采集机器人的一些运动参数及环境空间各类数据。在一些实施例中，传感器单元包括激光雷达,激光雷达安装于壳体上方，其安装高度高于所述壳体的顶部面壳高度，激光雷达用于检测机器人与障碍物之间的障碍物距离。在一些实施例中，传感器单元还可以包括惯性测量单元(Inertialmeasurementunit，IMU)、里程计、陀螺仪、磁场计、加速度计或速度计、光学摄像头等等。

驱动轮部件安装于壳体并驱动机器人在各种空间上移动，在一些实施例中，驱动轮部件包括左驱动轮、右驱动轮及全向轮，左驱动轮和右驱动轮分别安装于壳体的相对两侧。左驱动轮和右驱动轮被配置为至少部分可伸出及缩回壳体的底部。全向轮安装于壳体的底部的靠前位置，全向轮为活动脚轮，可以水平360度旋转，以使得机器人可以灵活转向。左驱动轮、右驱动轮及全向轮的安装构成三角形，以提高机器人行走的平稳性。当然，在一些实施例中，驱动轮部件还可以采用其他结构，比如全向轮可被省略，只留左驱动轮与右驱动轮亦可以驱动机器人正常行走。

在一些实施例中，机器人还配置有存储部件，存储部件安装于收容槽内，从而完成递送任务等等。

控制器分别与左驱动轮、右驱动轮、全向轮及激光雷达电连接。控制器作为机器人的控制核心，用于控制机器人行走、后退以及一些业务逻辑处理。

在一些实施例中，控制器可以为通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机、AR(AcornRISCMachine)或其它可编程逻辑器件、分立门或晶体管逻辑、分立的硬件组件或者这些部件的任何组合。还有，控制器还可以是任何传统处理器、控制器、微控制器或状态机。控制器也可以被实现为计算设备的组合，例如，DSP和微处理器的组合、多个微处理器、一个或多个微处理器结合DSP和/或任何其它这种配置。

在一些实施例中，上述机器人在活动的过程中，控制器采用SLAM(simultaneouslocalizationandmapping，即时定位与建图技术)技术，根据环境数据构建地图和定位，从而移动到目标位置完成递送任务、清洁任务等。控制器基于被建立好的地图以及机器人的位置，通过全覆盖路径规划算法指示机器人完全遍历一个环境空间。

可以理解的是，控制器还可以根据多种方式识别已遍历位置或区域，或者，识别障碍物，从而作出满足产品需求的控制策略。

请参阅图1，为本发明实施例1提供的一种机器人的扫图方法的流程示意图，基于cartographer算法。Cartographer算法是一个实时的室内建图算法,能生成分辨率r＝5cm的栅格地图，其包括前端和后端，在前端扫描匹配器将最新的激光雷达扫描数据在相邻的子图上(整个地图的一小块)完成扫描匹配,得到一个在短时间内准确的最佳插入位置(局部优化位姿)后,将该扫描数据插入到子图中。扫描匹配中,位姿估计的误差会在整个地图中随时间逐渐累积，因此可以在后端中,通过回环检测加约束进行优化消除误差。具体如图1所示，该机器人的扫图方法包括以下步骤：

步骤1，获取机器人的当前里程计数据、当前惯性测量单元数据、第一实时激光扫描数据以及前端扫描匹配器输出的局部优化位姿。

步骤2，对所述当前里程计数据、所述当前惯性测量单元数据以及所述局部优化位姿进行融合，生成融合结果，以根据所述融合结果生成机器人在任意时刻的预测位姿和预测运动速度。

通常来说，惯性测量单元数据和里程计数据的采样频率为100Hz(10ms一帧)，而激光的采样频率为40～18Hz(25～60ms一帧)，因此激光雷达的采样频率较低，不能满足创建子地图的需求。为了获得任意时刻的定位和速度信息，本发明通过EKF(扩展卡尔曼滤波)，对惯性测量单元数据、里程计数据以及前端扫描匹配器输出的局部优化位姿进行了融合，以获得任意时刻机器人的位置和速度信息，从而为子地图构建提供了定位先验。优选实施例中，通过扩展卡尔曼滤波对里程计数据、惯性测量单元数据以及局部优化位姿进行融合，具体包括以下步骤：

S201，采用惯性滤波器对所述当前里程计数据和所述当前惯性测量单元数据滤波后生成机器人的初始位置、初始速度和初始姿态；

S202，将所述局部优化位姿与所述初始位置、所述初始姿态进行融合，并根据融合结果提取控制点的初始坐标；

S203，将所述控制点的预设实际坐标与所述初始坐标求差，生成坐标残差；

S204，将所述坐标残差通过扩展卡尔曼滤波计算，生成惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量以及初始姿态的误差修正量，并将所述惯性测量单元的误差反馈到所述惯性滤波器中；

S205，根据所述初始位置的误差修正量、所述初始速度的误差修正量和所述初始姿态的误差修正量，对所述初始位置、所述初始速度和所述初始姿态进行修正，生成对应的融合结果。

上述优选实施例构建了统一并融合当前里程计数据、当前惯性测量单元数据以及局部优化位姿的扩展卡尔曼滤波模型，不仅可以高效且精确地对机器人任意时刻的位置和速度进行预测，而且计算效率高，可以在嵌入式平台运行，从而保证子地图构建和更新的速度和精度，同时降低了机器人的成本。

更加优选实施例中，该机器人具有可拆卸模块，在未来的应用中，可将可拆卸模块(比如仓盒)设计为扫图专用的扫图模块，通过集成多种定位传感器，如三维激光雷达、双目摄像头、rgbd传感器等，来辅助建图，适用于更大的场景，而每一种传感器在扫图过程中，都能提供独立的定位输出。为了不改变建图核心算法同时可以兼容未来的多种传感器，在扫图算法的前端使用了扩展卡尔曼算法，将多种定位输出，并根据各个传感器的置信度进行融合，输出最为可信的融合定位。

然后执行步骤3，根据对应时刻的目标预测位姿和目标预测运动速度对所述第一实时激光扫描数据的运动畸变进行校正，生成第二实时激光扫描数据。

为了降低服务类机器人成本，其通常安装扫描频率低、采样点稀疏的激光雷达。激光雷达通常分成固定部分和旋转部分，固定部分固定在机器人或其他移动平台上；旋转部分则是利用旋转测量单元从而实现光路对环境的360°扫描，以获得整个平面环境的点云数据。当激光雷达固定部分相对于被测物体处于静止的情况下，仅有激光雷达旋转部分的旋转运动而无其他相对运动时，其给出的平面扫描点云数据接近于真实情况，误差仅为激光雷达本身测量误差。而当激光雷达处于移动状态下时，尤其是在机器人运动速度较快时，其扫描周期内的扫描运动将不仅仅包含激光雷达旋转部分的旋转运动，还有其相对被测物体因机器人所产生的平移及旋转运动，这个就是运动畸变。

优选实施例中，根据以上扩展卡尔曼滤波模型可以对机器人任意时刻的位置和速度进行预测，然后根据对应时刻的目标预测位姿和目标预测运动速度对第一实时激光扫描数据的运动畸变进行校正，具体包括：

S301，接收第一实时激光扫描数据；

S302，根据对应时刻的目标预测位姿和目标预测运动速度，并基于预设插值方法，比如线性插值方法或者二次插值法求解所述第一实时激光扫描数据中每个激光点对应的机器人位姿；

S303，根据所述每个激光点对应的机器人位姿将所述第一实时激光扫描数据中的所有激光点转换到同一坐标系下，并重新封装形成运动畸变校正后的第二实时激光扫描数据。从而获得每一束激光的真实扫描角度，当采用第二实时激光扫描数据构建子地图时，进一步提高了子地图构建的精度。

然后执行步骤4，将所述融合结果作为所述前端扫描匹配器的初始位姿，并基于所述第二实时激光扫描数据构建子地图。

所述第二实时激光扫描数据包含一个起点和许多的终点。起点称为origin，终点称为scan points，可以用H表示点云集，其表达形式如下：

H＝{h

当第二实时激光扫描数据需要插入到子地图中时，第二实时激光扫描数据中{h

而子地图由一定数量的激光扫描(laserscan)组成，具体由5cm*5cm大小的概率栅格[p

S401，利用相关性扫描匹配方法(即Ceres Scan Matching方法)将所述第二实时激光扫描数据在当前子地图上进行扫描匹配；

S402，在扫描匹配过程中，采用高斯牛顿迭代方法生成所述第二实时激光扫描数据在所述子地图的目标位置，以使所述第二实时激光扫描数据中所有激光点在当前子地图的概率之和最大；

S403，将所述第二实时激光扫描数据插入到所述目标位置，并对所述子地图进行更新。

具体来说，第二实时激光扫描数据需要插入到子地图中最优的位置，使第二实时激光扫描数据中激光点束的位姿经过转换后落子地图中时，每个点的信度之和最高。通过扫描匹配方法对T

其中M

同时，子地图更新过程中，根据当前帧的匹配得分以及相对上一相邻帧的移动量，筛选出关键帧，并将该关键帧输出作为局部优化位姿与里程计数据、惯性测量单元数据进行融合。

然后执行步骤5，获取机器人的当前激光扫描数据，采用分支定界方法对所述子地图进行回环检测，并根据回环检测结果对全部子地图进行优化；

步骤6，将优化后的子地图拼接为大地图。

基于需要创建地图的室内场景较大，虽然子地图在短时间内的准确度是可靠的，但长时间会存在累积误差，为了消除累积误差，需要通过回环检测来优化所有子地图的位姿。对于回环检测，如果机器人的当前激光扫描数据和所有已创建完成的子地图中的某帧激光扫描数据的位姿在距离上足够近，那么通过分支定界方法就可以找到该闭环(即该闭环的匹配足够好)，完成闭环检测。然后根据当前激光扫描数据的位姿和匹配到得最接近的子地图中的某一个位姿来对所有的子地图中的位姿进行优化，即使残差E最小，最后完成优化过程，并对所有子地图进行拼接形成大地图。

本实施例的机器人的扫图方法，不仅采用扩展卡尔曼滤波方法对里程计数据、惯性测量单元数据以及激光扫描数据进行融合，而且采用插值方法消除了激光扫描数据中的运动畸变，同时对用于更新子地图的高斯牛顿迭代过程进行优化，从而大幅提高了服务机器人创建环境地图的效率和精度，提高了机器人的服务质量和用户满意度。

需要说明的是，在上述各个实施例中，上述各步骤之间并不必然存在一定的先后顺序，本领域普通技术人员，根据本发明实施例的描述可以理解，不同实施例中，上述各步骤可以有不同的执行顺序，亦即，可以并行执行，亦可以交换执行等等。

作为本发明实施例的另一方面，本发明实施例还提供一种机器人的扫图装置。其中，机器人的扫图装置可以为软件模块，所述软件模块包括若干指令，其存储在存储器内，处理器可以访问该存储器，调用指令进行执行，以完成上述各个实施例所阐述的机器人的扫图方法。

在一些实施例中，机器人的扫图装置亦可以由硬件器件搭建成的，例如，机器人的扫图装置可以由一个或两个以上的芯片搭建而成，各个芯片可以互相协调工作，以完成上述各个实施例所阐述的机器人的扫图方法。再例如，机器人的扫图装置还可以由各类逻辑器件搭建而成，诸如由通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机、ARM(AcornRISCMachine)或其它可编程逻辑器件、分立门或晶体管逻辑、分立的硬件组件或者这些部件的任何组合而搭建成。

图2是本发明实施例2提供一种机器人的扫图装置的结构示意图，该机器人的扫图装置包括获取模块100、预测模块200、校正模块300、子地图构建模块400、回环模块500和拼接模块600，

所述获取模块100用于获取机器人的当前里程计数据、当前惯性测量单元数据、第一实时激光扫描数据以及前端扫描匹配器输出的局部优化位姿；

所述预测模块200用于对所述当前里程计数据、所述当前惯性测量单元数据以及所述局部优化位姿进行融合，生成融合结果，以根据所述融合结果生成机器人在任意时刻的预测位姿和预测运动速度；

所述校正模块300用于根据对应时刻的目标预测位姿和目标预测运动速度对所述第一实时激光扫描数据的运动畸变进行校正，生成第二实时激光扫描数据；

所述子地图构建模块400用于将所述融合结果作为所述前端扫描匹配器的初始位姿，并基于所述第二实时激光扫描数据构建子地图；

所述回环模块500用于获取机器人的当前激光扫描数据，采用分支定界方法对所述子地图进行回环检测，并根据回环检测结果对全部子地图进行优化；

所述拼接模块600用于将优化后的子地图拼接为大地图。

在一个优选实施方式中，所述预测模块200包括融合单元，所述融合单元具体包括：

滤波单元，用于采用惯性滤波器对所述当前里程计数据和所述当前惯性测量单元数据滤波后生成机器人的初始位置、初始速度和初始姿态；

坐标提取单元，用于将所述局部优化位姿与所述初始位置、所述初始姿态进行融合，并根据融合结果提取控制点的初始坐标；

第一计算单元，用于将所述控制点的预设实际坐标与所述初始坐标求差，得到坐标残差；

第二计算单元，用于将所述坐标残差通过扩展卡尔曼滤波计算，生成惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量以及初始姿态的误差修正量，并将所述惯性测量单元的误差反馈到所述惯性滤波器中；

修正单元，用于根据所述初始位置的误差修正量、所述初始速度的误差修正量和所述初始姿态的误差修正量，对所述初始位置、所述初始速度和所述初始姿态进行修正，生成对应的融合结果。

在一个优选实施方式中，所述子地图构建模块400具体包括：

扫描匹配单元，用于利用相关性扫描匹配方法将所述第二实时激光扫描数据在当前子地图上进行扫描匹配；

解算单元，用于在扫描匹配过程中，采用高斯牛顿迭代方法生成所述第二实时激光扫描数据在所述子地图的目标位置，以使所述第二实时激光扫描数据中所有激光点在当前子地图的概率之和最大；

更新单元，用于将所述第二实时激光扫描数据插入到所述目标位置，并对所述子地图进行更新。

在一个优选实施方式中，所述校正模块300具体包括：

接收单元，用于接收第一实时激光扫描数据；

插值单元，用于根据对应时刻的目标预测位姿和目标预测运动速度，并基于预设插值方法求解所述第一实时激光扫描数据中每个激光点对应的机器人位姿；

转换单元，用于根据所述每个激光点对应的机器人位姿将所述第一实时激光扫描数据中的所有激光点转换到同一坐标系下，并重新封装形成运动畸变校正后的第二实时激光扫描数据。

需要说明的是，上述机器人的扫图装置可执行本发明实施例所提供的机器人的扫图方法，具备执行方法相应的功能模块和有益效果。未在机器人的扫图装置实施例中详尽描述的技术细节，可参见本发明实施例所提供的机器人的扫图方法。

图3是本发明实施例提供的一种控制器的电路结构示意图。如图3所示，该控制器600包括一个或多个处理器61以及存储器62。其中，图3中以一个处理器61为例。

处理器61和存储器62可以通过总线或者其他方式连接，图3中以通过总线连接为例。

存储器62作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本发明实施例中的机器人的扫图方法对应的程序指令/模块。处理器61通过运行存储在存储器62中的非易失性软件程序、指令以及模块，从而执行机器人的扫图装置的各种功能应用以及数据处理，即实现上述方法实施例提供的机器人的扫图方法以及上述装置实施例的各个模块或单元的功能。

存储器62可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器62可选包括相对于处理器61远程设置的存储器，这些远程存储器可以通过网络连接至处理器61。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述程序指令/模块存储在所述存储器62中，当被所述一个或者多个处理器61执行时，执行上述任意方法实施例中的机器人的扫图方法。

本发明实施例还提供了一种非易失性计算机存储介质，所述计算机存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，例如图3中的一个处理器61，可使得上述一个或多个处理器可执行上述任意方法实施例中的机器人的扫图方法。

本发明实施例还提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被电子设备执行时，使所述电子设备执行任一项所述的机器人的扫图方法。

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

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。