三维点云地图构建方法、装置、电子设备及存储介质

摘要

本申请属于机器人定位导航技术领域，公开了一种三维点云地图构建方法、装置、电子设备及存储介质，通过使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图；以移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图；从而可高效地获取高精度的环境地图。

权利要求

1.一种三维点云地图构建方法，应用于移动机器人以获取环境地图，其特征在于，所述移动机器人包括移动机器人本体、设置在所述移动机器人本体上的机械臂以及设置在所述机械臂末端的感知模块；

所述三维点云地图构建方法包括步骤：

A1.使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图；

A2.以所述移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个所述停止位置对应的所述第一位姿之间的第一位姿变化量和相邻两个所述停止位置对应的所述局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；

A3.对所述第一因子图进行图优化处理，得到所述移动机器人本体在各停止位置处的第一优化位姿；

A4.根据所述第一优化位姿，对各所述局部点云地图进行拼接，得到全局的环境地图；

步骤A4包括：

根据第一优化位姿，对各局部点云地图进行坐标变换处理，以把各局部点云地图中的云点相对于移动机器人本体的位置数据转换为相对于世界坐标系的位置数据；具体地，根据以下公式进行转换处理： ， 为云点相对于世界坐标系的位置数据，为第一优化位姿；

把坐标变换处理后的各局部点云地图合并在一起，形成全局的环境地图。

2.根据权利要求1所述的三维点云地图构建方法，其特征在于，步骤A1包括：A101.使移动机器人分步移动；

A102.在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块移动以采集多帧点云数据；

A103.针对同一停止位置，以各采集时刻的所述感知模块的第二位姿为节点，以相邻两个所述采集时刻的所述点云数据之间的第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿之间的第二位姿变化量为约束因子，生成第二因子图；

A104.对所述第二因子图进行图优化处理，得到各所述采集时刻的所述感知模块的第二优化位姿；

A105.根据所述第二优化位姿，对各帧所述点云数据进行拼接，得到当前停止位置的所述局部点云地图。

3.根据权利要求2所述的三维点云地图构建方法，其特征在于，步骤A103包括针对同一停止位置执行的步骤：对相邻两个所述采集时刻的所述点云数据进行点云匹配，以获取相邻两个所述采集时刻的所述点云数据之间的所述第二位姿变换关系；

获取各所述采集时刻的所述机械臂末端的第三位姿，以计算各所述采集时刻的所述感知模块的所述第二位姿；

根据各所述采集时刻的所述第二位姿，计算相邻两个所述采集时刻的所述第二位姿之间的第二位姿变化量；

以各所述采集时刻的所述第二位姿为节点，以相邻两个所述采集时刻的所述第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿变化量为约束因子，生成所述第二因子图。

4.根据权利要求3所述的三维点云地图构建方法，其特征在于，步骤A102包括：采集至少两帧包含同一标志物的云点的点云数据；

所述以各所述采集时刻的所述第二位姿为节点，以相邻两个所述采集时刻的所述第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿变化量为约束因子，生成所述第二因子图的步骤包括：以采集到所述标志物的云点的所述采集时刻为特征时刻，在相邻两个所述特征时刻对应的所述第二位姿之间添加第二闭环约束因子；所述第二闭环约束因子为相邻两个所述特征时刻对应的感知模块位姿之间的第四位姿变换关系。

5.根据权利要求1所述的三维点云地图构建方法，其特征在于，步骤A2包括：A201.通过对IMU数据积分获取相邻两个所述停止位置对应的所述第一位姿之间的所述第一位姿变化量；

A202.根据所述第一位姿变化量获取所述移动机器人本体在各所述停止位置处的第一位姿；

A203.对相邻两个所述停止位置的所述局部点云地图进行点云匹配，以获取相邻两个所述停止位置对应的所述局部点云地图之间的所述第一位姿变换关系；

A204.以各所述停止位置的所述第一位姿为节点，以相邻两个所述停止位置的所述第一位姿变换关系和相邻两个所述停止位置的所述第一位姿变化量为约束因子，生成所述第一因子图。

6.根据权利要求5所述的三维点云地图构建方法，其特征在于，步骤A1包括：生成至少两个包含同一标志物的云点的局部点云地图；

步骤A204包括：

以包含所述标志物的云点的所述局部点云地图对应的所述停止位置为特征位置，在相邻两个所述特征位置对应的所述第一位姿之间添加第一闭环约束因子；所述第一闭环约束因子为相邻两个所述停止位置对应的移动机器人位姿之间的第三位姿变换关系。

7.一种三维点云地图构建装置，应用于移动机器人以获取环境地图，其特征在于，所述移动机器人包括移动机器人本体、设置在所述移动机器人本体上的机械臂以及设置在所述机械臂末端的感知模块；

所述三维点云地图构建装置包括：

局部地图生成模块，用于使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图；

第一因子图生成模块，用于以所述移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个所述停止位置对应的所述第一位姿之间的第一位姿变化量和相邻两个所述停止位置对应的所述局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；

第一优化模块，用于对所述第一因子图进行图优化处理，得到所述移动机器人本体在各停止位置处的第一优化位姿；

第一拼接模块，用于根据所述第一优化位姿，对各所述局部点云地图进行拼接，得到全局的环境地图；

第一拼接模块在根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图的时候，执行：根据第一优化位姿，对各局部点云地图进行坐标变换处理，以把各局部点云地图中的云点相对于移动机器人本体的位置数据转换为相对于世界坐标系的位置数据；具体地，根据以下公式进行转换处理： ， 为云点相对于世界坐标系的位置数据，为第一优化位姿；

把坐标变换处理后的各局部点云地图合并在一起，形成全局的环境地图。

8.根据权利要求7所述的三维点云地图构建装置，其特征在于，所述局部地图生成模块用于使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图，具体包括：使移动机器人分步移动；

在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块移动以采集多帧点云数据；

