本发明涉及对环境的3d地图构建，特别是涉及用于移动机器人工作场景的3d地图构建的方法及系统。
背景技术：
在导航、模拟、机器人、虚拟/增强现实等众多智能控制或智能设备领域中，均需要以二维或三维的电子地图为基础，来进行进一步的开发或使用，因此，对真实环境进行地图构建，快速、有效并准确地将真实环境转化为二维或三维的电子地图，是广泛应用于智能控制或智能设备领域的基础性技术。在移动机器人领域，良好的电子地图更是机器人实现自主移动的前提和基础。
对于移动机器人来说，自主行走是最重要的能力，也是最难解决的问题。为了能够实现移动机器人的自主行走，通常需要解决四个层面的问题：一、地图构建：对运行环境的轮廓进行感知测量，重建出环境的2d或3d结构，即构建出环境的2d或3d地图；二、定位解算：基于构建的地图和机器人当前的观测数据，解算出机器人在地图中的位置；三、路径规划：根据移动机器人当前位置和目标位置，规划出行走的路线；四、轨迹控制：根据移动机器人要走的路线和机器人与目标路线的偏差，控制机器人使得机器人沿着路线精确行走。
从上述阐述中可以看到，移动机器人的定位解算、路径规划、轨迹控制与地图构建都密不可分，都是要基于地图构建才能得以实现，因此高效率高质量的3d地图构建具有非常重要的意义。
3d地图构建主要有基于视觉的3d地图构建和基于3d激光的3d地图构建两类。基于视觉的3d地图构建通常具有远距离精度低、构建时易受光照和运动干扰等缺点。3d激光因为测距精度高，抗干扰能力强等优点，是目前来说比较理想的3d地图构建传感器。
现有的3d激光地图构建方法中：有的搭配差分gps以解决误差累计的问题，但依赖差分gps的地图构建方法，无法解决gps信号差的环境的地图构建；有的利用全站仪等测绘设备以解决地图扭曲变形问题，但需要另外搭配全站仪，而且测绘过程操作复杂；有的利用自动闭环检测算法以减小误差累积，但是对环境的结构化特征要求比较高，结构化环境不好的场景往往地图质量不佳，并且比较难构建大型地图。
技术实现要素：
鉴于现有技术的上述缺陷，本发明通过融合移动机器人上的里程计、惯导和3d激光，利用里程计和惯导做航迹递推为激光的配准提供初值，加速了激光配准的速度，并能有效避免激光数据不佳时配准的失效。本发明还可以通过自动或人工闭环的方式，对移动机器人走过的任意环境做精确的闭环，能适应室外结构化不好的场景，可以构建室内室外各种环境的大型地图。同时，本发明通过将地图数据采集和地图构建分离，可以离线对地图进行反复精细的调整，能够构建出高精度的各种规模的3d地图。并且本发明不需依赖gps和全站仪，因此操作简单方便且高效。
一方面，本发明提供一种基于3d激光的地图构建方法，包括以下步骤：s1、规划路线，并在移动机器人移动的过程中采集数据；s2、使用地图构建工具载入所采集的数据；s3、对相邻的激光数据进行初始配准；s4、对相邻的激光数据进行精确配准；s5、对移动机器人经过的相同区域进行闭环关联，建立闭环约束；s6、对建立了闭环约束的整个地图进行全局优化求解；s7、叠加所有的激光数据，输出激光地图。
本发明的一个实施例中，步骤s1包括：根据以下一个或多个原则规划路线：(i)先走大的闭环路线，再走内部的小闭环路线；(ii)尽量避免在同一条路上来回走；(iii)能闭合的路线尽量闭合。
本发明的另一个实施例中，步骤s1包括：采集移动机器人行走过程中的里程数据、惯导数据和激光数据。
本发明的另一个实施例中，步骤s1包括：通过融合里程数据和惯导数据递推移动机器人的行走轨迹，然后持续判断移动机器人的行走，每行走一定距离或转过一定角度即保存一帧激光数据。
本发明的另一个实施例中，步骤s3包括：利用里程数据和惯导数据对移动机器人的行走轨迹进行航迹递推解算，然后将行走过程中保存的每帧激光数据按航迹递推出来的位置进行叠加，从而完成激光数据的初始配准。
本发明的另一个实施例中，步骤s4包括：将步骤s3中得到的初始配准结果作为相邻两帧激光数据配准的初始值，利用正态分布变换对相邻两帧激光再进行精确配准。
本发明的另一个实施例中，步骤s4包括：利用精确配准得到的位姿来更新移动机器人行走的轨迹。
本发明的另一个实施例中，步骤s5包括：对移动机器人行走过程中的激光数据进行遍历，对移动机器人经过的相同区域的数据进行关联。
本发明的另一个实施例中，步骤s5包括：s5.1、选择可以闭环关联的激光数据，将其移动到能够配准的相对位姿处，从而得到匹配初始值；s5.2、通过正态分布变换，基于所得到的匹配初始值进行精确配准，以得到两帧闭环激光数据精确的相对位姿。
本发明的另一个实施例中，步骤s5包括：以自动和/或人工的方式进行闭环关联。
本发明的另一个实施例中，步骤s6包括：根据相邻帧激光数据的匹配关系和闭环建立的激光数据的匹配关系，从概率上求解最优的移动机器人采集的每帧激光数据的全局位姿。
本发明的另一个实施例中，步骤s7包括：将激光数据叠加全局位姿，并将激光数据全部变换到统一的坐标系中去，从而得到统一坐标系的激光数据。
另一方面，本发明提供一种基于3d激光的地图构建系统，包括：
路线规划模块，所述路线规划模块被配置为人工针对要构建地图的实际场景，规划移动机器人行走的路线；
数据采集模块，所述数据采集模块包括：移动机器人，被配置为能够根据预先设定的指令或者以实时控制的方式进行移动或行走；传感器，包括里程计、惯导、3d激光；以及数据采集装置，搭载在所述移动机器人上，被配置为能够在所述移动机器人行走的过程中采集各项数据；
地图构建模块，所述地图构建模块包括：数据载入模块，被配置为能够载入所采集到的各项数据；初始配准模块，被配置为能够利用所载入的数据对激光数据进行初始配准，以得到初始值；精确配准模块，被配置为能够基于所述初始值，进一步对激光数据进行精确配准；闭环关联模块，被配置为能够对所述移动机器人经过的相同区域进行闭环关联，建立闭环约束；以及全局优化模块，被配置为能够对建立了闭环约束的整个地图进行全局优化求解；以及
地图输出模块，所述地图输出模块被配置为能够基于所述地图构建模块的处理结果输出激光地图。
本发明的一个实施例中，所述路线规划模块被配置为人工根据以下一个或多个原则规划路线：(i)先走大的闭环路线，再走内部的小闭环路线；(ii)尽量避免在同一条路上来回走；(iii)能闭合的路线尽量闭合。
本发明的另一个实施例中，所述数据采集模块被配置为能够采集所述移动机器人行走过程中的里程数据、惯导数据和激光数据。
本发明的另一个实施例中，所述数据采集模块被配置为能够通过融合所述里程数据和所述惯导数据递推所述移动机器人的行走轨迹，持续判断所述移动机器人的行走，每行走一定距离或转过一定角度即保存一帧所述激光数据。
本发明的另一个实施例中，所述初始配准模块被配置为能够利用所述里程数据和所述惯导数据对所述移动机器人的行走轨迹进行航迹递推解算，并将行走过程中保存的每帧激光数据按航迹递推出来的位置进行叠加。
本发明的另一个实施例中，所述精确配准模块被配置为能够将初始配准结果作为相邻两帧激光数据配准的初始值，利用正态分布变换对相邻两帧激光再进行精确配准。
本发明的另一个实施例中，所述精确配准模块被配置为能够利用精确配准得到的位姿来更新移动机器人行走的轨迹。
本发明的另一个实施例中，所述闭环关联模块被配置为能够对移动机器人行走过程中的激光数据进行遍历，对移动机器人经过的相同区域的数据进行关联。
本发明的另一个实施例中，所述闭环关联模块被配置为能够选择可以闭环关联的激光数据，将其移动到能够配准的相对位姿处，从而得到匹配初始值，并且能够通过正态分布变换，基于所得到的匹配初始值进行精确配准，以得到两帧闭环激光数据精确的相对位姿。
本发明的另一个实施例中，所述闭环关联模块被配置为能够以自动和/或人工的方式进行操作来进行闭环关联。
本发明的另一个实施例中，所述全局优化模块被配置为能够根据相邻帧激光数据的匹配关系和闭环建立的激光数据的匹配关系，从概率上求解最优的移动机器人采集的每帧激光数据的全局位姿。
本发明的另一个实施例中，所述地图输出模块被配置为能够将激光数据叠加全局位姿，并将激光数据全部变换到统一的坐标系中去，从而得到统一坐标系的激光数据。
本发明提供的以上方法和系统，不依赖gps，不受gps信号的制约；不用使用全站仪等测绘设备，避免了繁琐的测绘操作；可以使用人工闭环的方式，能够构建大规模室内外各种环境的地图；并且将数据采集和构图分离，可以离线对地图进行精细的调整，可以构建高精度的地图。
以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。
附图说明
图1是本发明的一个实施例中的地图构建方法步骤示意图；
图2是本发明的一个实施例中的地图构建工具示意图；
图3是本发明的一个实施例中的3d激光地图示意图；
图4是本发明的一个实施例中的地图构建系统模块示意图。
具体实施方式
以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。
在下文的描述中，给出了大量具体的细节以便提供对本申请更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本申请可以无需一个或多个这些细节而得以实施。应当理解的是，本申请能够以不同形式实施，而不应当解释为局限于这里提出的实施例。相反地，提供这些实施例将使本申请的技术内容更为清楚和便于理解。
高精度的3d地图对移动机器人来说非常重要，它是移动机器人自主行走的先决条件，也是移动机器人精确到点执行任务的前提。因此研究可靠高效高质量的3d地图构建方法具有非常重要的意义。
下面结合本发明的具体实施例对本发明的技术方案进行详细阐述。
本发明提供一种基于3d激光的3d地图构建方法及系统，如图1所示为本发明的一个实施例中的地图构建方法步骤示意图，其过程主要包括以下几个步骤：
(1)规划路线，并在移动机器人移动的过程中采集数据；
(2)使用地图构建工具载入所采集的数据；
(3)对相邻的激光数据进行初始配准；
(4)对相邻的激光数据进行精确配准；
(5)对移动机器人经过的相同区域进行闭环关联，建立闭环约束；
(6)对建立了闭环约束的整个地图进行全局优化求解；
(7)叠加所有的激光数据，输出激光地图。
在规划路线并采集数据的步骤中，根据机器人运行的场景，规划合理的数据采集路线，并利用数据采集装置，在机器人移动过程中采集相关的各类数据或信息。在本发明的一个实施例中，可以基于以下几个基本原则进行规划和采集：
(i)先走大的闭环路线，再走内部的小闭环路线；
(ii)尽量避免在同一条路上来回走；
(iii)能闭合的路线尽量闭合。
基于以上原则，一方面可以尽可能地精简路线，以减少机器人行走的路线总长，从而减少所要采集的数据量，并加快整体数据采集速度；另一方面，可以尽可能多地形成闭合路线，以便于后续的处理，提高构建效率。
路线规划完成后，可以开启搭载在移动机器人上的数据采集装置，并控制机器人沿着所规划的路线运行或移动，同时将机器人行走过程中的里程数据、惯导数据、3d激光数据等各项数据或信息采集下来。其中，由于激光数据的数据量较大，所以优选的以一定条件对其进行刷选保存。在本发明的一个实施例中，具体做法是：通过融合里程数据和惯导数据递推机器人的行走轨迹，然后持续判断机器人的行走，每行走一定距离或转过一定角度(如超过1米或30度)即保存一帧激光数据。
然后通过数据载入工具将所采集的数据导入地图构建工具。如图2所示为本发明的一个实施例中的地图构建工具示意图，其中可以导入所采集的各项数据，进行相应的各种操作并设置或调整相应的各项参数。在本发明的一个实施例中，可以在载入数据时设置或调整以下一个或多个参数：像素比例、点大小、加载id及范围、激光帧位姿、是否追踪、高度差、半径、最少点数量、叶子大小及坐标等；也可以进行以下一项或多项操作：选择测绘文件路径、选择drnode文件、加载、保存图、加载图、加载单个点云文件、优化、保存图及点云、取消、增加边、删除边、重置边、删除激光帧、固定激光帧、不固定激光帧、调节高度差、加载点云、保存点云、滤波等。并且，地图构建工具还可以直观地显示处理过程中的地图，并根据不同的操作动态地进行更新和展示，以便于对地图数据进行调整和处理。
将采集的数据载入地图构建工具，从而对地图数据进行处理以构建地图。通过将地图数据采集和地图构建分离，可以离线对地图进行反复精细的调整，能够构建出高精度的各种规模的3d地图。
对激光数据进行初始配准。在本发明的一个实施例中，可以在载入的时候，利用里程数据和惯导数据对移动机器人的行走轨迹进行航迹递推解算，然后将行走过程中保存的每帧激光数据按航迹递推出来的位置进行叠加，从而完成激光数据的初始配准。其中，航迹递推的原理是：
令惯导测量的机器人的运动角速度为ω＝[ωxωyωz]t，机器人的姿态矩阵为r，则有：
令里程计得到的机器人的运行速度为v，机器人位置为p＝[xyz]t，则有：
对相邻的激光数据进行精确配准。点云的匹配算法都比较依赖于较好的初值，尤其是对于结构化不太好的场景来说，激光帧的配准结果很容易陷入局部最优，从而影响到整体的输出结果。在本发明的一个实施例中，可以通过航迹递推为每帧激光数据提供初始的位姿，并将此初始位姿作为相邻两帧激光数据配准的初始值，然后利用ndt(正态分布变换)对相邻两帧激光再进行精确配准，从而既提高了配准的速度，又能避免配准陷入局部最优，可以有效避免激光数据不佳时配准的失效。在所有相邻帧激光数据配准后，可以利用精确配准得到的位姿来更新机器人行走的轨迹。
对移动机器人经过的相同区域进行闭环关联，建立闭环约束，是指机器人多次走过同一个地方，每次走过的时候激光扫描到了同样的结构，将这些相同的结构进行关联，从而在关联后，得到闭环激光帧数据之间的相对位姿。
对于结构化较好的环境，可以利用闭环关联算法进行自动的闭环关联，但是可能存在一定的不确定性：一是环境中经常存在相似的区域，激光数据因为特征相对不多，很容易将相似的区域检测为闭环；二是对于结构不好的区域，激光数据没有稳定的特征可以提取来进行闭环计算；三是激光位姿都存在一定误差，使得同一个地方的激光数据在计算的轨迹中可能离的比较远，因而闭环关联失败率会相对较高。由于人工相对能够较为容易地区分哪些地方是相似区域，哪些地方是真正重复走过的可以闭环的区域。因此，在本发明的一个实施例中，可以选择人工闭环的操作方式，或者在自动闭环的基础上增加人工闭环的操作方式。
在本发明的一个实施例中，闭环关联的具体做法是：对机器人行走过程中的激光数据进行遍历，观察多次走过的地方的数据，并对这些数据进行关联。其中，主要包括两个步骤：
(1)选择可以闭环的激光数据，将它们移动到差不多可以配准的相对位姿处，从而得到匹配的初始值；
(2)利用ndt，基于所得到的匹配初始值进行精确配准，配准后，即得到了两帧闭环激光数据精确的相对位姿。
相对于自动闭环的局限性，通过人工闭环的方式，可以对移动机器人走过的任意环境做更为精确的闭环，能够适应室外结构化不好的场景，可以构建室内室外各种环境的大型地图。
前面的步骤都属于激光数据的匹配，匹配包括相邻两帧激光数据的配准和相同地方数据的闭环匹配，在此基础上，可以利用图优化对建立了闭环约束的整个地图进行全局优化求解。在本发明的一个实施例中，在匹配后对机器人行走的轨迹进行全局优化求解，同时考虑相邻帧激光数据的匹配关系和闭环建立的激光数据的匹配关系，从概率上求解最优的机器人采集的每帧激光数据的全局位姿。在本发明的一个实施例中，可以利用开源库g2o(generalgraphoptimization)进行全局求解。
最后输出3d激光地图。因为每帧激光数据的坐标都是相对于当时激光设备本身坐标系的坐标，可以把所有激光数据都统一到同一个坐标系下，以得到整体的3d激光地图。在本发明的一个实施例中，可以在求解到每帧激光数据的最优全局位姿后，将激光数据叠加激光的全局位姿，并将激光数据全部变换到某个统一的坐标系(如第一帧激光的坐标系)中去，从而得到统一坐标系的3d激光数据，这些数据即组成了最终的3d激光地图。如图3所示为发明的一个实施例中的3d激光地图示意图。
基于以上的方法和操作，本发明还提供一种基于3d激光的地图构建系统，主要包括路线规划模块、数据采集模块、地图构建模块和地图输出模块。
其中，路线规划模块能够针对要构建地图的实际区域，规划移动机器人行走的路线。在不同的实施例中，可以通过自动路线规划算法进行路线规划，也可以利用路线规划辅助工具，以人工的方式或者自动方式与人工方式相结合的方式进行路线规划。
数据采集模块，主要包括：移动机器人，能够根据预先设立的指令或者以实时控制的方式进行移动或行走；传感器，包括里程计、惯导、3d激光；数据采集装置，可以搭载在移动机器人上，在移动机器人行走的过程中，采集各项数据，如惯导数据、里程数据、激光数据等。在不同的实施例中，可以使用集成的数据采集装置同时采集各项数据，也可以使用不同的数据采集装置分别采集各项数据。
地图构建模块，主要包括：数据载入模块，能够载入所采集到的各项数据；初始配准模块，能够利用载入的数据对激光数据进行初始配准，以得到较好的初始值；精确配准模块，能够基于初始值，进一步对激光数据进行精确配准；闭环关联模块，能够对移动机器人经过的相同区域进行闭环关联，建立闭环约束；全局优化模块，能够对建立了闭环约束的整个地图进行全局优化求解。
地图输出模块，能够在求解到每帧激光数据的最优全局位姿后，将激光数据叠加激光的全局位姿，并将激光数据全部变换到统一的坐标系中去，从而得到统一坐标系的3d激光数据。
如图4所示为本发明一个实施例中的地图构建系统的模块示意图，该地图构建系统中的各个装置和模块，可以以硬件、软件或软硬件结合的方式实现。本发明所提供的方法和系统，不需要依赖gps，不受gps信号的制约；也不需要使用全站仪等测绘设备，避免了繁琐的测绘操作，因此操作简单方便且高效。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。