针对同一停止位置，以各采集时刻的所述感知模块的第二位姿为节点，以相邻两个所述采集时刻的所述点云数据之间的第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿之间的第二位姿变化量为约束因子，生成第二因子图；

对所述第二因子图进行图优化处理，得到各所述采集时刻的所述感知模块的第二优化位姿；

根据所述第二优化位姿，对各帧所述点云数据进行拼接，得到当前停止位置的所述局部点云地图。

9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如权利要求1‑6任一项所述三维点云地图构建方法中的步骤。

10.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1‑6任一项所述三维点云地图构建方法中的步骤。

说明书

三维点云地图构建方法、装置、电子设备及存储介质

技术领域

[0001] 本申请涉及机器人定位导航技术领域，具体而言，涉及一种三维点云地图构建方法、装置、电子设备及存储介质。

背景技术

[0002] 传统的SLAM（simultaneous localization and mapping，即时定位与地图构建）技术是由无人车搭载激光雷达等感知模块，对环境信息进行扫描，构建环境三维点云信息，以确定自身位姿状态。

[0003] 一般地，使用感知模块帧间匹配、轮式里程计、IMU里程计或GPS信息等构建约束因子关系，生成位姿图，优化每一时刻机器人位姿状态，从而更加精确进行建图。但是，在室内环境中， GPS信息无法提供，只能依赖激光雷达、轮式里程计、IMU等进行位姿估计，这些传感器随着时间推移会产生累积误差，导致定位不准确，在室外环境中，虽然有GPS信息，但其频率较低、存在一定延迟，也会影响定位精度。

[0004] 此外，一般的感知模块是直接固定在无人车上的，在进行建图时，需要无人车不断调整车身位姿，效率较低。

发明内容

[0005] 本申请的目的在于提供一种三维点云地图构建方法、装置、电子设备及存储介质，可高效地获取高精度的环境地图。

[0006] 第一方面，本申请提供了一种三维点云地图构建方法，应用于移动机器人以获取环境地图，所述移动机器人包括移动机器人本体、设置在所述移动机器人本体上的机械臂以及设置在所述机械臂末端的感知模块；

[0007] 所述三维点云地图构建方法包括步骤：

[0008] A1.使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图；

[0009] A2.以所述移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个所述停止位置对应的所述第一位姿之间的第一位姿变化量和相邻两个所述停止位置对应的所述局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；

[0010] A3.对所述第一因子图进行图优化处理，得到所述移动机器人本体在各停止位置处的第一优化位姿；

[0011] A4.根据所述第一优化位姿，对各所述局部点云地图进行拼接，得到全局的环境地图。

[0012] 该三维点云地图构建方法，在移动机器人每次移动后保持移动机器人本体不动，由机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图，可得到精度较高的局部点云地图，进而，通过图优化方法得到移动机器人本体在不同停止位置处的精确的位姿（即第一优化位姿），并依此对各局部点云地图进行拼接得到全局的环境地图，该全局的环境地图的精度高；且与通过调节移动机器人本体来获取不同方向的点云数据相比，通过机械臂带动感知模块移动来获取不同方向的点云数据，效率更高。

[0013] 优选地，步骤A1包括：

[0014] A101.使移动机器人分步移动；

[0015] A102.在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块移动以采集多帧点云数据；

[0016] A103.针对同一停止位置，以各采集时刻的所述感知模块的第二位姿为节点，以相邻两个所述采集时刻的所述点云数据之间的第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿之间的第二位姿变化量为约束因子，生成第二因子图；

[0017] A104.对所述第二因子图进行图优化处理，得到各所述采集时刻的所述感知模块的第二优化位姿；

[0018] A105.根据所述第二优化位姿，对各帧所述点云数据进行拼接，得到当前停止位置的所述局部点云地图。

[0019] 通过图优化方法对感知模块的位姿进行优化，可得到更加准确的感知模块的位姿，从而保证拼接后的局部点云地图具有较高的精度。

[0020] 优选地，步骤A103包括针对同一停止位置执行的步骤：

[0021] 对相邻两个所述采集时刻的所述点云数据进行点云匹配，以获取相邻两个所述采集时刻的所述点云数据之间的所述第二位姿变换关系；

[0022] 获取各所述采集时刻的所述机械臂末端的第三位姿，以计算各所述采集时刻的所述感知模块的所述第二位姿；

[0023] 根据各所述采集时刻的所述第二位姿，计算相邻两个所述采集时刻的所述第二位姿之间的第二位姿变化量；

[0024] 以各所述采集时刻的所述第二位姿为节点，以相邻两个所述采集时刻的所述第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿变化量为约束因子，生成所述第二因子图。

[0025] 优选地，步骤A102包括：采集至少两帧包含同一标志物的云点的点云数据；

[0026] 所述以各所述采集时刻的所述第二位姿为节点，以相邻两个所述采集时刻的所述第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿变化量为约束因子，生成所述第二因子图的步骤包括：

[0027] 以采集到所述标志物的云点的所述采集时刻为特征时刻，在相邻两个所述特征时刻对应的所述第二位姿之间添加第二闭环约束因子；所述第二闭环约束因子为相邻两个所述特征时刻对应的感知模块位姿之间的第四位姿变换关系。

[0028] 通过加入第二闭环约束因子，可减小积累误差，从而使局部点云地图的准确性更高。

[0029] 优选地，步骤A2包括：

[0030] A201.通过对IMU数据积分获取相邻两个所述停止位置对应的所述第一位姿之间的所述第一位姿变化量；

[0031] A202.根据所述第一位姿变化量获取所述移动机器人本体在各所述停止位置处的第一位姿；

[0032] A203.对相邻两个所述停止位置的所述局部点云地图进行点云匹配，以获取相邻两个所述停止位置对应的所述局部点云地图之间的所述第一位姿变换关系；

[0033] A204.以各所述停止位置的所述第一位姿为节点，以相邻两个所述停止位置的所述第一位姿变换关系和相邻两个所述停止位置的所述第一位姿变化量为约束因子，生成所述第一因子图。

[0034] 优选地，步骤A1包括：生成至少两个包含同一标志物的云点的局部点云地图；

[0035] 步骤A204包括：

[0036] 以包含所述标志物的云点的所述局部点云地图对应的所述停止位置为特征位置，在相邻两个所述特征位置对应的所述第一位姿之间添加第一闭环约束因子；所述第一闭环约束因子为相邻两个所述停止位置对应的移动机器人位姿之间的第三位姿变换关系。

[0037] 第二方面，本申请提供了一种三维点云地图构建装置，应用于移动机器人以获取环境地图，所述移动机器人包括移动机器人本体、设置在所述移动机器人本体上的机械臂以及设置在所述机械臂末端的感知模块；

[0038] 所述三维点云地图构建装置包括：

[0039] 局部地图生成模块，用于使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图；

[0040] 第一因子图生成模块，用于以所述移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个所述停止位置对应的所述第一位姿之间的第一位姿变化量和相邻两个所述停止位置对应的所述局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；

[0041] 第一优化模块，用于对所述第一因子图进行图优化处理，得到所述移动机器人本体在各停止位置处的第一优化位姿；

[0042] 第一拼接模块，用于根据所述第一优化位姿，对各所述局部点云地图进行拼接，得到全局的环境地图。

[0043] 该三维点云地图构建装置，在移动机器人每次移动后保持移动机器人本体不动，由机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图，可得到精度较高的局部点云地图，进而，通过图优化方法得到移动机器人本体在不同停止位置处的精确的位姿（即第一优化位姿），并依此对各局部点云地图进行拼接得到全局的环境地图，该全局的环境地图的精度高；且与通过调节移动机器人本体来获取不同方向的点云数据相比，通过机械臂带动感知模块移动来获取不同方向的点云数据，效率更高。

[0044] 优选地，所述局部地图生成模块用于使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图，具体包括：

[0045] 使移动机器人分步移动；

[0046] 在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块移动以采集多帧点云数据；

[0047] 针对同一停止位置，以各采集时刻的所述感知模块的第二位姿为节点，以相邻两个所述采集时刻的所述点云数据之间的第二位姿变换关系和相邻两个所述采集时刻的所述第二位姿之间的第二位姿变化量为约束因子，生成第二因子图；

[0048] 对所述第二因子图进行图优化处理，得到各所述采集时刻的所述感知模块的第二优化位姿；

[0049] 根据所述第二优化位姿，对各帧所述点云数据进行拼接，得到当前停止位置的所述局部点云地图。

[0050] 第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述三维点云地图构建方法中的步骤。

[0051] 第四方面，本申请提供了一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述三维点云地图构建方法中的步骤。

[0052] 有益效果：

[0053] 本申请提供的三维点云地图构建方法、装置、电子设备及存储介质，通过使移动机器人分步移动，在每次移动后保持所述移动机器人本体的位姿不变，并通过所述机械臂带动所述感知模块在不同位姿采集多帧点云数据以生成局部点云地图；以所述移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个所述停止位置对应的所述第一位姿之间的第一位姿变化量和相邻两个所述停止位置对应的所述局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；对所述第一因子图进行图优化处理，得到所述移动机器人本体在各停止位置处的第一优化位姿；根据所述第一优化位姿，对各所述局部点云地图进行拼接，得到全局的环境地图；从而可高效地获取高精度的环境地图。

附图说明

[0054] 图1为本申请实施例提供的三维点云地图构建方法的流程图。

[0055] 图2为本申请实施例提供的三维点云地图构建装置的结构示意图。

[0056] 图3为本申请实施例提供的电子设备的结构示意图。

[0057] 图4为示例性的第二因子图。

[0058] 图5为示例性的第一因子图。

具体实施方式

[0059] 下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

[0060] 应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

[0061] 请参照图1，图1是本申请一些实施例中的一种三维点云地图构建方法，应用于移动机器人以获取环境地图，该移动机器人包括移动机器人本体、设置在移动机器人本体上的机械臂以及设置在机械臂末端的感知模块；

[0062] 三维点云地图构建方法包括步骤：

[0063] A1.使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图（该成局部点云地图为三维点云地图）；

[0064] A2.以移动机器人本体在各停止位置（停止位置是指移动机器人每次移动后所处的位置）处的第一位姿（为了便于说明，把移动机器人本体在各停止位置处的位姿称为第一位姿，该第一位姿为相对世界坐标系的位姿）为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量（为了方便描述，把相邻两个停止位置对应的第一位姿之间的位姿变化量称为第一位姿变化量）和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系（为了便于描述，把相邻两个停止位置对应的局部点云地图之间的位姿变换关系称为第一位姿变换关系）为约束因子，生成第一因子图；

[0065] A3.对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；

[0066] A4.根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图（该全局的环境地图为三维点云地图）。

[0067] 该三维点云地图构建方法，在移动机器人每次移动后保持移动机器人本体不动，由机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图，可得到精度较高的局部点云地图，进而，通过图优化方法得到移动机器人本体在不同停止位置处的精确的位姿（即第一优化位姿），并依此对各局部点云地图进行拼接得到全局的环境地图，该全局的环境地图的精度高；且与通过调节移动机器人本体来获取不同方向的点云数据相比，通过机械臂带动感知模块移动来获取不同方向的点云数据，效率更高。使用该全局的环境地图进行定位导航时，可提高定位精度。

[0068] 其中，感知模块可以但不限于是激光雷达传感器、视觉传感器等可采集点云数据的设备。

[0069] 具体地，步骤A1包括：

[0070] A101.使移动机器人分步移动；

[0071] A102.在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块移动以采集多帧点云数据；

[0072] A103.针对同一停止位置，以各采集时刻的感知模块的第二位姿（为了便于说明，把各采集时刻的感知模块的位姿称为第二位姿，该第二位姿是感知模块相对于移动机器人本体的位姿）为节点，以相邻两个采集时刻的点云数据之间的第二位姿变换关系（为了便于说明，把相邻两个采集时刻的点云数据之间的位姿变换关系称为第二位姿变换关系）和相邻两个采集时刻的第二位姿之间的第二位姿变化量（为了便于说明，把相邻两个采集时刻的第二位姿之间的变化量称为第二位姿变化量）为约束因子，生成第二因子图；

[0073] A104.对第二因子图进行图优化处理，得到各采集时刻的感知模块的第二优化位姿；

[0074] A105.根据第二优化位姿，对各帧点云数据进行拼接，得到当前停止位置的局部点云地图。

[0075] 通过图优化方法对感知模块的位姿进行优化，可得到更加准确的感知模块的位姿，从而保证拼接后的局部点云地图具有较高的精度。

[0076] 其中，可使移动机器人沿预设的移动路径分步移动，该预设的移动路径优选为首尾连接的闭环路径，有利于在后续进行图优化时形成闭环约束因子（第一闭环约束因子），从而进一步提高全局的环境地图的准确性。其中，可按该预设的移动路径移动一次或循环移动多次，具体可根据实际需要设置。

[0077] 其中，通过机械臂带动感知模块移动以采集多帧点云数据时，可使机械臂末端按预设轨迹移动，在移动过程中，感知模块按预设频率（可根据实际需要设置）连续采集点云数据，其中，预设轨迹优选为首尾连接的闭环轨迹，有利于在后续进行图优化时形成闭环约束因子（第二闭环约束因子），从而进一步提高局部点云地图的准确性。其中，可按该预设轨迹移动一次或循环移动多次，具体可根据实际需要设置。

[0078] 其中，感知模块采集到的各帧点云数据，包含多个云点相对于感知模块的位置数据。

[0079] 进一步地，步骤A103包括针对同一停止位置执行的步骤：

[0080] 对相邻两个采集时刻的点云数据进行点云匹配，以获取相邻两个采集时刻的点云数据之间的第二位姿变换关系；

[0081] 获取各采集时刻的机械臂末端的第三位姿（为了便于说明，把各采集时刻的机械臂末端的位姿称为第三位姿，该第三位姿为机械臂末端相对移动机器人本体的位姿），以计算各采集时刻的感知模块的第二位姿；

[0082] 根据各采集时刻的第二位姿，计算相邻两个采集时刻的第二位姿之间的第二位姿变化量（两者相减即可）；

[0083] 以各采集时刻的第二位姿为节点，以相邻两个采集时刻的第二位姿变换关系和相邻两个采集时刻的第二位姿变化量为约束因子，生成第二因子图。

[0084] 其中，可采用点云配准算法（可以但不限于是ICP算法、NDT算法等）对相邻两个采集时刻的点云数据进行点云匹配，此为现有技术，此处不对其进行详述。在点云匹配后，可通过两帧点云数据中相互匹配的特征点（特征点可以但不限于是外部环境中的物体的角点）的相对感知模块的位置数据计算得到两帧点云数据之间的第二位姿变换关系，具体计算过程为现有技术，此处不对其进行详述。

[0085] 其中，各采集时刻的机械臂末端的第三位姿可通过以下方式获取：直接从机械臂的控制器读取得到机械臂末端相对机械臂基座的初始位姿，然后根据机械臂基座相对于移动机器人本体的位姿转换关系（该位姿转换关系可预先标定得到）对该初始位姿进行坐标变换，得到对应的第三位姿；具体地， ， 为第三位姿， 为机械臂末端相对机械臂基座的初始位姿， 为机械臂基座相对于移动机器人本体的位姿转换矩阵（表示机械臂基座相对于移动机器人本体的位姿转换关系）。

[0086] 得到第三位姿后，可根据感知模块相对于机械臂末端的位姿转换关系（该位姿转换关系可预先标定得到）与该第三位姿计算对应的第二位姿；具体地， ，为第二位姿， 感知模块相对于机械臂末端的位姿转换矩阵（表示感知模块相对于机械臂末端的位姿转换关系）。

[0087] 优选地，步骤A102包括：采集至少两帧包含同一标志物的云点的点云数据；

[0088] 从而，以各采集时刻的第二位姿为节点，以相邻两个采集时刻的第二位姿变换关系和相邻两个采集时刻的第二位姿变化量为约束因子，生成第二因子图的步骤包括：

[0089] 以采集到该标志物的云点的采集时刻为特征时刻，在相邻两个特征时刻对应的第二位姿之间添加第二闭环约束因子；第二闭环约束因子为相邻两个特征时刻对应的感知模块位姿（该感知模块位姿可以是感知模块相对于机械臂基座、移动机器人本体或世界世界坐标系的位姿）之间的第四位姿变换关系（为了方便说明，把相邻两个特征时刻对应的感知模块位姿之间的位姿变换关系称为第四位姿变换关系）。

[0090] 通过加入第二闭环约束因子，可减小积累误差，从而使局部点云地图的准确性更高。

[0091] 其中，标志物是在外部环境中的物体或标记，是用于对前后帧点云进行匹配时选取的参考物。在添加第二闭环约束因子时，仅在对应的点云数据包含同一个标志物的两个节点之间添加对应的第二闭环约束因子。

[0092] 假设在某一采集时刻，采集到的点云数据包含某标志物的云点，此时，通过图像识别方法识别该标志物相对于感知模块的位姿为Pi，经过若干个采集时刻后，再次采集到包含该标志物的云点的点云数据，则通过图像识别方法识别该标志物相对于感知模块的位姿为Qi，由于同一标志物在外部环境中是固定的，则该两个采集时刻（即相邻两个特征时刻）对应的感知模块位姿之间的第四位姿变换关系为Pi/Qi。

[0093] 例如图4所示的一种示例性的第二因子图， 为各采集时刻的第二位姿；第二先验因子为感知模块相对于移动机器人本体的初始位姿，在每个停止位置处，完成局部点云地图的生成后，机械臂会带动感知模块复位到预设的初始位姿，在到达下一个停止位置时，该初始位姿为在该停止位置的第一个第二位姿，该初始位姿是确定的，在图优化过程中不参与优化，因此记为第二先验因子；第二位姿变化量约束因子为相邻两个采集时刻的第二位姿之间的第二位姿变化量对应的约束因子；第二位姿变换关系约束因子为相邻两个采集时刻的点云数据之间的第二位姿变换关系对应的约束因子。

[0094] 其中，对因子图进行图优化的优化方法为现有技术，此处不对其进行详述。优化后得到的第二优化位姿仍为感知模块相对于移动机器人本体的位姿。

[0095] 其中，步骤A105包括：

[0096] 根据第二优化位姿，对各帧点云数据进行坐标变换处理，以把各帧点云数据中的云点相对于感知模块的位置数据转换为相对于移动机器人本体的位置数据；具体地，根据以下公式进行转换处理： ， 为云点相对于移动机器人本体的位置数据，为云点相对于感知模块的位置数据， 为第二优化位姿；

[0097] 把坐标变换处理后的各帧点云数据合并在一起，形成当前停止位置的局部点云地图。

[0098] 由于在同一停止位置处，移动机器人本体是停止不动的，进行坐标变换处理后的各帧点云数据中，各云点的位置数据均是相对移动机器人本体的位置数据，从而坐标变换处理后的各帧点云数据中的位置数据均是相对于同一位置点（当前时刻的移动机器人本体坐标系原点）的位置数据，因此，直接把各帧点云数据合并在一起即可得到当前停止位置的局部点云地图。得到的局部点云地图中，各云点的位置数据为相对于移动机器人本体的位置数据。

[0099] 具体地，步骤A2包括：

[0100] A201.通过对IMU数据积分获取相邻两个停止位置对应的第一位姿之间的第一位姿变化量；

[0101] A202.根据第一位姿变化量获取移动机器人本体在各停止位置处的第一位姿；

[0102] A203.对相邻两个停止位置的局部点云地图进行点云匹配，以获取相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系；

[0103] A204.以各停止位置的第一位姿为节点，以相邻两个停止位置的第一位姿变换关系和相邻两个停止位置的第一位姿变化量为约束因子，生成第一因子图。

[0104] 在实际应用中，移动机器人的起始点为地图坐标系原点，即第一个停止位置为地图坐标系原点，其对应的第一位姿是已知的（该第一位姿一般为（0,0,0），表示坐标位置和姿态角度均为0）。

[0105] 进而，在后续每次移动的移动过程中，可对移动机器人本体上的IMU模块测得的IMU数据进行积分，得到移动机器人本体在两个停止位置之间的位姿变化量（即第一位姿变化量），用该第一位姿变化量加上该两个停止位置中前一个停止位置的第一位姿即可得到后一个停止位置的第一位姿。通过这种方式得到各停止位置处的第一位姿，无需依赖于GPS信息，无论在室内还是在室外均能可靠地获得第一位姿，且可避免由于GPS信息频率较低、存在一定延迟而影响定位精度。

[0106] 在实际应用中，不限于通过IMU数据获取相邻两个停止位置对应的第一位姿之间的第一位姿变化量并根据第一位姿变化量获取移动机器人本体在各停止位置处的第一位姿；例如，也可使用激光测距仪对设置在移动机器人本体上的多个标记点进行测距，然后根据测距结果计算移动机器人本体的第一位姿，再用相邻两个停止位置的第一位姿相减得到对应的第一位姿变化量。

[0107] 其中，可采用点云配准算法（可以但不限于是ICP算法、NDT算法等）对相邻两个停止位置的局部点云地图进行点云匹配，此为现有技术，此处不对其进行详述。在点云匹配后，可通过两个局部点云地图中相互匹配的特征点（特征点可以但不限于是外部环境中的物体的角点）的相对移动机器人本体的位置数据计算得到两个局部点云地图之间的第一位姿变换关系，具体计算过程为现有技术，此处不对其进行详述。

[0108] 优选地，步骤A1包括：生成至少两个包含同一标志物的云点的局部点云地图；

[0109] 步骤A204包括：

[0110] 以包含该标志物的云点的局部点云地图对应的停止位置为特征位置，在相邻两个特征位置对应的第一位姿之间添加第一闭环约束因子；第一闭环约束因子为相邻两个停止位置对应的移动机器人位姿（该移动机器人位姿可以是移动机器人本体相对于世界坐标系的位姿）之间的第三位姿变换关系（为了便于描述，把相邻两个停止位置对应的移动机器人位姿之间的位姿变换关系称为第三位姿变换关系）。

[0111] 通过加入第二闭环约束因子，可减小积累误差，从而使局部点云地图的准确性更高。

[0112] 其中，仅在对应的局部点云地图包含同一个标志物的两个节点之间添加对应的第一闭环约束因子。

[0113] 假设在某一停止位置，得到的局部点云地图包含某标志物的云点，此时，通过图像识别方法识别该标志物相对于移动机器人本体的位姿为Mi，经过若干次移动后，再次得到包含该标志物的云点的局部点云地图，则通过图像识别方法识别该标志物相对于移动机器人本体的位姿为Ni，由于同一标志物在外部环境中是固定的，则该两个停止位置（即相邻两个特征位置）对应的移动机器人位姿之间的第三位姿变换关系为Mi/Ni。

[0114] 例如图5所示的一种示例性的第一因子图， 为各停止位置对应的第一位姿；第一先验因子为移动机器人最初的位姿，为已知值，一般地，移动机器人最初的位置为地图坐标系的原点，此时的位姿为（0,0,0），该最初的位姿是确定的，在图优化过程中不参与优化，因此记为第一先验因子；第一位姿变化量约束因子为相邻两个停止位置的第一位姿之间的第一位姿变化量对应的约束因子；第一位姿变换关系约束因子为相邻两个停止位置的局部点云地图之间的第一位姿变换关系对应的约束因子。

[0115] 其中，优化后得到的第一优化位姿仍为移动机器人本体相对于世界坐标系的位姿。

[0116] 其中，步骤A4包括：

[0117] 根据第一优化位姿，对各局部点云地图进行坐标变换处理，以把各局部点云地图中的云点相对于移动机器人本体的位置数据转换为相对于世界坐标系的位置数据；具体地，根据以下公式进行转换处理： ， 为云点相对于世界坐标系的位置数据， 为第一优化位姿；

[0118] 把坐标变换处理后的各局部点云地图合并在一起，形成全局的环境地图。

[0119] 由于世界坐标系是固定的，进行坐标变换处理后的各局部点云地图中，各云点的位置数据均是相对世界坐标系的位置数据，从而坐标变换处理后的各局部点云地图中的位置数据均是相对于同一位置点（世界坐标系原点）的位置数据，因此，直接把各局部点云地图合并在一起即可得到全局的环境地图。得到的全局的环境地图中，各云点的位置数据为相对于世界坐标系的位置数据。

[0120] 由上可知，该三维点云地图构建方法，通过使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图；以移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图；从而可高效地获取高精度的环境地图。

[0121] 参考图2，本申请提供了一种三维点云地图构建装置，应用于移动机器人以获取环境地图，移动机器人包括移动机器人本体、设置在移动机器人本体上的机械臂以及设置在机械臂末端的感知模块；

[0122] 三维点云地图构建装置包括：

[0123] 局部地图生成模块1，用于使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图（该成局部点云地图为三维点云地图）；

[0124] 第一因子图生成模块2，用于以移动机器人本体在各停止位置（停止位置是指移动机器人每次移动后所处的位置）处的第一位姿（为了便于说明，把移动机器人本体在各停止位置处的位姿称为第一位姿，该第一位姿为相对世界坐标系的位姿）为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量（为了方便描述，把相邻两个停止位置对应的第一位姿之间的位姿变化量称为第一位姿变化量）和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系（为了便于描述，把相邻两个停止位置对应的局部点云地图之间的位姿变换关系称为第一位姿变换关系）为约束因子，生成第一因子图；

[0125] 第一优化模块3，用于对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；

[0126] 第一拼接模块4，用于根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图（该全局的环境地图为三维点云地图）。

[0127] 该三维点云地图构建装置，移动机器人每次移动后保持移动机器人本体不动，由机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图，可得到精度较高的局部点云地图，进而，通过图优化方法得到移动机器人本体在不同停止位置处的精确的位姿（即第一优化位姿），并依此对各局部点云地图进行拼接得到全局的环境地图，该全局的环境地图的精度高；且与通过调节移动机器人本体来获取不同方向的点云数据相比，通过机械臂带动感知模块移动来获取不同方向的点云数据，效率更高。使用该全局的环境地图进行定位导航时，可提高定位精度。

[0128] 其中，感知模块可以但不限于是激光雷达传感器、视觉传感器等可采集点云数据的设备。

[0129] 优选地，局部地图生成模块1用于使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图，具体包括：

[0130] 使移动机器人分步移动；

[0131] 在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块移动以采集多帧点云数据；

[0132] 针对同一停止位置，以各采集时刻的感知模块的第二位姿（为了便于说明，把各采集时刻的感知模块的位姿称为第二位姿，该第二位姿是感知模块相对于移动机器人本体的位姿）为节点，以相邻两个采集时刻的点云数据之间的第二位姿变换关系（为了便于说明，把相邻两个采集时刻的点云数据之间的位姿变换关系称为第二位姿变换关系）和相邻两个采集时刻的第二位姿之间的第二位姿变化量（为了便于说明，把相邻两个采集时刻的第二位姿之间的变化量称为第二位姿变化量）为约束因子，生成第二因子图；

[0133] 对第二因子图进行图优化处理，得到各采集时刻的感知模块的第二优化位姿；

[0134] 根据第二优化位姿，对各帧点云数据进行拼接，得到当前停止位置的局部点云地图。

[0135] 通过图优化方法对感知模块的位姿进行优化，可得到更加准确的感知模块的位姿，从而保证拼接后的局部点云地图具有较高的精度。

[0136] 其中，可使移动机器人沿预设的移动路径分步移动，该预设的移动路径优选为首尾连接的闭环路径，有利于在后续进行图优化时形成闭环约束因子（第一闭环约束因子），从而进一步提高全局的环境地图的准确性。其中，可按该预设的移动路径移动一次或循环移动多次，具体可根据实际需要设置。

[0137] 其中，通过机械臂带动感知模块移动以采集多帧点云数据时，可使机械臂末端按预设轨迹移动，在移动过程中，感知模块按预设频率（可根据实际需要设置）连续采集点云数据，其中，预设轨迹优选为首尾连接的闭环轨迹，有利于在后续进行图优化时形成闭环约束因子（第二闭环约束因子），从而进一步提高局部点云地图的准确性。其中，可按该预设轨迹移动一次或循环移动多次，具体可根据实际需要设置。

[0138] 其中，感知模块采集到的各帧点云数据，包含多个云点相对于感知模块的位置数据。

[0139] 进一步地，局部地图生成模块1在生成第二因子图时，针对同一停止位置执行：

[0140] 对相邻两个采集时刻的点云数据进行点云匹配，以获取相邻两个采集时刻的点云数据之间的第二位姿变换关系；

[0141] 获取各采集时刻的机械臂末端的第三位姿（为了便于说明，把各采集时刻的机械臂末端的位姿称为第三位姿，该第三位姿为机械臂末端相对移动机器人本体的位姿），以计算各采集时刻的感知模块的第二位姿；

[0142] 根据各采集时刻的第二位姿，计算相邻两个采集时刻的第二位姿之间的第二位姿变化量（两者相减即可）；

[0143] 以各采集时刻的第二位姿为节点，以相邻两个采集时刻的第二位姿变换关系和相邻两个采集时刻的第二位姿变化量为约束因子，生成第二因子图。

[0144] 其中，可采用点云配准算法（可以但不限于是ICP算法、NDT算法等）对相邻两个采集时刻的点云数据进行点云匹配，此为现有技术，此处不对其进行详述。在点云匹配后，可通过两帧点云数据中相互匹配的特征点（特征点可以但不限于是外部环境中的物体的角点）的相对感知模块的位置数据计算得到两帧点云数据之间的第二位姿变换关系，具体计算过程为现有技术，此处不对其进行详述。

[0145] 其中，各采集时刻的机械臂末端的第三位姿可通过以下方式获取：直接从机械臂的控制器读取得到机械臂末端相对机械臂基座的初始位姿，然后根据机械臂基座相对于移动机器人本体的位姿转换关系（该位姿转换关系可预先标定得到）对该初始位姿进行坐标变换，得到对应的第三位姿；具体地， ， 为第三位姿， 为机械臂末端相对机械臂基座的初始位姿， 为机械臂基座相对于移动机器人本体的位姿转换矩阵（表示机械臂基座相对于移动机器人本体的位姿转换关系）。

[0146] 得到第三位姿后，可根据感知模块相对于机械臂末端的位姿转换关系（该位姿转换关系可预先标定得到）与该第三位姿计算对应的第二位姿；具体地， ，为第二位姿， 感知模块相对于机械臂末端的位姿转换矩阵（表示感知模块相对于机械臂末端的位姿转换关系）。

[0147] 优选地，局部地图生成模块1在通过机械臂带动感知模块移动以采集多帧点云数据的时候，执行：采集至少两帧包含同一标志物的云点的点云数据；

[0148] 从而，局部地图生成模块1在以各采集时刻的第二位姿为节点，以相邻两个采集时刻的第二位姿变换关系和相邻两个采集时刻的第二位姿变化量为约束因子，生成第二因子图的时候，执行：

[0149] 以采集到该标志物的云点的采集时刻为特征时刻，在相邻两个特征时刻对应的第二位姿之间添加第二闭环约束因子；第二闭环约束因子为相邻两个特征时刻对应的感知模块位姿之间的第四位姿变换关系。

[0150] 通过加入第二闭环约束因子，可减小积累误差，从而使局部点云地图的准确性更高。

[0151] 其中，标志物是在外部环境中的物体或标记，是用于对前后帧点云进行匹配时选取的参考物。在添加第二闭环约束因子时，仅在对应的点云数据包含同一个标志物的两个节点之间添加对应的第二闭环约束因子。

[0152] 假设在某一采集时刻，采集到的点云数据包含某标志物的云点，此时，通过图像识别方法识别该标志物相对于感知模块的位姿为Pi，经过若干个采集时刻后，再次采集到包含该标志物的云点的点云数据，则通过图像识别方法识别该标志物相对于感知模块的位姿为Qi，由于同一标志物在外部环境中是固定的，则该两个采集时刻（即相邻两个特征时刻）对应的感知模块位姿之间的第四位姿变换关系为Pi/Qi。

[0153] 例如图4所示的一种示例性的第二因子图， 为各采集时刻的第二位姿；第二先验因子为感知模块相对于移动机器人本体的初始位姿，在每个停止位置处，完成局部点云地图的生成后，机械臂会带动感知模块复位到预设的初始位姿，在到达下一个停止位置时，该初始位姿为在该停止位置的第一个第二位姿，该初始位姿是确定的，在图优化过程中不参与优化，因此记为第二先验因子；第二位姿变化量约束因子为相邻两个采集时刻的第二位姿之间的第二位姿变化量对应的约束因子；第二位姿变换关系约束因子为相邻两个采集时刻的点云数据之间的第二位姿变换关系对应的约束因子。

[0154] 其中，对因子图进行图优化的优化方法为现有技术，此处不对其进行详述。优化后得到的第二优化位姿仍为感知模块相对于移动机器人本体的位姿。

[0155] 其中，局部地图生成模块1在根据第二优化位姿，对各帧点云数据进行拼接，得到当前停止位置的局部点云地图的时候，执行：

[0156] 根据第二优化位姿，对各帧点云数据进行坐标变换处理，以把各帧点云数据中的云点相对于感知模块的位置数据转换为相对于移动机器人本体的位置数据；具体地，根据以下公式进行转换处理： ， 为云点相对于移动机器人本体的位置数据，为云点相对于感知模块的位置数据， 为第二优化位姿；

[0157] 把坐标变换处理后的各帧点云数据合并在一起，形成当前停止位置的局部点云地图。

[0158] 由于在同一停止位置处，移动机器人本体是停止不动的，进行坐标变换处理后的各帧点云数据中，各云点的位置数据均是相对移动机器人本体的位置数据，从而坐标变换处理后的各帧点云数据中的位置数据均是相对于同一位置点（当前时刻的移动机器人本体坐标系原点）的位置数据，因此，直接把各帧点云数据合并在一起即可得到当前停止位置的局部点云地图。得到的局部点云地图中，各云点的位置数据为相对于移动机器人本体的位置数据。

[0159] 具体地，第一因子图生成模块2在生成第一因子图时，执行：

[0160] 通过对IMU数据积分获取相邻两个停止位置对应的第一位姿之间的第一位姿变化量；

[0161] 根据第一位姿变化量获取移动机器人本体在各停止位置处的第一位姿；

[0162] 对相邻两个停止位置的局部点云地图进行点云匹配，以获取相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系；

[0163] 以各停止位置的第一位姿为节点，以相邻两个停止位置的第一位姿变换关系和相邻两个停止位置的第一位姿变化量为约束因子，生成第一因子图。

[0164] 在实际应用中，移动机器人的起始点为地图坐标系原点，即第一个停止位置为地图坐标系原点，其对应的第一位姿是已知的（该第一位姿一般为（0,0,0），表示坐标位置和姿态角度均为0）。

[0165] 进而，在后续每次移动的移动过程中，可对移动机器人本体上的IMU模块测得的IMU数据进行积分，得到移动机器人本体在两个停止位置之间的位姿变化量（即第一位姿变化量），用该第一位姿变化量加上该两个停止位置中前一个停止位置的第一位姿即可得到后一个停止位置的第一位姿。通过这种方式得到各停止位置处的第一位姿，无需依赖于GPS信息，无论在室内还是在室外均能可靠地获得第一位姿，且可避免由于GPS信息频率较低、存在一定延迟而影响定位精度。

[0166] 在实际应用中，不限于通过IMU数据获取相邻两个停止位置对应的第一位姿之间的第一位姿变化量并根据第一位姿变化量获取移动机器人本体在各停止位置处的第一位姿；例如，也可使用激光测距仪对设置在移动机器人本体上的多个标记点进行测距，然后根据测距结果计算移动机器人本体的第一位姿，再用相邻两个停止位置的第一位姿相减得到对应的第一位姿变化量。

[0167] 其中，可采用点云配准算法（可以但不限于是ICP算法、NDT算法等）对相邻两个停止位置的局部点云地图进行点云匹配，此为现有技术，此处不对其进行详述。在点云匹配后，可通过两个局部点云地图中相互匹配的特征点（特征点可以但不限于是外部环境中的物体的角点）的相对移动机器人本体的位置数据计算得到两个局部点云地图之间的第一位姿变换关系，具体计算过程为现有技术，此处不对其进行详述。

[0168] 优选地，局部地图生成模块1在通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图的时候，执行：生成至少两个包含同一标志物的云点的局部点云地图；

[0169] 第一因子图生成模块2在以各停止位置的第一位姿为节点，以相邻两个停止位置的第一位姿变换关系和相邻两个停止位置的第一位姿变化量为约束因子，生成第一因子图的时候，执行：

[0170] 以包含该标志物的云点的局部点云地图对应的停止位置为特征位置，在相邻两个特征位置对应的第一位姿之间添加第一闭环约束因子；第一闭环约束因子为相邻两个停止位置对应的移动机器人位姿之间的第三位姿变换关系。

[0171] 通过加入第二闭环约束因子，可减小积累误差，从而使局部点云地图的准确性更高。

[0172] 其中，仅在对应的局部点云地图包含同一个标志物的两个节点之间添加对应的第一闭环约束因子。

[0173] 假设在某一停止位置，得到的局部点云地图包含某标志物的云点，此时，通过图像识别方法识别该标志物相对于移动机器人本体的位姿为Mi，经过若干次移动后，再次得到包含该标志物的云点的局部点云地图，则通过图像识别方法识别该标志物相对于移动机器人本体的位姿为Ni，由于同一标志物在外部环境中是固定的，则该两个停止位置（即相邻两个特征位置）对应的移动机器人位姿之间的第三位姿变换关系为Mi/Ni。

[0174] 例如图5所示的一种示例性的第一因子图， 为各停止位置对应的第一位姿；第一先验因子为移动机器人最初的位姿，为已知值，一般地，移动机器人最初的位置为地图坐标系的原点，此时的位姿为（0,0,0），该最初的位姿是确定的，在图优化过程中不参与优化，因此记为第一先验因子；第一位姿变化量约束因子为相邻两个停止位置的第一位姿之间的第一位姿变化量对应的约束因子；第一位姿变换关系约束因子为相邻两个停止位置的局部点云地图之间的第一位姿变换关系对应的约束因子。

[0175] 其中，优化后得到的第一优化位姿仍为移动机器人本体相对于世界坐标系的位姿。

[0176] 其中，第一拼接模块4在根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图的时候，执行：

[0177] 根据第一优化位姿，对各局部点云地图进行坐标变换处理，以把各局部点云地图中的云点相对于移动机器人本体的位置数据转换为相对于世界坐标系的位置数据；具体地，根据以下公式进行转换处理： ， 为云点相对于世界坐标系的位置数据， 为第一优化位姿；

[0178] 把坐标变换处理后的各局部点云地图合并在一起，形成全局的环境地图。

[0179] 由于世界坐标系是固定的，进行坐标变换处理后的各局部点云地图中，各云点的位置数据均是相对世界坐标系的位置数据，从而坐标变换处理后的各局部点云地图中的位置数据均是相对于同一位置点（世界坐标系原点）的位置数据，因此，直接把各局部点云地图合并在一起即可得到全局的环境地图。得到的全局的环境地图中，各云点的位置数据为相对于世界坐标系的位置数据。

[0180] 由上可知，该三维点云地图构建装置，通过使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图；以移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图；从而可高效地获取高精度的环境地图。

[0181] 请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的三维点云地图构建方法，以实现以下功能：使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图；以移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图。

[0182] 本申请实施例提供一种存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的三维点云地图构建方法，以实现以下功能：使移动机器人分步移动，在每次移动后保持移动机器人本体的位姿不变，并通过机械臂带动感知模块在不同位姿采集多帧点云数据以生成局部点云地图；以移动机器人本体在各停止位置处的第一位姿为节点，以相邻两个停止位置对应的第一位姿之间的第一位姿变化量和相邻两个停止位置对应的局部点云地图之间的第一位姿变换关系为约束因子，生成第一因子图；对第一因子图进行图优化处理，得到移动机器人本体在各停止位置处的第一优化位姿；根据第一优化位姿，对各局部点云地图进行拼接，得到全局的环境地图。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read‑Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red‑Only Memory, 简称PROM），只读存储器（Read‑Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。

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

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

[0185] 再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

[0186] 在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

[0187] 以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。